dynamixel_motor launch file timing problems
Hello,
I've been using the most excellent dynamixelmotor stack for a couple of years now. Recently (Ubuntu 12.04, Groovy debs), there seems to be an issue with the timing of when the dynamixelmotor controller nodes are launched. Sometimes all the nodes fire up and connect to each other. At other times it appears that not all the joint controllers are loaded. If I split the launch file into three separate launch files and run them in sequence, I can make it work 100% of the time. But if I glue them together as one launch file, at least one joint usually does not load.
I will post my launch file below but first here is a typical error output when something goes wrong:
[INFO] [WallTime: 1374805698.373428] dynamixel_ax12: Pinging motor IDs 1 through 6...
process[robot_monitor-6]: started with pid [25323]
[INFO] [WallTime: 1374805698.651452] dynamixel_ax12: Found 6 motors - 6 AX-12 [1, 2, 3, 4, 5, 6], initialization complete.
[INFO] [WallTime: 1374805698.662196] dynamixel_rx24: Pinging motor IDs 21 through 22...
[INFO] [WallTime: 1374805698.741967] dynamixel_rx24 controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace...
[INFO] [WallTime: 1374805698.775654] dynamixel_rx24: Found 2 motors - 2 RX-24 [21, 22], initialization complete.
process[world_base_broadcaster-7]: started with pid [25476]
[INFO] [WallTime: 1374805698.946841] dynamixel_ax12 controller_spawner: waiting for controller_manager pi_dynamixel_manager to startup in global namespace...
[INFO] [WallTime: 1374805698.952901] dynamixel_ax12 controller_spawner: All services are up, spawning controllers...
[INFO] [WallTime: 1374805698.994594] Controller right_arm_shoulder_roll_controller successfully started.
process[right_arm_controller-8]: started with pid [25559]
[INFO] [WallTime: 1374805699.042321] Controller right_arm_elbow_flex_controller successfully started.
[INFO] [WallTime: 1374805699.055872] dynamixel_rx24 controller_spawner: All services are up, spawning controllers...
Traceback (most recent call last):
File "/opt/ros/groovy/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 619, in _handle_request
response = convert_return_to_response(self.handler(request), self.response_class)
File "/opt/ros/groovy/stacks/dynamixel_motor/dynamixel_controllers/nodes/controller_manager.py", line 222, in start_controller
controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name)
File "/opt/ros/groovy/stacks/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py", line 59, in __init__
JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
File "/opt/ros/groovy/stacks/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py", line 70, in __init__
self.joint_name = rospy.get_param(self.controller_namespace + '/joint_name')
File "/opt/ros/groovy/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/groovy/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()
My hardware setup includes two USB2Dynamixel controllers, one attached to six AX-12 servos and the other to two RX-24F servos.
Here now is my launch file all in one piece. Below that I list the three separate launch files that work if I run them one at a time:
<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_no_gripper.xacro'" />
<rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_bringup)/config/right_arm_dynamixel_params.yaml" command="load"/>
<!-- Publish the robot state -->
<node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" value="50.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="robot_monitor" type="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.66 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>
And here are the three separate launch files that always work when launched in sequence:
dynamixels_a.launch
<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_no_gripper.xacro'" />
<rosparam ns="$(arg dynamixel_namespace)" file="$(find rbx2_bringup)/config/right_arm_dynamixel_params.yaml" command="load"/>
<!-- Publish the robot state -->
<node name="rob_st_pub" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" value="50.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>
dynamixels_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="robot_monitor" type="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.66 0 0 0 /world /base_link 100" />
</launch>
dynamixels_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>
Asked by Pi Robot on 2013-07-25 16:34:37 UTC
Comments
This is weird, I have never had problems with launch order. The error message doesn't look like launch order problem though.
Asked by arebgun on 2013-08-02 16:26:40 UTC
@arebgun - any idea what the error means or what might cause it? I never get it if I split up the launch files as noted in my question. I was wondering if maybe there needs to be a "wait_for_message" or "wait_for_service" call somewhere in the code that starts the controllers?
Asked by Pi Robot on 2013-08-04 04:47:16 UTC