dynamixels.launch 2.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051
  1. <launch>
  2. <param name="/use_sim_time" value="false" />
  3. <!-- Load the URDF/Xacro model of our robot -->
  4. <param name="robot_description" command="$(find xacro)/xacro.py '$(find rbx1_description)/urdf/turtlebot_with_head.xacro'" />
  5. <!-- Publish the robot state -->
  6. <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
  7. <param name="publish_frequency" value="20.0"/>
  8. </node>
  9. <!-- Start the Dynamixel low-level driver manager with parameters -->
  10. <node name="dynamixel_manager" pkg="dynamixel_controllers"
  11. type="controller_manager.py" required="true" output="screen">
  12. <rosparam>
  13. namespace: turtlebot_dynamixel_manager
  14. serial_ports:
  15. dynamixel_ax12:
  16. port_name: /dev/ttyUSB0
  17. baud_rate: 1000000
  18. min_motor_id: 1
  19. max_motor_id: 2
  20. update_rate: 20
  21. </rosparam>
  22. </node>
  23. <!-- Load the joint controller configuration from a YAML file -->
  24. <rosparam file="$(find rbx1_dynamixels)/config/dynamixel_params.yaml" command="load"/>
  25. <!-- Start the head pan and tilt controllers -->
  26. <node name="dynamixel_controller_spawner_ax12" pkg="dynamixel_controllers"
  27. type="controller_spawner.py"
  28. args="--manager=turtlebot_dynamixel_manager
  29. --port=dynamixel_ax12
  30. --type=simple
  31. head_pan_joint
  32. head_tilt_joint"
  33. output="screen" />
  34. <!-- Start the Dynamixel Joint States Publisher -->
  35. <node name="dynamixel_joint_states_publisher" pkg="rbx1_dynamixels" type="dynamixel_joint_state_publisher.py" output="screen" />
  36. <!-- Start all Dynamixels in the relaxed state -->
  37. <node pkg="rbx1_dynamixels" type="relax_all_servos.py" name="relax_all_servos" />
  38. <!-- We need a static transforms for the TurtleBot wheels -->
  39. <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0.13 0.015 0 0 0 /base_link /base_l_wheel_link 100" />
  40. <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 -0.13 0.015 0 0 0 /base_link /base_r_wheel_link 100" />
  41. <node pkg="tf" type="static_transform_publisher" name="odom_front_wheel_broadcaster" args="0.13 0 0 0 0 0 /base_link /front_wheel_link 100" />
  42. </launch>