def __init__(self):
rospy.init_node('dynamixel_controller_manager', anonymous=True)
rospy.on_shutdown(self.on_shutdown)
self.waiting_meta_controllers = []
self.controllers = {}
self.serial_proxies = {}
self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1)
self.start_controller_lock = Lock()
self.stop_controller_lock = Lock()
self.simulation_only = rospy.get_param('~use_sim', False)
manager_namespace = rospy.get_param('~namespace')
serial_ports = rospy.get_param('~serial_ports')
for port_namespace, port_config in serial_ports.items():
port_name = port_config['port_name']
baud_rate = port_config['baud_rate']
readback_echo = port_config[
'readback_echo'] if 'readback_echo' in port_config else False
min_motor_id = port_config[
'min_motor_id'] if 'min_motor_id' in port_config else 0
max_motor_id = port_config[
'max_motor_id'] if 'max_motor_id' in port_config else 253
update_rate = port_config[
'update_rate'] if 'update_rate' in port_config else 5
error_level_temp = 75
warn_level_temp = 70
if 'diagnostics' in port_config:
if 'error_level_temp' in port_config['diagnostics']:
error_level_temp = port_config['diagnostics'][
'error_level_temp']
if 'warn_level_temp' in port_config['diagnostics']:
warn_level_temp = port_config['diagnostics'][
'warn_level_temp']
serial_proxy = SerialProxy(port_name, port_namespace, baud_rate,
min_motor_id, max_motor_id, update_rate,
self.diagnostics_rate, error_level_temp,
warn_level_temp, readback_echo,
self.simulation_only)
serial_proxy.connect()
# will create a set of services for each serial port under common manager namesapce
# e.g. /dynamixel_manager/robot_arm_port/start_controller
# /dynamixel_manager/robot_head_port/start_controller
# where 'dynamixel_manager' is manager's namespace
# 'robot_arm_port' and 'robot_head_port' are human readable names for serial ports
rospy.Service(
'%s/%s/start_controller' % (manager_namespace, port_namespace),
StartController, self.start_controller)
rospy.Service(
'%s/%s/stop_controller' % (manager_namespace, port_namespace),
StopController, self.stop_controller)
rospy.Service(
'%s/%s/restart_controller' %
(manager_namespace, port_namespace), RestartController,
self.restart_controller)
self.serial_proxies[port_namespace] = serial_proxy
# services for 'meta' controllers, e.g. joint trajectory controller
# these controllers don't have their own serial port, instead they rely
# on regular controllers for serial connection. The advantage of meta
# controller is that it can pack commands for multiple motors on multiple
# serial ports.
# NOTE: all serial ports that meta controller needs should be managed by
# the same controler manager.
rospy.Service('%s/meta/start_controller' % manager_namespace,
StartController, self.start_controller)
rospy.Service('%s/meta/stop_controller' % manager_namespace,
StopController, self.stop_controller)
rospy.Service('%s/meta/restart_controller' % manager_namespace,
RestartController, self.restart_controller)
self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
if self.diagnostics_rate > 0:
Thread(target=self.diagnostics_processor).start()
def __init__(self):
super().__init__('dynamixel_controller_manager',
automatically_declare_parameters_from_overrides=True)
self.waiting_meta_controllers = []
self.controllers = {}
self.serial_proxies = {}
self.diagnostics_rate = self.get_parameter_or('diagnostics_rate', 1)
if not isinstance(self.diagnostics_rate, int):
self.diagnostics_rate = self.diagnostics_rate.value
self.start_controller_lock = Lock()
self.stop_controller_lock = Lock()
manager_namespace = self.get_parameter('namespace', ).value
serial_ports = {}
for name, param in self.get_parameters_by_prefix(
'serial_ports').items():
name1, name2 = name.split('.')
port = serial_ports.setdefault(name1, {})
port[name2] = param.value
for port_namespace, port_config in serial_ports.items():
port_name = port_config['port_name']
baud_rate = port_config['baud_rate']
readback_echo = port_config[
'readback_echo'] if 'readback_echo' in port_config else False
min_motor_id = port_config[
'min_motor_id'] if 'min_motor_id' in port_config else 0
max_motor_id = port_config[
'max_motor_id'] if 'max_motor_id' in port_config else 253
update_rate = port_config[
'update_rate'] if 'update_rate' in port_config else 5
error_level_temp = 75
warn_level_temp = 70
if 'diagnostics' in port_config:
if 'error_level_temp' in port_config['diagnostics']:
error_level_temp = port_config['diagnostics'][
'error_level_temp']
if 'warn_level_temp' in port_config['diagnostics']:
warn_level_temp = port_config['diagnostics'][
'warn_level_temp']
serial_proxy = SerialProxy(port_name,
port_namespace,
baud_rate,
min_motor_id,
max_motor_id,
update_rate,
self.diagnostics_rate,
error_level_temp,
warn_level_temp,
readback_echo,
node_namespace=self.get_namespace())
serial_proxy.connect()
# will create a set of services for each serial port under common manager namesapce
# e.g. /dynamixel_manager/robot_arm_port/start_controller
# /dynamixel_manager/robot_head_port/start_controller
# where 'dynamixel_manager' is manager's namespace
# 'robot_arm_port' and 'robot_head_port' are human readable names for serial ports
self.create_service(
StartController,
'%s/%s/start_controller' % (manager_namespace, port_namespace),
self.start_controller)
self.create_service(
StopController,
'%s/%s/stop_controller' % (manager_namespace, port_namespace),
self.stop_controller)
self.create_service(
RestartController, '%s/%s/restart_controller' %
(manager_namespace, port_namespace), self.restart_controller)
self.serial_proxies[port_namespace] = serial_proxy
# services for 'meta' controllers, e.g. joint trajectory controller
# these controllers don't have their own serial port, instead they rely
# on regular controllers for serial connection. The advantage of meta
# controller is that it can pack commands for multiple motors on multiple
# serial ports.
# NOTE: all serial ports that meta controller needs should be managed by
# the same controler manager.
self.create_service(StartController,
'%s/meta/start_controller' % manager_namespace,
self.start_controller)
self.create_service(StopController,
'%s/meta/stop_controller' % manager_namespace,
self.stop_controller)
self.create_service(RestartController,
'%s/meta/restart_controller' % manager_namespace,
self.restart_controller)
self.diagnostics_pub = self.create_publisher(DiagnosticArray,
'/diagnostics', 1)
if self.diagnostics_rate > 0:
Thread(target=self.diagnostics_processor).start()