def fill_msg():
    motor_list = MotorStateList()
    for i in range(5):
        motor_state = MotorState()
        motor_state.voltage = 16.2 - i / 5.0
        motor_list.motor_states.append(motor_state)
    return motor_list
Esempio n. 2
0
 def loop(self):
     while not rospy.is_shutdown():
         ms = MotorState()
         self.lock.acquire()
         ms.load = self.head.current_load
         self.lock.release()
         self.head_load.publish(ms)
         rospy.Rate(50)
Esempio n. 3
0
        def _BroadcastJointStateinfo_P1(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = ((321-float(lineParts[1]))*0.008)-0.15
                        P2 = self.left_tilt
                        P3 = float(lineParts[3])# current
                        P4 = 0#float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P1_MotorPublisher.publish(Motor_State)
                        self._left_tilt_Publisher.publish(P1)
                        Joint_State = JointState()
                        Joint_State.name = "left_arm_tilt_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P1_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))
Esempio n. 4
0
        def _BroadcastJointStateinfo_P4(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57)
                        P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57)
                        P3 = float(lineParts[3])
                        P4 = float(lineParts[4])
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P4_MotorPublisher.publish(Motor_State)
                        #rospy.logwarn(Motor_State)
                        #rospy.logwarn(val)

                        Joint_State = JointState()
                        Joint_State.name = "right_arm_rotate_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P4_JointPublisher.publish(Joint_State)
                except:
                        rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))
Esempio n. 5
0
        def _BroadcastJointStateinfo_P1(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position
                        P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target
                        P3 = float(lineParts[3])# current
                        P4 = float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P2_MotorPublisher.publish(Motor_State)
                       
                        Joint_State = JointState()
                        Joint_State.name = "right_arm_pan_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P1_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))
 def __update_motor_states(self):
   num_events = 50
   rates = deque([float(self.update_rate)]*num_events, maxlen=num_events)
   last_time = rospy.Time.now()
   
   rate = rospy.Rate(self.update_rate)
   while not rospy.is_shutdown() and self.running:
     # get current state of all motors and publish to motor_states topic
     motor_states = []
     for motor_id in self.motors:
       try:
         state = self.get_feedback(motor_id)
         if state:
           motor_states.append(MotorState(**state))
       except OSError, ose:
         if ose.errno != errno.EAGAIN:
           rospy.logfatal(errno.errorcode[ose.errno])
           rospy.signal_shutdown(errno.errorcode[ose.errno])
           
     if motor_states:
       msl = MotorStateList()
       msl.motor_states = motor_states
       self.motor_states_pub.publish(msl)
       
       self.current_state = msl
       
       # calculate actual update rate
       current_time = rospy.Time.now()
       rates.append(1.0 / (current_time - last_time).to_sec())
       self.actual_rate = round(sum(rates)/num_events, 2)
       last_time = current_time
         
     rate.sleep()
Esempio n. 7
0
 def __process_motor_feedback(self, motor_id):
     try:
         state = self.dxl_io.get_feedback(motor_id)
         if state:
             # motor_states.append(MotorState(**state))
             if dynamixel_io.exception:
                 raise dynamixel_io.exception
             return MotorState(**state)
     except dynamixel_io.FatalErrorCodeError as fece:
         rospy.logerr(fece)
         me = MotorError(error_type="FatalErrorCodeError",
                         error_message=fece.message,
                         extra_info=fece.message.split(' ')[3][1:])
         self.error_pub.publish(me)
     except dynamixel_io.NonfatalErrorCodeError as nfece:
         self.error_counts['non_fatal'] += 1
         rospy.logdebug(nfece)
     except dynamixel_io.ChecksumError as cse:
         self.error_counts['checksum'] += 1
         rospy.logdebug(cse)
     except dynamixel_io.DroppedPacketError as dpe:
         self.error_counts['dropped'] += 1
         rospy.logdebug(dpe.message)
     except OSError as ose:
         if ose.errno != errno.EAGAIN:
             rospy.logfatal(errno.errorcode[ose.errno])
             rospy.signal_shutdown(errno.errorcode[ose.errno])
Esempio n. 8
0
    def __update_motor_states(self):
        num_events = 50
        rates = deque([float(self.update_rate)] * num_events,
                      maxlen=num_events)
        last_time = rospy.Time.now()

        #        rospy.logerr( 'in __update_motor_states' )

        rate = rospy.Rate(self.update_rate)
        while not rospy.is_shutdown() and self.running:
            # get current state of all motors and publish to motor_states topic
            motor_states = []
            for motor_id in self.motors:
                try:
                    state = self.dxl_io.get_feedback(motor_id)

                    #                    rospy.logerr( 'position for motor: %d: %d', motor_id, state[ 'position' ] )

                    if state:
                        motor_states.append(MotorState(**state))
                        if dynamixel_io.exception: raise dynamixel_io.exception
                except dynamixel_io.FatalErrorCodeError, fece:
                    rospy.logerr(fece)
                except dynamixel_io.NonfatalErrorCodeError, nfece:
                    self.error_counts['non_fatal'] += 1
                    rospy.logdebug(nfece)
                except dynamixel_io.ChecksumError, cse:
                    self.error_counts['checksum'] += 1
                    rospy.logdebug(cse)
def main():
    rospy.init_node('motor_monitor_test')
    pub = rospy.Publisher('motor_states', MotorStateList, queue_size=2)
    motor_ids = [1, 2, 3]
    motor_list = MotorStateList()

    for m_id in motor_ids:
        m = MotorState()
        m.id = m_id
        motor_list.motor_states.append(m)

    rate = rospy.Rate(50)
    i = 20
    while not rospy.is_shutdown():
        for state in motor_list.motor_states:
            state.load = random.random()
            i += 0.01
            state.temperature = min(i, 80)
            state.error = random.randint(-20, 20)
        pub.publish(motor_list)
        rate.sleep()
Esempio n. 10
0
    def __update_motor_states(self):
        num_events = 50
        rates = deque([float(self.update_rate)] * num_events,
                      maxlen=num_events)
        last_time = self.get_clock().now()

        rate = self.create_rate(self.update_rate)
        while rclpy.ok() and self.running:
            # get current state of all motors and publish to motor_states topic
            motor_states = []
            for motor_id in self.motors:
                try:
                    state = self.dxl_io.get_feedback(motor_id)
                    if state:
                        motor_states.append(MotorState(**state))
                        if dynamixel_io.exception: raise dynamixel_io.exception
                except dynamixel_io.FatalErrorCodeError as fece:
                    self.get_logger().error(fece)
                except dynamixel_io.NonfatalErrorCodeError as nfece:
                    self.error_counts['non_fatal'] += 1
                    self.get_logger().debug(nfece)
                except dynamixel_io.ChecksumError as cse:
                    self.error_counts['checksum'] += 1
                    self.get_logger().debug(cse)
                except dynamixel_io.DroppedPacketError as dpe:
                    self.error_counts['dropped'] += 1
                    self.get_logger().debug(dpe.message)
                except OSError as ose:
                    if ose.errno != errno.EAGAIN:
                        self.get_logger().fatal(errno.errorcode[ose.errno])
                        #rospy.signal_shutdown(errno.errorcode[ose.errno])
                        rclpy.shutdown()

            if motor_states:
                msl = MotorStateList()
                msl.motor_states = motor_states
                self.motor_states_pub.publish(msl)

                self.current_state = msl

                # calculate actual update rate
                current_time = self.get_clock().now()
                rates.append(1000000000.0 /
                             (current_time - last_time).nanoseconds)
                self.actual_rate = round(sum(rates) / num_events, 2)
                last_time = current_time

            rate.sleep()
Esempio n. 11
0
    def _BroadcastJointStateinfo_P5(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 7):
            pass
        try:
            P1 = 0 - ((radians((float(lineParts[1]))) / 10) - 0.65)
            P2 = self.right_rotate  #0-((float(lineParts[2])* 0.00174532925)-1.57)
            P3 = float(lineParts[3])
            P4 = 0
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P5_MotorPublisher.publish(Motor_State)
            #rospy.logwarn(Motor_State)
            self._right_rotate_Publisher.publish(P1)

            Joint_State = JointState()
            Joint_State.name = "right_arm_rotate_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now()
            self._P5_JointPublisher.publish(Joint_State)
            #rospy.logwarn(val)

        except:
            rospy.logwarn("Unexpected error:right_arm_rotate_joint" +
                          str(sys.exc_info()[0]))
Esempio n. 12
0
    def _BroadcastJointStateinfo_P2(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 4):
            pass
        try:

            P1 = radians(float(lineParts[1]) * 0.2)
            P2 = self.right_tilt
            P3 = 0  #float(lineParts[3])# current
            P4 = 0  #float(lineParts[4])# speed
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P2_MotorPublisher.publish(Motor_State)
            self._right_tilt_Publisher.publish(P1)
            Joint_State = JointState()
            Joint_State.name = "right_arm_tilt_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now(
            )  #.stamprospy.Time.from_sec(state.timestamp)
            self._P2_JointPublisher.publish(Joint_State)
            #rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:right_arm_tilt_joint" +
                          str(sys.exc_info()[0]))
Esempio n. 13
0
        def _BroadcastJointStateinfo_P8(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(lineParts)
                if (partsCount  < 4):
                        pass
                try:
                        P1 = radians(float(lineParts[1]))#P1 = radians(float(lineParts[1]))
                        P2 = self.pan
                        P3 = 0#float(lineParts[3])
                        P4 = 0
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P8_MotorPublisher.publish(Motor_State)
                        self._pan_Publisher.publish(P1)

                        Joint_State = JointState()
                        Joint_State.name = "pan_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P8_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(val)

                except:
                        pass#rospy.logwarn("Unexpected error:pan_joint" + str(sys.exc_info()[0]))
Esempio n. 14
0
        def _BroadcastJointStateinfo_P2(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        cal = 0
                        A1 = (1023 + cal) - float(lineParts[1])
                        P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position
                        A2 = (1023 + cal) - float(lineParts[1])
                        P2 = (A2* 0.003070961)#float(lineParts[2])# target
                        P3 = float(lineParts[3])# current
                        P4 = float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P2_MotorPublisher.publish(Motor_State)
                       
                        Joint_State = JointState()
                        Joint_State.name = "right_arm_lift_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P2_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))
Esempio n. 15
0
    def _BroadcastJointStateinfo_P2(self, lineParts):
        partsCount = len(lineParts)
        # rospy.logwarn(partsCount)
        if partsCount < 7:
            pass
        try:
            off = 299
            A1 = float(lineParts[1]) - off
            P1 = 0 - ((A1 * 0.00174532925) - 0)
            A2 = float(lineParts[2]) - off
            P2 = 0 - ((A2 * 0.00174532925) - 0)
            P3 = float(lineParts[3])  # current
            P4 = 0  # float(lineParts[4])# speed
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P2_MotorPublisher.publish(Motor_State)
            self._right_tilt_Publisher.publish(P1)
            Joint_State = JointState()
            Joint_State.name = "right_arm_tilt_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now()  # .stamprospy.Time.from_sec(state.timestamp)
            self._P2_JointPublisher.publish(Joint_State)
            # rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))
Esempio n. 16
0
        def _BroadcastJointStateinfo_P3(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        #offset = Float(-1.57)
                        P1 = (float(lineParts[1])/1000)-0.6
                        P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
                        P3 = 0#float(lineParts[3])
                        P4 = 0
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P3_MotorPublisher.publish(Motor_State)
                        #rospy.logwarn(Motor_State)
                        self._left_lift_Publisher.publish(P1)

                        Joint_State = JointState()
                        Joint_State.name = "left_arm_lift_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P3_JointPublisher.publish(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))
Esempio n. 17
0
    def __init__(self):
        self.axis_for_right = float(rospy.get_param(
            '~axis_for_right',
            0))  # if right calibrates first, this should be 0, else 1
        self.wheel_track = float(rospy.get_param(
            '~wheel_track', 0.285))  # m, distance between wheel centres
        self.tyre_circumference = float(
            rospy.get_param('~tyre_circumference', 0.341)
        )  # used to translate velocity commands in m/s into motor rpm

        self.connect_on_startup = rospy.get_param('~connect_on_startup', False)
        self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup',
                                                    False)
        self.engage_on_startup = rospy.get_param('~engage_on_startup', False)

        self.max_speed = rospy.get_param('~max_speed', 0.5)
        self.max_angular = rospy.get_param('~max_angular', 1.0)

        self.publish_current = rospy.get_param('~publish_current', True)

        self.has_preroll = rospy.get_param('~use_preroll', False)

        self.publish_current = rospy.get_param('~publish_current', True)

        self.publish_odom = rospy.get_param('~publish_odom', True)
        self.publish_tf = rospy.get_param('~publish_odom_tf', True)
        self.odom_topic = rospy.get_param('~odom_topic', "odom")
        self.odom_frame = rospy.get_param('~odom_frame', "odom")
        self.base_frame = rospy.get_param('~base_frame', "base_link")
        self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20)

        self.publish_joint_state = rospy.get_param('~publish_joint_state',
                                                   True)
        self.joint_state_topic = rospy.get_param('~joint_state_topic',
                                                 "joint_state")
        self.Calibrate_Axis = rospy.get_param('~Calibrate_Axis', 1)
        self.motor_id = rospy.get_param('~motor_id', "0")

        rospy.on_shutdown(self.terminate)

        rospy.Service('connect_driver', std_srvs.srv.Trigger,
                      self.connect_driver)
        rospy.Service('disconnect_driver', std_srvs.srv.Trigger,
                      self.disconnect_driver)

        rospy.Service('calibrate_motors', std_srvs.srv.Trigger,
                      self.calibrate_motor)
        rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor)
        rospy.Service('release_motors', std_srvs.srv.Trigger,
                      self.release_motor)

        self.vel_subscribe = rospy.Subscriber("/cmd_vel",
                                              Twist,
                                              self.cmd_vel_callback,
                                              queue_size=2)

        self.timer = rospy.Timer(
            rospy.Duration(0.1),
            self.timer_check)  # stop motors if no cmd_vel received > 1second

        if self.publish_current:
            self.current_loop_count = 0
            self.left_current_accumulator = 0.0
            self.right_current_accumulator = 0.0
            self.current_timer = rospy.Timer(
                rospy.Duration(0.05), self.timer_current
            )  # publish motor currents at 10Hz, read at 50Hz
            self.current_publisher_left = rospy.Publisher(
                'odrive/left_current', Float64, queue_size=2)
            self.current_publisher_right = rospy.Publisher(
                'odrive/right_current', Float64, queue_size=2)
            rospy.loginfo("ODrive will publish motor currents.")

        if self.publish_odom:
            rospy.Service('reset_odometry', std_srvs.srv.Trigger,
                          self.reset_odometry)

            self.odom_publisher = rospy.Publisher(self.odom_topic,
                                                  Odometry,
                                                  queue_size=2)
            # setup message
            self.odom_msg = Odometry()
            #print(dir(self.odom_msg))
            self.odom_msg.header.frame_id = self.odom_frame
            self.odom_msg.child_frame_id = self.base_frame
            self.odom_msg.pose.pose.position.z = 0.0  # always on the ground, we hope
            self.odom_msg.pose.pose.orientation.x = 0.0  # always vertical
            self.odom_msg.pose.pose.orientation.y = 0.0  # always vertical
            self.odom_msg.twist.twist.linear.y = 0.0  # no sideways
            self.odom_msg.twist.twist.linear.z = 0.0  # or upwards... only forward
            self.odom_msg.twist.twist.angular.x = 0.0  # or roll
            self.odom_msg.twist.twist.angular.y = 0.0  # or pitch... only yaw

            # store current location to be updated.
            self.x = 0.0
            self.y = 0.0
            self.theta = 0.0

            # setup transform
            self.tf_publisher = tf2_ros.TransformBroadcaster()
            self.tf_msg = TransformStamped()
            self.tf_msg.header.frame_id = self.odom_frame
            self.tf_msg.child_frame_id = self.base_frame
            self.tf_msg.transform.translation.z = 0.0
            self.tf_msg.transform.rotation.x = 0.0
            self.tf_msg.transform.rotation.y = 0.0

            self.odom_timer = rospy.Timer(
                rospy.Duration(1 / float(self.odom_calc_hz)),
                self.timer_odometry)

        if self.publish_joint_state:
            self.joint_state_publisher = rospy.Publisher(
                self.joint_state_topic, JointState, queue_size=2)
            # setup message
            self.joint_state_msg = JointState()
            self.joint_state_msg.name = self.motor_id

            self.state_subscriber = rospy.Subscriber(
                'motor_states/%s' % self.motor_id, MotorState,
                self.state_callback)
            # setup message
            self.state = MotorState()

            self.RADIANS_PER_ENCODER_TICK = 2.0 * 3.1415926 / 4000.0
            self.ENCODER_TICKS_PER_RADIAN = 1.0 / self.RADIANS_PER_ENCODER_TICK
            #print(dir(self.odom_msg))
            # self.joint_state_msg.header.frame_id = self.odom_frame
            # self.joint_state_msg.child_frame_id = self.base_frame

        if not self.connect_on_startup:
            rospy.loginfo("ODrive node started, but not connected.")
            return

        if not self.connect_driver(None)[0]:
            return  # Failed to connect

        if not self.calibrate_on_startup:
            rospy.loginfo("ODrive node started and connected. Not calibrated.")
            return

        if not self.calibrate_motor(None)[0]:
            return

        if not self.engage_on_startup:
            rospy.loginfo("ODrive connected and configured. Engage to drive.")
            return

        if not self.engage_motor(None)[0]:
            return

        rospy.loginfo("ODrive connected and configured. Ready to drive.")