stretch_driver

#! /usr/bin/env python3

import yaml
import copy
import numpy as np
import threading
from rwlock import RWLock
import stretch_body.robot as rb
from stretch_body.hello_utils import ThreadServiceExit
import stretch_body

import tf2_ros
import tf_conversions

import rospy
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TransformStamped

import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryResult

from std_srvs.srv import Trigger, TriggerResponse
from std_srvs.srv import SetBool, SetBoolResponse

from nav_msgs.msg import Odometry
from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField
from std_msgs.msg import Bool, Header, String

from joint_trajectory_server import JointTrajectoryAction
from stretch_diagnostics import StretchDiagnostics


class StretchDriverNode:

    def __init__(self):
        self.default_goal_timeout_s = 10.0
        self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s)

        self.robot_stop_lock = threading.Lock()
        self.stop_the_robot = False
        self.robot_mode_rwlock = RWLock()
        self.robot_mode = None
        self.control_modes = ['position', 'navigation']
        self.prev_runstop_state = None

        self.voltage_history = []
        self.charging_state_history = [BatteryState.POWER_SUPPLY_STATUS_UNKNOWN] * 10
        self.charging_state = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN

    ###### MOBILE BASE VELOCITY METHODS #######

    def set_mobile_base_velocity_callback(self, twist):
        self.robot_mode_rwlock.acquire_read()
        if self.robot_mode != 'navigation':
            error_string = '{0} action server must be in navigation mode to receive a twist on cmd_vel. Current mode = {1}.'.format(self.node_name, self.robot_mode)
            rospy.logerr(error_string)
            self.robot_mode_rwlock.release_read()
            return
        self.linear_velocity_mps = twist.linear.x
        self.angular_velocity_radps = twist.angular.z
        self.last_twist_time = rospy.get_time()
        self.robot_mode_rwlock.release_read()

    def command_mobile_base_velocity_and_publish_state(self):
        self.robot_mode_rwlock.acquire_read()

        # set new mobile base velocities if available
        if self.robot_mode == 'navigation': 
            time_since_last_twist = rospy.get_time() - self.last_twist_time
            if time_since_last_twist < self.timeout:
                self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps)
                #self.robot.push_command() #Moved to main
            else:
                # Watchdog timer stops motion if no communication within timeout
                self.robot.base.set_velocity(0.0, 0.0)
                #self.robot.push_command() #Moved to main


        # TODO: In the future, consider using time stamps from the robot's
        # motor control boards and other boards. These would need to
        # be synchronized with the rospy clock.
        #robot_time = robot_status['timestamp_pc']
        #current_time = rospy.Time.from_sec(robot_time)
        current_time = rospy.Time.now()
        robot_status = self.robot.get_status()

        ##################################################
        # obtain odometry
        base_status = robot_status['base']
        x = base_status['x']
        y = base_status['y']
        theta = base_status['theta']
        q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta)
        x_vel = base_status['x_vel']
        x_effort = base_status['effort'][0]
        theta_vel = base_status['theta_vel']
        pose_time_s = base_status['pose_time_s']

        if self.broadcast_odom_tf:
            # publish odometry via TF
            t = TransformStamped()
            t.header.stamp = current_time
            t.header.frame_id = self.odom_frame_id
            t.child_frame_id = self.base_frame_id
            t.transform.translation.x = x
            t.transform.translation.y = y
            t.transform.translation.z = 0.0
            t.transform.rotation.x = q[0]
            t.transform.rotation.y = q[1]
            t.transform.rotation.z = q[2]
            t.transform.rotation.w = q[3]
            self.tf_broadcaster.sendTransform(t)

        # publish odometry
        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = self.odom_frame_id
        odom.child_frame_id = self.base_frame_id
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.orientation.x = q[0]
        odom.pose.pose.orientation.y = q[1]
        odom.pose.pose.orientation.z = q[2]
        odom.pose.pose.orientation.w = q[3]
        odom.twist.twist.linear.x = x_vel
        odom.twist.twist.angular.z = theta_vel
        self.odom_pub.publish(odom)

        ##################################################
        # obstain battery state
        pimu_hardware_id = self.robot.pimu.board_info['hardware_id']
        invalid_reading = float('NaN')
        v = float(robot_status['pimu']['voltage'])
        self.voltage_history.append(v)
        if len(self.voltage_history) > 100:
            self.voltage_history.pop(0)
            self.charging_state_history.pop(0)
            if v > np.mean(self.voltage_history) + 3 * np.std(self.voltage_history):
                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_CHARGING)
            elif v < np.mean(self.voltage_history) - 3 * np.std(self.voltage_history):
                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_DISCHARGING)
            else:
                self.charging_state_history.append(BatteryState.POWER_SUPPLY_STATUS_UNKNOWN)
        filtered_charging_state = max(set(self.charging_state_history), key=self.charging_state_history.count)
        if filtered_charging_state != BatteryState.POWER_SUPPLY_STATUS_UNKNOWN:
            if pimu_hardware_id == 0:
                self.charging_state = filtered_charging_state
            elif pimu_hardware_id == 1:
                if robot_status['pimu']['charger_connected'] == True and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_CHARGING
                elif robot_status['pimu']['charger_connected'] == False and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
            elif pimu_hardware_id == 2:
                if robot_status['pimu']['charger_connected'] == True and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_CHARGING
                elif robot_status['pimu']['charger_connected'] == False and filtered_charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
                    self.charging_state = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING

        i = float(robot_status['pimu']['current'])
        if self.charging_state == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
            i = float(robot_status['pimu']['current'])
        elif self.charging_state == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING:
            i = -1 * float(robot_status['pimu']['current'])

        # publish battery state
        battery_state = BatteryState()
        battery_state.header.stamp = current_time
        battery_state.voltage = v
        battery_state.current = i
        battery_state.temperature = invalid_reading
        battery_state.charge = invalid_reading
        battery_state.capacity = invalid_reading
        battery_state.percentage = invalid_reading # TODO: Calculate the percentage
        battery_state.design_capacity = 18.0
        battery_state.power_supply_status = self.charging_state
        # misuse the 'present' flag to indicated whether the barrel jack button is pressed (i.e. charger is present, but may or may not be providing power)
        if pimu_hardware_id == 0:
            battery_state.present = False
        elif pimu_hardware_id == 1 or pimu_hardware_id == 2:
            battery_state.present = robot_status['pimu']['charger_connected']
        self.power_pub.publish(battery_state)

        ##################################################
        # publish homed status
        calibration_status = Bool()
        calibration_status.data = self.robot.is_calibrated()
        self.calibration_pub.publish(calibration_status)
        self.homed_pub.publish(calibration_status)

        # publish runstop event
        runstop_event = Bool()
        runstop_event.data = robot_status['pimu']['runstop_event']
        self.runstop_event_pub.publish(runstop_event)

        # publish mode status
        mode_msg = String()
        mode_msg.data = self.robot_mode
        self.mode_pub.publish(mode_msg)

        # publish end of arm tool
        tool_msg = String()
        tool_msg.data = self.robot.end_of_arm.name
        self.tool_pub.publish(tool_msg)

        ##################################################
        # publish joint state
        joint_state = JointState()
        joint_state.header.stamp = current_time
        cgs = list(set(self.joint_trajectory_action.command_groups) - set([self.joint_trajectory_action.mobile_base_cg]))
        for cg in cgs:
            pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode)
            joint_state.name.append(cg.name)
            joint_state.position.append(pos)
            joint_state.velocity.append(vel)
            joint_state.effort.append(effort)

        # add telescoping joints and wrist_extension to joint state
        arm_cg = self.joint_trajectory_action.arm_cg
        joint_state.name.extend(arm_cg.telescoping_joints)
        pos, vel, effort = arm_cg.joint_state(robot_status)
        for _ in range(len(arm_cg.telescoping_joints)):
            joint_state.position.append(pos / len(arm_cg.telescoping_joints))
            joint_state.velocity.append(vel / len(arm_cg.telescoping_joints))
            joint_state.effort.append(effort)
        joint_state.name.append(arm_cg.wrist_extension_name)
        joint_state.position.append(pos)
        joint_state.velocity.append(vel)
        joint_state.effort.append(effort)

        # add gripper joints to joint state
        gripper_cg = self.joint_trajectory_action.gripper_cg
        if gripper_cg is not None:
            missing_gripper_joint_names = list(set(gripper_cg.gripper_joint_names) - set(joint_state.name))
            for j in missing_gripper_joint_names:
                pos, vel, effort = gripper_cg.joint_state(robot_status, joint_name=j)
                joint_state.name.append(j)
                joint_state.position.append(pos)
                joint_state.velocity.append(vel)
                joint_state.effort.append(effort)

        self.joint_state_pub.publish(joint_state)

        ##################################################
        # publish IMU sensor data
        imu_status = robot_status['pimu']['imu']
        ax = imu_status['ax']
        ay = imu_status['ay']
        az = imu_status['az']
        gx = imu_status['gx']
        gy = imu_status['gy']
        gz = imu_status['gz']
        mx = imu_status['mx']
        my = imu_status['my']
        mz = imu_status['mz']

        i = Imu()
        i.header.stamp = current_time
        i.header.frame_id = 'imu_mobile_base'
        i.angular_velocity.x = gx
        i.angular_velocity.y = gy
        i.angular_velocity.z = gz
        i.linear_acceleration.x = ax
        i.linear_acceleration.y = ay
        i.linear_acceleration.z = az
        self.imu_mobile_base_pub.publish(i)

        m = MagneticField()
        m.header.stamp = current_time
        m.header.frame_id = 'imu_mobile_base'
        self.magnetometer_mobile_base_pub.publish(m)

        accel_status = robot_status['wacc']
        ax = accel_status['ax']
        ay = accel_status['ay']
        az = accel_status['az']

        i = Imu()
        i.header.stamp = current_time
        i.header.frame_id = 'accel_wrist'
        i.linear_acceleration.x = ax
        i.linear_acceleration.y = ay
        i.linear_acceleration.z = az
        self.imu_wrist_pub.publish(i)
        ##################################################

        self.robot_mode_rwlock.release_read()
        # must happen after the read release, otherwise the write lock in change_mode() will cause a deadlock
        if (self.prev_runstop_state == None and runstop_event.data) or (self.prev_runstop_state != None and runstop_event.data != self.prev_runstop_state):
            self.runstop_the_robot(runstop_event.data, just_change_mode=True)
        self.prev_runstop_state = runstop_event.data

    ######## CHANGE MODES #########

    def change_mode(self, new_mode, code_to_run):
        self.robot_mode_rwlock.acquire_write()
        self.robot_mode = new_mode
        code_to_run()
        rospy.loginfo('{0}: Changed to mode = {1}'.format(self.node_name, self.robot_mode))
        self.robot_mode_rwlock.release_write()

    # TODO : add a freewheel mode or something comparable for the mobile base?

    def turn_on_navigation_mode(self):
        # Navigation mode enables mobile base velocity control via
        # cmd_vel, and disables position-based control of the mobile
        # base.
        def code_to_run():
            self.linear_velocity_mps = 0.0
            self.angular_velocity_radps = 0.0
        self.change_mode('navigation', code_to_run)

    def turn_on_position_mode(self):
        # Position mode enables mobile base translation and rotation
        # using position control with sequential incremental rotations
        # and translations. It also disables velocity control of the
        # mobile base. It does not update the virtual prismatic
        # joint. The frames associated with 'floor_link' and
        # 'base_link' become identical in this mode.
        def code_to_run():
            self.robot.base.enable_pos_incr_mode()
        self.change_mode('position', code_to_run)

    def home_the_robot(self):
        self.robot_mode_rwlock.acquire_read()
        can_home = self.robot_mode in self.control_modes
        last_robot_mode = copy.copy(self.robot_mode)
        self.robot_mode_rwlock.release_read()
        if not can_home:
            errmsg = f'Cannot home while in mode={last_robot_mode}.'
            rospy.logerr(errmsg)
            return False, errmsg
        def code_to_run():
            pass
        self.change_mode('homing', code_to_run)
        self.robot.home()
        self.change_mode(last_robot_mode, code_to_run)
        return True, 'Homed.'

    def stow_the_robot(self):
        self.robot_mode_rwlock.acquire_read()
        can_stow = self.robot_mode in self.control_modes
        last_robot_mode = copy.copy(self.robot_mode)
        self.robot_mode_rwlock.release_read()
        if not can_stow:
            errmsg = f'Cannot stow while in mode={last_robot_mode}.'
            rospy.logerr(errmsg)
            return False, errmsg
        def code_to_run():
            pass
        self.change_mode('stowing', code_to_run)
        self.robot.stow()
        self.change_mode(last_robot_mode, code_to_run)
        return True, 'Stowed.'

    def runstop_the_robot(self, runstopped, just_change_mode=False):
        if runstopped:
            self.robot_mode_rwlock.acquire_read()
            already_runstopped = self.robot_mode == 'runstopped'
            if not already_runstopped:
                self.prerunstop_mode = copy.copy(self.robot_mode)
            self.robot_mode_rwlock.release_read()
            if already_runstopped:
                return
            self.change_mode('runstopped', lambda: None)
            if not just_change_mode:
                self.robot.pimu.runstop_event_trigger()
        else:
            self.robot_mode_rwlock.acquire_read()
            already_not_runstopped = self.robot_mode != 'runstopped'
            self.robot_mode_rwlock.release_read()
            if already_not_runstopped:
                return
            self.change_mode(self.prerunstop_mode, lambda: None)
            if not just_change_mode:
                self.robot.pimu.runstop_event_reset()

    ######## SERVICE CALLBACKS #######

    def stop_the_robot_callback(self, request):
        with self.robot_stop_lock:
            self.stop_the_robot = True

            self.robot.base.translate_by(0.0)
            self.robot.base.rotate_by(0.0)
            self.robot.arm.move_by(0.0)
            self.robot.lift.move_by(0.0)
            #self.robot.push_command() Moved to main

            for joint in self.robot.head.joints:
                self.robot.head.move_by(joint, 0.0)
            for joint in self.robot.end_of_arm.joints:
                self.robot.end_of_arm.move_by(joint, 0.0)

        rospy.loginfo('Received stop_the_robot service call, so commanded all actuators to stop.')
        return TriggerResponse(
            success=True,
            message='Stopped the robot.'
            )

    def home_the_robot_callback(self, request):
        rospy.loginfo('Received home_the_robot service call.')
        did_succeed, msg = self.home_the_robot()
        return TriggerResponse(
            success=did_succeed,
            message=msg
        )

    def stow_the_robot_callback(self, request):
        rospy.loginfo('Recevied stow_the_robot service call.')
        did_succeed, msg = self.stow_the_robot()
        return TriggerResponse(
            success=did_succeed,
            message=msg
        )

    def navigation_mode_service_callback(self, request):
        self.turn_on_navigation_mode()
        return TriggerResponse(
            success=True,
            message='Now in navigation mode.'
        )

    def position_mode_service_callback(self, request):
        self.turn_on_position_mode()
        return TriggerResponse(
            success=True,
            message='Now in position mode.'
        )

    def runstop_service_callback(self, request):
        self.runstop_the_robot(request.data)
        return SetBoolResponse(
            success=True,
            message='is_runstopped: {0}'.format(request.data)
        )

    ########### MAIN ############

    def main(self):

        rospy.init_node('stretch_driver')
        self.node_name = rospy.get_name()

        rospy.loginfo("For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.")

        rospy.loginfo("{0} started".format(self.node_name))

        if int(stretch_body.__version__.split('.')[1]) < 5:
            rospy.logerr("ERROR: Found old stretch_body version. Please upgrade stretch_body to v0.5.0 or above.")
            rospy.signal_shutdown('Found old stretch_body version.')

        self.robot = rb.Robot()
        self.robot.startup(start_non_dxl_thread=False,start_dxl_thread=True,start_sys_mon_thread=True) #Handle the non_dxl status in local loop, not thread
        if not self.robot.is_calibrated():
            rospy.logwarn(f'{self.node_name} robot is not homed')

        mode = rospy.get_param('~mode', "position")
        if mode not in self.control_modes:
            rospy.logwarn(f'{self.node_name} given invalid mode={mode}, using position instead')
            mode = 'position'
        rospy.loginfo('mode = ' + str(mode))
        if mode == "position":
            self.turn_on_position_mode()
        elif mode == "navigation":
            self.turn_on_navigation_mode()

        self.broadcast_odom_tf = rospy.get_param('~broadcast_odom_tf', False)
        rospy.loginfo('broadcast_odom_tf = ' + str(self.broadcast_odom_tf))
        if self.broadcast_odom_tf:
            self.tf_broadcaster = tf2_ros.TransformBroadcaster()

        large_ang = np.radians(45.0)
        filename = rospy.get_param('~controller_calibration_file')
        rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename))
        with open(filename, 'r') as fid:
            self.controller_parameters = yaml.safe_load(fid)
            rospy.loginfo('controller parameters loaded = {0}'.format(self.controller_parameters))

            head_tilt_calibrated_offset_rad = self.controller_parameters['tilt_angle_offset']
            if (abs(head_tilt_calibrated_offset_rad) > large_ang):
                rospy.logwarn('WARNING: head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
            rospy.loginfo('head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_offset_rad)))

            head_pan_calibrated_offset_rad = self.controller_parameters['pan_angle_offset']
            if (abs(head_pan_calibrated_offset_rad) > large_ang):
                rospy.logwarn('WARNING: head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
            rospy.loginfo('head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_offset_rad)))

            head_pan_calibrated_looked_left_offset_rad = self.controller_parameters['pan_looked_left_offset']
            if (abs(head_pan_calibrated_looked_left_offset_rad) > large_ang):
                rospy.logwarn('WARNING: head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
            rospy.loginfo('head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(head_pan_calibrated_looked_left_offset_rad)))

            head_tilt_backlash_transition_angle_rad = self.controller_parameters['tilt_angle_backlash_transition']
            rospy.loginfo('head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(head_tilt_backlash_transition_angle_rad)))

            head_tilt_calibrated_looking_up_offset_rad = self.controller_parameters['tilt_looking_up_offset']
            if (abs(head_tilt_calibrated_looking_up_offset_rad) > large_ang):
                rospy.logwarn('WARNING: head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
            rospy.loginfo('head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(head_tilt_calibrated_looking_up_offset_rad)))

            arm_calibrated_retracted_offset_m = self.controller_parameters['arm_retracted_offset']
            if (abs(arm_calibrated_retracted_offset_m) > 0.05):
                rospy.logwarn('WARNING: arm_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE')
            rospy.loginfo('arm_calibrated_retracted_offset_m in meters = {0}'.format(arm_calibrated_retracted_offset_m))

        self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103)
        self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)

        self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)

        self.power_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
        self.calibration_pub = rospy.Publisher('is_calibrated', Bool, queue_size=1)
        self.homed_pub = rospy.Publisher('is_homed', Bool, queue_size=1)
        self.mode_pub = rospy.Publisher('mode', String, queue_size=1)
        self.tool_pub = rospy.Publisher('tool', String, queue_size=1)

        self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1)
        self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1)
        self.imu_wrist_pub = rospy.Publisher('imu_wrist', Imu, queue_size=1)
        self.runstop_event_pub = rospy.Publisher('is_runstopped', Bool, queue_size=1)

        rospy.Subscriber("cmd_vel", Twist, self.set_mobile_base_velocity_callback)

        # ~ symbol gets parameter from private namespace
        self.joint_state_rate = rospy.get_param('~rate', 30.0)
        self.timeout = rospy.get_param('~timeout', 0.5)
        rospy.loginfo("{0} rate = {1} Hz".format(self.node_name, self.joint_state_rate))
        rospy.loginfo("{0} timeout = {1} s".format(self.node_name, self.timeout))

        self.use_fake_mechaduinos = rospy.get_param('~use_fake_mechaduinos', False)
        rospy.loginfo("{0} use_fake_mechaduinos = {1}".format(rospy.get_name(), self.use_fake_mechaduinos))

        self.base_frame_id = 'base_link'
        rospy.loginfo("{0} base_frame_id = {1}".format(self.node_name, self.base_frame_id))
        self.odom_frame_id = 'odom'
        rospy.loginfo("{0} odom_frame_id = {1}".format(self.node_name, self.odom_frame_id))

        self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)

        command_base_velocity_and_publish_joint_state_rate = rospy.Rate(self.joint_state_rate)
        self.last_twist_time = rospy.get_time()

        # start action server for joint trajectories
        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', False)
        self.joint_trajectory_action = JointTrajectoryAction(self)
        self.joint_trajectory_action.server.start()
        self.diagnostics = StretchDiagnostics(self, self.robot)

        self.switch_to_navigation_mode_service = rospy.Service('/switch_to_navigation_mode',
                                                               Trigger,
                                                               self.navigation_mode_service_callback)

        self.switch_to_position_mode_service = rospy.Service('/switch_to_position_mode',
                                                             Trigger,
                                                             self.position_mode_service_callback)
        
        self.stop_the_robot_service = rospy.Service('/stop_the_robot',
                                                    Trigger,
                                                    self.stop_the_robot_callback)

        # TODO: deprecated, will be removed
        self.calibrate_the_robot_service = rospy.Service('/calibrate_the_robot',
                                                         Trigger,
                                                         self.home_the_robot_callback)

        self.home_the_robot_service = rospy.Service('/home_the_robot',
                                                    Trigger,
                                                    self.home_the_robot_callback)

        self.stow_the_robot_service = rospy.Service('/stow_the_robot',
                                                    Trigger,
                                                    self.stow_the_robot_callback)

        self.runstop_service = rospy.Service('/runstop',
                                              SetBool,
                                              self.runstop_service_callback)

        try:
            # start loop to command the mobile base velocity, publish
            # odometry, and publish joint states
            while not rospy.is_shutdown():
                self.robot.non_dxl_thread.step()
                if self.robot.pimu.params.get('ros_fan_on', True):
                    self.robot.pimu.set_fan_on()
                self.command_mobile_base_velocity_and_publish_state()
                self.robot.push_command()
                self.dirty_command=False
                command_base_velocity_and_publish_joint_state_rate.sleep()
        except (rospy.ROSInterruptException, ThreadServiceExit):
            self.robot.stop()
            rospy.signal_shutdown("stretch_driver shutdown")


if __name__ == '__main__':
    node = StretchDriverNode()
    node.main()