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 __init__(self,
port_name='/dev/ttyUSB0',
port_namespace='ttyUSB0',
baud_rate='1000000',
min_motor_id=1,
max_motor_id=25,
update_rate=5,
diagnostics_rate=1,
error_level_temp=75,
warn_level_temp=70,
readback_echo=False):
self.port_name = port_name
self.port_namespace = port_namespace
self.baud_rate = baud_rate
self.min_motor_id = min_motor_id
self.max_motor_id = max_motor_id
self.update_rate = update_rate
self.diagnostics_rate = diagnostics_rate
self.error_level_temp = error_level_temp
self.warn_level_temp = warn_level_temp
self.readback_echo = readback_echo
self.actual_rate = update_rate
self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
self.current_state = MotorStateList()
self.num_ping_retries = 5
self.motor_states_pub = rospy.Publisher('motor_states/%s' %
self.port_namespace,
MotorStateList,
queue_size=1)
self.diagnostics_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
def __init__(self,
port_name='/dev/ttyUSB0',
port_namespace='ttyUSB0',
baud_rate='1000000',
min_motor_id=1,
max_motor_id=25,
update_rate=5,
diagnostics_rate=1,
error_level_temp=75,
warn_level_temp=70,
readback_echo=False):
self.port_name = port_name
self.port_namespace = port_namespace
self.baud_rate = baud_rate
self.min_motor_id = min_motor_id
self.max_motor_id = max_motor_id
self.update_rate = update_rate
self.diagnostics_rate = diagnostics_rate
self.error_level_temp = error_level_temp
self.warn_level_temp = warn_level_temp
self.readback_echo = readback_echo
self.encoder_id = ENC_ID
self.actual_rate = update_rate
self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
self.current_state = MotorStateList()
self.num_ping_retries = 5
# Not sure why the author put this in here, it messed up a lot of stuff along the line,
# as the user is supposed to state the min/max motors already
self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList, queue_size=1)
self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
# Publisher to declare whether all motors communication has been established
self.done_init_pub = rospy.Publisher('/done_init', Bool, queue_size = 1)
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 __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()
def __init__(self,
port_name='/dev/ttyUSB0',
port_namespace='ttyUSB0',
baud_rate='1000000',
min_motor_id=1,
max_motor_id=25,
update_rate=5,
diagnostics_rate=1,
error_level_temp=75,
warn_level_temp=70,
readback_echo=False):
self.port_name = port_name
self.port_namespace = port_namespace
self.baud_rate = baud_rate
self.min_motor_id = min_motor_id
self.max_motor_id = max_motor_id
self.update_rate = update_rate
self.diagnostics_rate = diagnostics_rate
self.error_level_temp = error_level_temp
self.warn_level_temp = warn_level_temp
self.readback_echo = readback_echo
self.actual_rate = update_rate
self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
self.current_state = MotorStateList()
self.num_ping_retries = 5
self.offsets = dict(
(key, 0.0)
for key in range(self.min_motor_id, self.max_motor_id + 1))
self.motor_encoder_ticks_per_radian = dict(
(key, 0.0)
for key in range(self.min_motor_id, self.max_motor_id + 1))
self.motor_states_pub = rospy.Publisher('motor_states/%s' %
self.port_namespace,
MotorStateList,
queue_size=1)
self.diagnostics_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
self.offsets_srv = rospy.Service(
'dynamixel/%s/set_offset' % (self.port_namespace), SetMotorOffset,
self.offsets_callback)
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 __init__(self,
port_name='/dev/ttyUSB0',
port_namespace='ttyUSB0',
baud_rate=1000000,
min_motor_id=1,
max_motor_id=25,
update_rate=5,
diagnostics_rate=1,
error_level_temp=75,
warn_level_temp=70,
readback_echo=False,
protocol_version=2.0):
# Initialize variables for Serial Proxy
self.port_name = port_name
self.port_namespace = port_namespace
self.baud_rate = baud_rate
self.min_motor_id = min_motor_id
self.max_motor_id = max_motor_id
self.update_rate = update_rate
self.diagnostics_rate = diagnostics_rate
self.error_level_temp = error_level_temp
self.warn_level_temp = warn_level_temp
self.readback_echp = readback_echo
self.protocol_version = protocol_version
self.actual_rate = update_rate
self.error_counts = {'non_fatal': 0, 'checksum': 0, 'dropped': 0}
self.current_state = MotorStateList()
self.num_ping_retries = 5
self.dynotools = dynamixel_tools.DynamixelTools()
self.sdk_io = sdk_serial_wrapper.SDKSerialWrapper(port_name, baud_rate)
self.angles = {}
# Start to publish motor states
self.motor_states_pub = rospy.Publisher('motor_states/%s' % self.port_namespace, MotorStateList, queue_size = 1)
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)
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()
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)
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()
