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()
