def __init__(self,_port_name, _setup_info):
"""
Constructor. Takes a string of the port name as input,
and uses it to set up ROS connections.
Also needs the settings that will be used to set up the proxies and wrapper.
On completion, the PortManager, SerialProxy, and Wrapper will function.
"""
# The port_name refers to the path to the USB port the motors are attached to
self.port_name = _port_name
# Initializes a ROS node for this port.
# Necessary for the real Wrapper and Proxy to work.
rospy.init_node('portManager', anonymous=True)
# Constructs a wrapper for the port using the settings provided
self.wrapper = SDKSerialWrapper('/dev/{_port_name}'.format(_port_name=_port_name),
_setup_info["baudrate"])
# Constructs a proxy for the port using the settings provided
self.proxy = DynomixSerialProxy("/dev/{_port_name}".format(_port_name=_port_name),
_port_name, _setup_info["baudrate"], _setup_info["minID"], _setup_info["maxID"],
_setup_info["updateRate"], _setup_info["diagnosticsRate"])
# Activates the proxy
# Turns on the connection to the physical motors.
# Gathers static information about them.
# Stores the list of motor id's attached in proxy.motors
self.proxy.connect()
# Stores the list of motors in the PortManager
# This may not be strictly necessary
# TODO: Consider removing and calling down to the proxy for needs.
self.servos = self.proxy.motors
def __init__(self):
rospy.init_node('dynomix_controller_manager', anonymous=True)
# rospy.on_shutdown(self.on_shutdown)
self.waiting_meta_controllers = []
# self.controllers = {
# "l_shoulder_pitch_controller":{
# "controller_name": "l_shoulder_pitch_controller",
# "port_name": "/dev/ttyUSB0",
# "joint_name": "l_shoulder_pitch_joint",
# "joint_speed": 1.17,
# "port_namespace": "l_arm_port"
# },
# "l_shoulder_pan_controller": {
# "controller_name": "l_shoulder_pan_controller",
# "port_name": "/dev/ttyUSB0",
# "joint_name": "l_shoulder_pan_joint",
# "joint_speed": 1.17,
# "port_namespace": "l_arm_port"
# }
# }
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']
readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False
protocol_version = port_config['protocol_version'] if 'protocol_version' in port_config else 2.0
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 = DynomixSerialProxy(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)
serial_proxy.connect()
# self.serial_proxies[port_namespace] = serial_proxy
# serial_proxy = DynomixSerialProxy("/dev/ttyUSB0",
# "l_arm_port",
# "1000000",
# 1,
# 20,
# 5,
# 5,
# 75,
# 70,
# 0)
# 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)