def __init__(self, name, i2c_rate, i2c_pull, i2c_power, limit, trigger=True, thresh_err=THRESH_ERR, thresh_time=THRESH_TIME): self.name = name # configuration self.config = THRUSTERS_SPEC self.throttles = np.zeros((len(self.config), )) # init speed vectors using spec self.limits = limit * np.ones_like(self.throttles) # init limits using default conf # low-level interface self.interface = SeabotixInterface(self.config, i2c_rate, i2c_pull, i2c_power) self.interface.reset() # enable protection mode if thrusters errors? self.trigger_protection = trigger self.thresh_err = thresh_err self.thresh_time = thresh_time # controller status and timing self.ctrl_status = STATUS_NORM # internal fsm status self.t_loop = time.time() # current loop time self.t_reset = time.time() # time of last error reset # ros interface self.pub_stat = rospy.Publisher(TOPIC_STATUS, ThrusterFeedback, tcp_nodelay=True, queue_size=1) self.sub_cmds = rospy.Subscriber(TOPIC_CMDS, ThrusterCommand, self._handle_commands, queue_size=1, tcp_nodelay=True) self.srv_switch = rospy.Service(SRV_SWITCH, BooleanService, self.handle_switch) # timers self.t_cmds = rospy.Timer(rospy.Duration(INTERVAL_RESET), self._handle_timer_cmds) # commands safety timer
class ThrustersDriver(object): def __init__(self, name, i2c_rate, i2c_pull, i2c_power, limit, trigger=True, thresh_err=THRESH_ERR, thresh_time=THRESH_TIME): self.name = name # configuration self.config = THRUSTERS_SPEC self.throttles = np.zeros((len(self.config), )) # init speed vectors using spec self.limits = limit * np.ones_like(self.throttles) # init limits using default conf # low-level interface self.interface = SeabotixInterface(self.config, i2c_rate, i2c_pull, i2c_power) self.interface.reset() # enable protection mode if thrusters errors? self.trigger_protection = trigger self.thresh_err = thresh_err self.thresh_time = thresh_time # controller status and timing self.ctrl_status = STATUS_NORM # internal fsm status self.t_loop = time.time() # current loop time self.t_reset = time.time() # time of last error reset # ros interface self.pub_stat = rospy.Publisher(TOPIC_STATUS, ThrusterFeedback, tcp_nodelay=True, queue_size=1) self.sub_cmds = rospy.Subscriber(TOPIC_CMDS, ThrusterCommand, self._handle_commands, queue_size=1, tcp_nodelay=True) self.srv_switch = rospy.Service(SRV_SWITCH, BooleanService, self.handle_switch) # timers self.t_cmds = rospy.Timer(rospy.Duration(INTERVAL_RESET), self._handle_timer_cmds) # commands safety timer # TIMERs related def _clear_timer_cmds(self): """Resets the timer used for forcing requested speeds to zero""" self.t_cmds.shutdown() self.t_cmds = rospy.Timer(rospy.Duration(INTERVAL_RESET), self._handle_timer_cmds) def _handle_timer_cmds(self, event): """Resets required speeds to zero if timer expires""" self.throttles = np.zeros_like(self.throttles) rospy.logdebug('%s resetting thrusters speeds ...', self.name) # SERVICEs related def handle_switch(self, req): if req.request is True: self.ctrl_status = STATUS_NORM return BooleanServiceResponse(True) else: self.ctrl_status = STATUS_STOP return BooleanServiceResponse(False) # SUBSCRIBERs related def _handle_commands(self, data): """Processes input commands from high-level modules""" try: # avoid out-of-boundary errors throttle = np.array(data.throttle[0:6]) except Exception: rospy.logerr('%s bad input command, skipping!') else: # calculate thruster limits and accept commands self.throttles = np.clip(throttle, -self.limits, self.limits) # reset commands timer self._clear_timer_cmds() # LOGIC related def loop(self): self.t_loop = time.time() # fsm state if self.ctrl_status == STATUS_NORM: # normal mode and reset counters at a slower rate (prevents spurious errors to trigger the protection) if self.t_loop - self.t_reset > self.thresh_time: self.interface.errors = np.zeros_like(self.interface.errors) # reset error counters self.t_reset = self.t_loop else: # any stopped mode (keep sending zeros to thrusters) # note: this state needs an external reset for safety reasons! self.throttles = np.zeros_like(self.throttles) # set speed to zero # check interface errors and prevent overflows if np.any(self.interface.errors > self.thresh_err): self.interface.errors = np.zeros_like(self.interface.errors) # reset error counters # abort operations if protection is enabled (state change) if self.trigger_protection: rospy.logerr('%s error threshold reached -> protection mode activated!', self.name) self.ctrl_status = STATUS_ERROR try: # send thrusters commands self.interface.process_thrusters(self.throttles) except Exception as e: rospy.logerr('%s thrusters interface error:\n%s', name, e) traceback.print_exc(e) self.interface.shutdown() # try a clean shutdown of thrusters raise e # and rethrow the exception # publish report tf = ThrusterFeedback() tf.header.stamp = rospy.Time.now() tf.throttle = self.interface.feedback_throttle.tolist() tf.current = self.interface.feedback_curr.tolist() tf.temp = self.interface.feedback_temp.tolist() tf.status = self.interface.feedback_fault.tolist() tf.errors = self.interface.errors.tolist() self.pub_stat.publish(tf) def shutdown(self): """safe shutdown and safety hooks""" rospy.loginfo('%s shutdown procedure', self.name) self.sub_cmds.unregister() self.t_cmds.shutdown() self.interface.shutdown() rospy.loginfo('%s shutdown completed!', self.name)