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 __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 __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: motor_states = [self.__process_motor_feedback(motor_id) for motor_id in self.motors] 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()
self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) except dynamixel_io.ChecksumError, cse: self.error_counts['checksum'] += 1 rospy.logdebug(cse) except dynamixel_io.DroppedPacketError, dpe: self.error_counts['dropped'] += 1 rospy.logdebug(dpe.message) 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 __publish_diagnostic_information(self): diag_msg = DiagnosticArray()
self.error_counts['non_fatal'] += 1 rospy.logdebug(nfece) except dynamixel_io.ChecksumError, cse: self.error_counts['checksum'] += 1 rospy.logdebug(cse) except dynamixel_io.DroppedPacketError, dpe: self.error_counts['dropped'] += 1 rospy.logdebug(dpe.message) 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 __publish_diagnostic_information(self): diag_msg = DiagnosticArray()