search

arebgun / dynamixel_motor

ROS stack for interfacing with Robotis Dynamixel line of servo motors.
BSD 3-Clause "New" or "Revised" License
70 stars 169 forks source link

Service call failed during startup #10

Open pirobot opened 10 years ago

pirobot commented 10 years ago

I'm running dynamixel_motor from source (cloned today) under ROS Hydro and Ubuntu 12.04. I have two USB2Dynamixels, one with 6 AX-12 servos attached and one with 2 RX-24F servos attached.

When I run my launch file (which I will include below), I almost always get the following error during startup:

Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 67, in init self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 218, in start_controller if controller.initialize(): File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 76, in initialize self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.motor_id)) File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in __getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1294, in single_request response = h.getresponse(buffering=True) File "/usr/lib/python2.7/httplib.py", line 1018, in getresponse raise ResponseNotReady() ResponseNotReady [ERROR] [WallTime: 1386640566.278920] Error processing request: None [ERROR] [WallTime: 1386640566.279883] Service call failed: service [/pi_dynamixel_manager/dynamixel_rx24/start_controller] responded with an error: error processing request: raise CannotSendRequest() CannotSendRequest

The complete output is as follows:

... logging to /home/patrick/.ros/log/37dfaa00-613e-11e3-abb2-0022683b2a0b/roslaunch-pirobot-destop-16165.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

DEPRECATED IN HYDRO: The tag should be prepended with 'xacro' if that is the intended use of it, such as <xacro:include ...>. Use the following script to fix incorrect xacro includes: sed -i 's/<include/<xacro:include/g' find . -iname *.xacro DEPRECATED IN HYDRO: The tag should be prepended with 'xacro' if that is the intended use of it, such as <xacro:include ...>. Use the following script to fix incorrect xacro includes: sed -i 's/<include/<xacro:include/g' find . -iname *.xacro DEPRECATED IN HYDRO: The tag should be prepended with 'xacro' if that is the intended use of it, such as <xacro:include ...>. Use the following script to fix incorrect xacro includes: sed -i 's/<include/<xacro:include/g' find . -iname *.xacro DEPRECATED IN HYDRO: The tag should be prepended with 'xacro' if that is the intended use of it, such as <xacro:include ...>. Use the following script to fix incorrect xacro includes: sed -i 's/<include/<xacro:include/g' find . -iname *.xacro DEPRECATED IN HYDRO: The tag should be prepended with 'xacro' if that is the intended use of it, such as <xacro:include ...>. Use the following script to fix incorrect xacro includes: sed -i 's/<include/<xacro:include/g' find . -iname *.xacro started roslaunch server http://pirobot-destop:58500/

SUMMARY

PARAMETERS

NODES / diagnostic_aggregator (diagnostic_aggregator/aggregator_node) dynamixel_controller_spawner_ax12 (dynamixel_controllers/controller_spawner.py) dynamixel_controller_spawner_rx24 (dynamixel_controllers/controller_spawner.py) dynamixel_joint_states_publisher (rbx2_dynamixels/dynamixel_joint_state_publisher.py) dynamixel_manager (dynamixel_controllers/controller_manager.py) head_controller (dynamixel_controllers/controller_spawner.py) right_arm_controller (dynamixel_controllers/controller_spawner.py) rob_st_pub (robot_state_publisher/state_publisher) robot_monitor (rqt_robot_monitor/rqt_robot_monitor) world_base_broadcaster (tf/static_transform_publisher)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found process[rob_st_pub-1]: started with pid [16186] process[dynamixel_manager-2]: started with pid [16203] process[dynamixel_controller_spawner_rx24-3]: started with pid [16206] process[dynamixel_controller_spawner_ax12-4]: started with pid [16207] process[dynamixel_joint_states_publisher-5]: started with pid [16213] [INFO] [WallTime: 1386640666.753350] dynamixel_rx24 controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace... [INFO] [WallTime: 1386640666.918454] dynamixel_ax12 controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace... process[diagnostic_aggregator-6]: started with pid [16233] [INFO] [WallTime: 1386640667.016935] dynamixel_ax12: Pinging motor IDs 1 through 6... process[robot_monitor-7]: started with pid [16343] [INFO] [WallTime: 1386640667.210627] dynamixel_ax12: Found 6 motors - 6 AX-12 [1, 2, 3, 4, 5, 6], initialization complete. [INFO] [WallTime: 1386640667.225635] dynamixel_ax12 controller_spawner: All services are up, spawning controllers... [INFO] [WallTime: 1386640667.229688] dynamixel_rx24: Pinging motor IDs 21 through 22... Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 61, in init self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.267144] Error processing request: None [ERROR] [WallTime: 1386640667.274150] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in __init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 67, in init self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.290596] Error processing request: None [ERROR] [WallTime: 1386640667.292306] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in __init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 72, in init self.torque_limit = rospy.get_param(self.controller_namespace + '/joint_torque_limit', None) File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.314348] Error processing request: None [ERROR] [WallTime: 1386640667.317229] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in __init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 67, in init self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.338139] Error processing request: None [ERROR] [WallTime: 1386640667.338861] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in __init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 67, in init self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.__verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.347894] Error processing request: None [ERROR] [WallTime: 1386640667.348432] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: Traceback (most recent call last): File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 618, in _handle_request response = convert_return_to_response(self.handler(request), self.response_class) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 216, in start_controller controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 56, in __init JointController.init(self, dxl_io, controller_namespace, port_namespace) File "/home/patrick/Dropbox/Robotics/catkin_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 67, in init self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/client.py", line 452, in get_param return _param_server[param_name] #MasterProxy does all the magic for us File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/msproxy.py", line 115, in getitem code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) File "/usr/lib/python2.7/xmlrpclib.py", line 1224, in call return self.send(self.name, args) File "/usr/lib/python2.7/xmlrpclib.py", line 1578, in request verbose=self.verbose File "/usr/lib/python2.7/xmlrpclib.py", line 1264, in request return self.single_request(host, handler, request_body, verbose) File "/usr/lib/python2.7/xmlrpclib.py", line 1289, in single_request self.send_request(h, handler, request_body) File "/usr/lib/python2.7/xmlrpclib.py", line 1391, in send_request connection.putrequest("POST", handler, skip_accept_encoding=True) File "/usr/lib/python2.7/httplib.py", line 856, in putrequest raise CannotSendRequest() CannotSendRequest [ERROR] [WallTime: 1386640667.356495] Error processing request: None [ERROR] [WallTime: 1386640667.357251] Service call failed: service [/pi_dynamixel_manager/dynamixel_ax12/start_controller] responded with an error: error processing request: [INFO] [WallTime: 1386640667.393351] dynamixel_rx24: Found 2 motors - 2 RX-24 [21, 22], initialization complete. Unhandled exception in thread started by sys.excepthook is missing lost sys.stderr process[world_base_broadcaster-8]: started with pid [16457] [dynamixel_controller_spawner_ax12-4] process has finished cleanly log file: /home/patrick/.ros/log/37dfaa00-613e-11e3-abb2-0022683b2a0b/dynamixel_controller_spawnerax12-4.log [INFO] [WallTime: 1386640667.673401] dynamixel_rx24 controller_spawner: All services are up, spawning controllers... [INFO] [WallTime: 1386640667.712129] Controller right_arm_shoulder_pan_controller successfully started. [INFO] [WallTime: 1386640667.743883] Controller right_arm_shoulder_lift_controller successfully started. process[right_arm_controller-9]: started with pid [16555] [dynamixel_controller_spawner_rx24-3] process has finished cleanly log file: /home/patrick/.ros/log/37dfaa00-613e-11e3-abb2-0022683b2a0b/dynamixel_controller_spawnerrx24-3.log process[head_controller-10]: started with pid [16587] [INFO] [WallTime: 1386640668.295675] meta controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace... [INFO] [WallTime: 1386640668.299565] meta controller_spawner: All services are up, spawning controllers... [WARN] [WallTime: 1386640668.328879] [right_arm_controller] not all dependencies started, still waiting for ['right_arm_wrist_flex_controller', 'right_arm_elbow_flex_controller', 'right_arm_forearm_flex_controller', 'right_arm_shoulder_roll_controller']... [INFO] [WallTime: 1386640668.329700] [INFO] [WallTime: 1386640668.471919] meta controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace... [INFO] [WallTime: 1386640668.476248] meta controller_spawner: All services are up, spawning controllers... [WARN] [WallTime: 1386640668.480420] [right_arm_controller] not all dependencies started, still waiting for ['right_arm_wrist_flex_controller', 'right_arm_elbow_flex_controller', 'right_arm_forearm_flex_controller', 'right_arm_shoulder_roll_controller']... [WARN] [WallTime: 1386640668.480763] [head_controller] not all dependencies started, still waiting for ['head_pan_controller', 'head_tilt_controller']... [INFO] [WallTime: 1386640668.481162] [right_arm_controller-9] process has finished cleanly log file: /home/patrick/.ros/log/37dfaa00-613e-11e3-abb2-0022683b2a0b/right_armcontroller-9.log [head_controller-10] process has finished cleanly log file: /home/patrick/.ros/log/37dfaa00-613e-11e3-abb2-0022683b2a0b/headcontroller-10.log

And my launch file looks like this:

<launch>
   <arg name="dynamixel_namespace" value="/" />

   <!-- Load the URDF/Xacro model of our robot -->
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find rbx2_description)/urdf/pedestal_pi/pedestal_pi_no_gripper.xacro'" />

   <rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_dynamixels)/config/pedestal_pi_dynamixel_no_gripper.yaml" command="load"/>

   <!-- Publish the robot state -->
     <node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
       <param name="publish_frequency" value="30.0"/>
   </node>

   <!-- Start the Dynamixel low-level driver manager with parameters -->
   <node ns="$(arg dynamixel_namespace)" name="dynamixel_manager" pkg="dynamixel_controllers" output="screen" 
      type="controller_manager.py" required="true">
      <rosparam>
         namespace: pi_dynamixel_manager
         diagnostics_rate: 1
         serial_ports:
            dynamixel_ax12:
               port_name: "/dev/dynamixel_ax12"
               baud_rate: 1000000
               min_motor_id: 1
               max_motor_id: 6
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
            dynamixel_rx24:
               port_name: "/dev/dynamixel_rx24"
               baud_rate: 1000000
               min_motor_id: 21
               max_motor_id: 22
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
      </rosparam>
   </node>   

   <!-- Start all Pi Robot RX24 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_rx24" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_rx24
              --type=simple
        right_arm_shoulder_pan_controller
        right_arm_shoulder_lift_controller"
        />

   <!-- Start all Pi Robot AX12 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_ax12" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_ax12
              --type=simple
        right_arm_shoulder_roll_controller
        right_arm_elbow_flex_controller
        right_arm_forearm_flex_controller
        right_arm_wrist_flex_controller
        head_pan_controller
        head_tilt_controller"
        />

   <!-- Start the Dynamixel Joint States Publisher -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" name="dynamixel_joint_states_publisher" type="dynamixel_joint_state_publisher.py">
      <param name="rate" value="20" />
   </node>

   <!-- Load diagnostics -->
   <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
      <rosparam command="load" file="$(find rbx2_bringup)/config/diagnostics.yaml" />
   </node>

   <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="robot_monitor" />

   <!-- Start all servos in a relaxed state -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" type="relax_all_servos.py" name="relax_all_servos" />

   <!-- Publish a static transform between the robot base and the world frame -->
   <node pkg="tf" type="static_transform_publisher" name="world_base_broadcaster" args="0 0 0 0 0 0 /world /base_link 100" />

  <!-- Start the right arm trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="right_arm_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      right_arm_controller
      right_arm_shoulder_pan_controller
      right_arm_shoulder_lift_controller
      right_arm_shoulder_roll_controller
      right_arm_elbow_flex_controller
      right_arm_forearm_flex_controller
      right_arm_wrist_flex_controller"
     />

  <!-- Start the head trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="head_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      head_controller
      head_pan_controller
      head_tilt_controller"
     />

</launch>
arebgun commented 10 years ago

Weird, doesn't look like this is specific to the dynamixel_motor from the error it throws - CannotSendRequest. Does it help if you start roscore and controller_manager nodes separately? i am not sure if that matters though.

pirobot commented 10 years ago

I always have a roscore running first so I don't think that could be it. I can get rid of the error if I split my dynamixel launch file into three separate launch files and run them about 10 seconds apart (I do this in a shell script). The three launch files are as follows:

dynamixel_a.launch:

<launch>
   <param name="/use_sim_time" value="false" />

   <arg name="dynamixel_namespace" value="/" />

   <!-- Load the URDF/Xacro model of our robot -->
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find rbx2_description)/urdf/pedestal_pi/pedestal_pi_no_gripper.xacro'" />

   <rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_dynamixels)/config/pedestal_pi_dynamixel_no_gripper.yaml" command="load"/>

   <!-- Publish the robot state -->
     <node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
       <param name="publish_frequency" value="30.0"/>
   </node>

   <!-- Start the Dynamixel low-level driver manager with parameters -->
   <node ns="$(arg dynamixel_namespace)" name="dynamixel_manager" pkg="dynamixel_controllers" output="screen" 
      type="controller_manager.py" required="true">
      <rosparam>
         namespace: pi_dynamixel_manager
         diagnostics_rate: 1
         serial_ports:
            dynamixel_ax12:
               port_name: "/dev/dynamixel_ax12"
               baud_rate: 1000000
               min_motor_id: 1
               max_motor_id: 6
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
            dynamixel_rx24:
               port_name: "/dev/dynamixel_rx24"
               baud_rate: 1000000
               min_motor_id: 21
               max_motor_id: 22
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
      </rosparam>
   </node>      
</launch>

dynamixel_b.launch:

<launch>
   <arg name="dynamixel_namespace" value="/" />

   <!-- Start all Pi Robot RX24 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_rx24" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_rx24
              --type=simple
        right_arm_shoulder_pan_controller
        right_arm_shoulder_lift_controller"
        />

   <!-- Start all Pi Robot AX12 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_ax12" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_ax12
              --type=simple
        right_arm_shoulder_roll_controller
        right_arm_elbow_flex_controller
        right_arm_forearm_flex_controller
        right_arm_wrist_flex_controller
        head_pan_controller
        head_tilt_controller"
        />

   <!-- Start the Dynamixel Joint States Publisher -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" name="dynamixel_joint_states_publisher" type="dynamixel_joint_state_publisher.py">
      <param name="rate" value="20" />
   </node>

   <!-- Load diagnostics -->
   <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
      <rosparam command="load" file="$(find rbx2_bringup)/config/diagnostics.yaml" />
   </node>

   <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="robot_monitor" />

   <!-- Publish a static transform between the robot base and the world frame -->
   <node pkg="tf" type="static_transform_publisher" name="world_base_broadcaster" args="0 0 0 0 0 0 /world /base_link 100" />

</launch>

dynamixel_c.launch:

<launch>
  <arg name="dynamixel_namespace" value="/" />

  <!-- Start the right arm trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="right_arm_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      right_arm_controller
      right_arm_shoulder_pan_controller
      right_arm_shoulder_lift_controller
      right_arm_shoulder_roll_controller
      right_arm_elbow_flex_controller
      right_arm_forearm_flex_controller
      right_arm_wrist_flex_controller"
     />

  <!-- Start the head trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="head_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      head_controller
      head_pan_controller
      head_tilt_controller"
     />

</launch>
arebgun commented 10 years ago

So it might be that the manager node isn't ready yet? If you combine b and c launch files, does it fail?

pirobot commented 10 years ago

Interesting--it does not fail if I combine b and c launch files. But as soon as I try then all together (a, b and c), I get the error again. Is there a service proxy somewhere that should be doing a wait_for_service() before proceeding?

arebgun commented 10 years ago

That's what it sounds like, I will look into it later today.

arebgun commented 10 years ago

Patrick,

Looking at the error messages it looks like it can't find certain parameters on the parameter server, e.g.:

self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name') ...

So it's not the service servers that are not there, there's some kind of problem with parameters being populated before they are looked up. Weird, I never had this problem.

Could you post your dynamixel configuration yamls?

Thanks!

pirobot commented 10 years ago

Yeah, every once in awhile I got a message that made me wonder if it was a parameter server thing.

Here is my config file:

joints: ['head_pan_joint', 'head_tilt_joint', 'right_arm_shoulder_pan_joint', 'right_arm_shoulder_lift_joint', 'right_arm_shoulder_roll_joint', 'right_arm_elbow_flex_joint', 'right_arm_forearm_flex_joint', 'right_arm_wrist_flex_joint']

head_pan_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: head_pan_joint
    joint_speed: 1.0
    motor:
        id: 1
        init: 512
        min: 0
        max: 1024

head_tilt_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: head_tilt_joint
    joint_speed: 1.0
    motor:
        id: 2
        init: 512
        min: 300
        max: 800

right_arm_shoulder_roll_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_shoulder_roll_joint
    joint_speed: 1.0
    motor:
        id: 3
        init: 512
        # flipped
        min: 1023
        max: 0

right_arm_elbow_flex_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_elbow_flex_joint
    joint_speed: 1.0
    motor:
        id: 4
        init: 512
        min: 0
        max: 1023

right_arm_forearm_flex_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_forearm_flex_joint
    joint_speed: 1.0
    motor:
        id: 5
        init: 512
        min: 0
        max: 1023

right_arm_wrist_flex_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_wrist_flex_joint
    joint_speed: 1.0
    motor:
        id: 6
        init: 512
        min: 0
        max: 1023

right_arm_shoulder_pan_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_shoulder_pan_joint
    joint_speed: 1.0
    motor:
        id: 21
        init: 512
        min: 0
        max: 1023

right_arm_shoulder_lift_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: right_arm_shoulder_lift_joint
    joint_speed: 1.0
    motor:
        id: 22
        init: 220
        # flipped
        min: 1023
        max: 0

right_arm_controller:
   controller:
       package: dynamixel_controllers
       module: joint_trajectory_action_controller
       type: JointTrajectoryActionController
   joint_trajectory_action_node:
       min_velocity: 0.25
       constraints:
           goal_time: 0.1

head_controller:
   controller:
       package: dynamixel_controllers
       module: joint_trajectory_action_controller
       type: JointTrajectoryActionController
   joint_trajectory_action_node:
       min_velocity: 0.25
       constraints:
           goal_time: 0.1
arebgun commented 10 years ago

The motor config looks good to me. Do you get this problem if you put the config on the parameter server first and run everything else in your other launch file. I am a bit at a loss here. I suppose there's no way to wait for a parameter to show up in the server, is there (besides rolling our own, that is).

OkkeHendriks commented 10 years ago

Any progress/new insights on the issue? I am having the exact same problem. Also when I try my launch file a couple of times, sometimes it does start without errors.

pirobot commented 10 years ago

I just got back to this myself. On a hunch, I changed the order of the nodes in my launch file to load the multi-joint trajectory controllers (e.g. arm controller) before the individual joint controllers and it now seems to work every time, even after a clean restart of roscore. Here is my launch file for reference. My arm has two RX-24F servos and 4 AX-12 servos and my pan and tilt head has two AX-12 servos. I am using two USB2Dynamixel controllers.

<launch>
   <arg name="dynamixel_namespace" value="/" />

   <!-- Load the URDF/Xacro model of our robot -->
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find rbx2_description)/urdf/pedestal_pi/pedestal_pi_no_gripper.xacro'" />

   <rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_dynamixels)/config/pedestal_pi_dynamixel_no_gripper.yaml" command="load"/>

   <!-- Publish the robot state -->
     <node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
       <param name="publish_frequency" value="30.0"/>
   </node>

   <!-- Start the Dynamixel low-level driver manager with parameters -->
   <node ns="$(arg dynamixel_namespace)" name="dynamixel_manager" pkg="dynamixel_controllers" output="screen" 
      type="controller_manager.py" required="true">
      <rosparam>
         namespace: pi_dynamixel_manager
         diagnostics_rate: 1
         serial_ports:
            dynamixel_ax12:
               port_name: "/dev/dynamixel_ax12"
               baud_rate: 1000000
               min_motor_id: 1
               max_motor_id: 6
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
            dynamixel_rx24:
               port_name: "/dev/dynamixel_rx24"
               baud_rate: 1000000
               min_motor_id: 21
               max_motor_id: 22
               update_rate: 20
               diagnostics:
                  error_level_temp: 65
                  warn_level_temp: 60
      </rosparam>
   </node>

  <!-- Start the right arm trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="right_arm_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      right_arm_controller
      right_arm_shoulder_pan_joint
      right_arm_shoulder_lift_joint
      right_arm_shoulder_roll_joint
      right_arm_elbow_flex_joint
      right_arm_forearm_flex_joint
      right_arm_wrist_flex_joint"
     />

  <!-- Start the head trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="head_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      head_controller
      head_pan_joint
      head_tilt_joint"
     />

   <!-- Start all Pi Robot RX24 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_rx24" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_rx24
              --type=simple
        right_arm_shoulder_pan_joint
        right_arm_shoulder_lift_joint"
        />

   <!-- Start all Pi Robot AX12 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_ax12" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_ax12
              --type=simple
        right_arm_shoulder_roll_joint
        right_arm_elbow_flex_joint
        right_arm_forearm_flex_joint
        right_arm_wrist_flex_joint
        head_pan_joint
        head_tilt_joint"
        />

   <!-- Start the Dynamixel Joint States Publisher -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" name="dynamixel_joint_states_publisher" type="dynamixel_joint_state_publisher.py">
      <param name="rate" value="20" />
   </node>

   <!-- Load diagnostics -->
   <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
      <rosparam command="load" file="$(find rbx2_bringup)/config/diagnostics.yaml" />
   </node>

   <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="robot_monitor" />

   <!-- Start all servos in a relaxed state -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" type="relax_all_servos.py" name="relax_all_servos" />

   <!-- Publish a static transform between the robot base and the world frame -->
   <node pkg="tf" type="static_transform_publisher" name="world_base_broadcaster" args="0 0 0 0 0 0 /world /base_link 100" />

</launch>
OkkeHendriks commented 10 years ago

Too bad, it does not work for me. I am still getting the same error.

arebgun commented 10 years ago

What kind of configuration do you have? What kind of joints, any dual joints, what controllers do you start? Does the error go away if you start a subset of controllers? I am trying to figure out if there's some ordering issue, when we try to read the parameter during startup which is not yet available.

OkkeHendriks commented 10 years ago

The following is the launch file that is giving me errors:

<launch>
<!-- Start the Dynamixel servo manager to control wubble arm -->
<node name="dynamixel_manager_arm" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
    <rosparam>
        namespace: rose_arm_manager
        serial_ports:
            rightArm:
                port_name: "/dev/ttyUSB0"
                baud_rate: 1000000
                min_motor_id: 0
                max_motor_id: 7
                update_rate: 20
    </rosparam>
</node>

<!-- Start joint trajectory action controller -->
<rosparam file="$(find rose20_platform)/launch/arm_controllers/trajectory_controllers.yaml" command="load"/>
<node name="trajectory_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
      args="--manager=rose_arm_manager
            --type=meta
            right_arm_trajectory_controller
            arm_joint_box_1
            arm_joint_cyl_1
            arm_joint_cyl_2
            arm_joint_cyl_3
            arm_joint_cyl_4
            arm_joint_cyl_5
            gripper_joint_1"
      output="screen"/>

<!-- Start upper and lower arm joint controllers -->
<rosparam file="$(find rose20_platform)/launch/arm_controllers/joints.yaml" command="load"/>
<node name="upper_arm_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
      args="--manager=rose_arm_manager
            --port=rightArm
            --type=simple
            arm_joint_box_1
            arm_joint_cyl_1
            arm_joint_cyl_2
            arm_joint_cyl_3
            arm_joint_cyl_4
            arm_joint_cyl_5
            gripper_joint_1"
      output="screen"/>

<!-- Start gripper joint controllers -->
<rosparam file="$(find rose20_platform)/launch/arm_controllers/joints.yaml" command="load"/>
<node name="gripper_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
      args="--manager=rose_arm_manager
            --port=rightArm
            --type=simple
           gripper_joint_2"
      output="screen"/>
</launch>

I now noticed that I have 2 controller_spawner nodes (upper_arm_controller_spawner , gripper_controller_spawner). When removing the following part of the launch file:

<!-- Start gripper joint controllers -->
<rosparam file="$(find rose20_platform)/launch/arm_controllers/joints.yaml" command="load"/>
<node name="gripper_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
      args="--manager=rose_arm_manager
            --port=rightArm
            --type=simple
           gripper_joint_2"
      output="screen"/>
</launch>

And adding that joint to the 'upper_arm_controller_spawner' I do not get the errors any longer.

arebgun commented 10 years ago

So the problem seemed to be two spawners working on the same manager and port combination trying to lunch different sets of controllers? Patrick's config files don't have that though: he launches two spawners for two separate ports.

OkkeHendriks commented 10 years ago

Yes, so maybe it has something to do with two nodes doing a service request at the same time to the same manager, in my case, rose_arm_manager?

arebgun commented 10 years ago

Ok, I will take a look at that part of the code later today, probably the function that services the requests is not thread safe or something like that.

arebgun commented 10 years ago

Folks, I just modified the controller manager, could you check if that changes the behavior?

pirobot commented 10 years ago

Hi Anton--that does not seem to have fixed the issue. It still takes at least two or three launches before all the joint controllers are up. Before then I get a warning like this:

[WARN] [WallTime: 1392346412.990366] [right_arm_controller] not all dependencies started, still waiting for ['right_arm_wrist_flex_joint', 'right_arm_shoulder_lift_joint', 'right_arm_shoulder_pan_joint', 'right_arm_elbow_flex_joint', 'right_arm_forearm_flex_joint']...

And rqt_monitor indicates that not all the joint controllers are coming up as shown in the following screen shots taken from a couple of different launches:

image

image

arebgun commented 10 years ago

Patrick, do you get the same error as before or does it just wait for the controllers to come up and they never do?

pirobot commented 10 years ago

Good question. You are right--the error has gone away but the controllers just never come up.

arebgun commented 10 years ago

Patrick, could you check again, I fixed what I think might be a related problem.

pirobot commented 10 years ago

I pulled the update, restarted roscore, and re-ran my launch file but still some of the joint controllers are not coming up.

I'm heading offline until tomorrow but can continue testing then.

OkkeHendriks commented 10 years ago

For me this works, also with the original, two controller spawners on the same manager/port, configuration. Thank you.

OkkeHendriks commented 10 years ago

Scratch that! I guess it just randomly worked ten times after each other, now i am back to the same error again.

arebgun commented 10 years ago

Yes, I think there's a problem with meta controller dependencies initialization, I need to look at it again.

pirobot commented 10 years ago

So here's an observation that might help. If I use the latest Debian dynamixel_motor packages for ROS Hydro and Ubuntu 12.04 (Precise) my startup launch file works every time. So perhaps there is a clue hidden in the state of the repository just before you created the last release update? I'm including my launch file below because it orders the nodes in a slightly backwards manner (I start the arm and head trajectory controllers before I start the individual joint controllers).

<launch>
   <arg name="dynamixel_namespace" value="/" />

   <!-- Load the URDF/Xacro model of our robot -->
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find rbx2_description)/urdf/pedestal_pi/pedestal_pi_no_gripper.xacro'" />

   <rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_dynamixels)/config/pedestal_pi_no_gripper.yaml" command="load"/>

   <!-- Publish the robot state -->
     <node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
       <param name="publish_frequency" value="30.0"/>
   </node>

   <!-- Start the Dynamixel low-level driver manager with parameters -->
   <node ns="$(arg dynamixel_namespace)" name="dynamixel_manager" pkg="dynamixel_controllers" output="screen" 
      type="controller_manager.py" required="true">
      <rosparam>
         namespace: pi_dynamixel_manager
         diagnostics_rate: 1
         serial_ports:
            dynamixel_ax12:
               port_name: "/dev/dynamixel_ax12"
               baud_rate: 1000000
               min_motor_id: 1
               max_motor_id: 6
               update_rate: 20
               diagnostics:
                  error_level_temp: 55
                  warn_level_temp: 45
            dynamixel_rx24:
               port_name: "/dev/dynamixel_rx24"
               baud_rate: 1000000
               min_motor_id: 21
               max_motor_id: 22
               update_rate: 20
               diagnostics:
                  error_level_temp: 55
                  warn_level_temp: 45
      </rosparam>
   </node>

  <!-- Start the right arm trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="right_arm_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      right_arm_controller
      right_arm_shoulder_pan_joint
      right_arm_shoulder_lift_joint
      right_arm_shoulder_roll_joint
      right_arm_elbow_flex_joint
      right_arm_forearm_flex_joint
      right_arm_wrist_flex_joint"
     />

  <!-- Start the head trajectory controller -->
  <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="head_controller" type="controller_spawner.py" output="screen" 
      args="--manager=pi_dynamixel_manager
            --type=meta
      head_controller
      head_pan_joint
      head_tilt_joint"
     />

   <!-- Start all Pi Robot RX24 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_rx24" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_rx24
              --type=simple
        right_arm_shoulder_pan_joint
        right_arm_shoulder_lift_joint"
        />

   <!-- Start all Pi Robot AX12 joint controllers -->
   <node ns="$(arg dynamixel_namespace)" pkg="dynamixel_controllers" name="dynamixel_controller_spawner_ax12" output="screen" 
   type="controller_spawner.py"
        args="--manager=pi_dynamixel_manager
              --port=dynamixel_ax12
              --type=simple
        right_arm_shoulder_roll_joint
        right_arm_elbow_flex_joint
        right_arm_forearm_flex_joint
        right_arm_wrist_flex_joint
        head_pan_joint
        head_tilt_joint"
        />   

   <!-- Start the Dynamixel Joint States Publisher -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" name="dynamixel_joint_states_publisher" type="dynamixel_joint_state_publisher.py">
      <param name="rate" value="20" />
   </node>

   <!-- Load diagnostics -->
   <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
      <rosparam command="load" file="$(find rbx2_bringup)/config/diagnostics.yaml" />
   </node>

   <node pkg="rqt_robot_monitor" type="rqt_robot_monitor" name="robot_monitor" />

   <!-- Start all servos in a relaxed state -->
   <node ns="$(arg dynamixel_namespace)" pkg="rbx2_dynamixels" type="relax_all_servos.py" name="relax_all_servos" />

   <!-- Publish a static transform between the robot base and the world frame -->
   <node pkg="tf" type="static_transform_publisher" name="world_base_broadcaster" args="0 0 0 0 0 0 /world /base_link 100" />

</launch>