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
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)
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]))
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]))
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()
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])
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()
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()
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]))
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]))
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]))
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]))
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]))
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]))
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.")