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)