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()
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']
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)
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()
class TrajectoryManager:
def __init__(self, controller_names):
rospy.init_node('trajectory_controller', anonymous=True)
rospy.on_shutdown(self.on_shutdown)
self.controllers = {}
self.serial_proxies = {}
self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1)
# Load ROS parameters for serial port
manager_namespace = rospy.get_param('~namespace')
serial_ports = rospy.get_param('~serial_ports')
# Set up serial port proxy
for port_namespace, port_config in serial_ports.items():
port_name = port_config['port_name']
baud_rate = port_config['baud_rate']
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
print(port_name)
self.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)
self.serial_proxy.connect()
self.converter = TrajectoryConverter()
for c_name in controller_names:
if "-1" == rospy.get_param(root_path + c_name + "/motor_master", "-1"):
self.controllers[c_name] = JointPositionController(self.serial_proxy.dxl_io, root_path + c_name, 'arbotix_port')
else:
self.controllers[c_name] = JointPositionControllerDual(self.serial_proxy.dxl_io, root_path + c_name, 'arbotix_port')
dyn_name = rospy.get_param(root_path + c_name + "/joint_name", "")
reverse = rospy.get_param(root_path + c_name + "/reverse", False)
offset = rospy.get_param(root_path + c_name + "/offset", 0)
self.converter.add_joint(dyn_name, dyn_name, reverse, offset)
for c in self.controllers.values():
if isinstance(c, JointPositionControllerDual):
print(c.controller_namespace, "Dual")
elif isinstance(c, JointPositionController):
print(c.controller_namespace, "Single")
else:
print(type(c))
c.initialize()
self.trajectory_manager = JointTrajectoryActionController("trajectory_controller", self.controllers.values())
self.trajectory_manager.initialize()
self.trajectory_manager.set_converter(self.converter)
self.trajectory_manager.start()
def on_shutdown(self):
self.serial_proxy.disconnect()
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()