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)