def start_controller(self, req):
port_name = req.port_name
package_path = req.package_path
module_name = req.module_name
class_name = req.class_name
controller_name = req.controller_name
self.start_controller_lock.acquire()
if controller_name in self.controllers:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Controller [%s] already started. If you want to restart it, call restart.'
% controller_name)
try:
if module_name not in sys.modules:
# import if module not previously imported
package_module = __import__(package_path, globals(), locals(),
[module_name], -1)
else:
# reload module if previously imported
package_module = reload(sys.modules[package_path])
controller_module = getattr(package_module, module_name)
except ImportError, ie:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Cannot find controller module. Unable to start controller %s\n%s'
% (module_name, str(ie)))
class ControllerManager:
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 on_shutdown(self):
for serial_proxy in self.serial_proxies.values():
serial_proxy.disconnect()
def diagnostics_processor(self):
diag_msg = DiagnosticArray()
rate = rospy.Rate(self.diagnostics_rate)
while not rospy.is_shutdown():
diag_msg.status = []
diag_msg.header.stamp = rospy.Time.now()
for controller in self.controllers.values():
try:
joint_state = controller.joint_state
temps = joint_state.motor_temps
max_temp = max(temps)
status = DiagnosticStatus()
status.name = 'Joint Controller (%s)' % controller.joint_name
status.hardware_id = 'Robotis Dynamixel %s on port %s' % (
str(joint_state.motor_ids), controller.port_namespace)
status.values.append(
KeyValue('Goal', str(joint_state.goal_pos)))
status.values.append(
KeyValue('Position', str(joint_state.current_pos)))
status.values.append(
KeyValue('Error', str(joint_state.error)))
status.values.append(
KeyValue('Velocity', str(joint_state.velocity)))
status.values.append(
KeyValue('Load', str(joint_state.load)))
status.values.append(
KeyValue('Moving', str(joint_state.is_moving)))
status.values.append(KeyValue('Temperature',
str(max_temp)))
status.level = DiagnosticStatus.OK
status.message = 'OK'
diag_msg.status.append(status)
except:
pass
self.diagnostics_pub.publish(diag_msg)
rate.sleep()
def check_deps(self):
controllers_still_waiting = []
for i, (controller_name, deps,
kls) in enumerate(self.waiting_meta_controllers):
if not set(deps).issubset(self.controllers.keys()):
controllers_still_waiting.append(
self.waiting_meta_controllers[i])
rospy.logwarn(
'[%s] not all dependencies started, still waiting for %s...'
%
(controller_name,
str(list(set(deps).difference(self.controllers.keys())))))
else:
dependencies = [
self.controllers[dep_name] for dep_name in deps
]
controller = kls(controller_name, dependencies)
if controller.initialize():
controller.start()
self.controllers[controller_name] = controller
self.waiting_meta_controllers = controllers_still_waiting[:]
def start_controller(self, req):
port_name = req.port_name
package_path = req.package_path
module_name = req.module_name
class_name = req.class_name
controller_name = req.controller_name
self.start_controller_lock.acquire()
if controller_name in self.controllers:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Controller [%s] already started. If you want to restart it, call restart.'
% controller_name)
try:
if module_name not in sys.modules:
# import if module not previously imported
package_module = __import__(package_path, globals(), locals(),
[module_name], -1)
else:
# reload module if previously imported
package_module = reload(sys.modules[package_path])
controller_module = getattr(package_module, module_name)
except ImportError, ie:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Cannot find controller module. Unable to start controller %s\n%s'
% (module_name, str(ie)))
except SyntaxError, se:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Syntax error in controller module. Unable to start controller %s\n%s'
% (module_name, str(se)))
except ImportError, ie:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Cannot find controller module. Unable to start controller %s\n%s'
% (module_name, str(ie)))
except SyntaxError, se:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Syntax error in controller module. Unable to start controller %s\n%s'
% (module_name, str(se)))
except Exception, e:
self.start_controller_lock.release()
return StartControllerResponse(
False,
'Unknown error has occured. Unable to start controller %s\n%s'
% (module_name, str(e)))
kls = getattr(controller_module, class_name)
if port_name == 'meta':
self.waiting_meta_controllers.append(
(controller_name, req.dependencies, kls))
self.check_deps()
self.start_controller_lock.release()
return StartControllerResponse(True, '')
if port_name != 'meta' and (port_name not in self.serial_proxies):
self.start_controller_lock.release()
return StartControllerResponse(
False,