|
| 1 | +Launch Files in MoveIt |
| 2 | +====================== |
| 3 | + |
| 4 | +Many of the MoveIt tutorials, as well as MoveIt packages you will encounter in the wild, use ROS 2 launch files. |
| 5 | + |
| 6 | +This tutorial walks through a typical Python launch file that sets up a working MoveIt example. |
| 7 | +We will do this by going through our :codedir:`Getting Started tutorial launch file <tutorials/quickstart_in_rviz/launch/demo.launch.py>` in detail. |
| 8 | + |
| 9 | +If you are unfamiliar with launch files in general, refer first to `the ROS 2 documentation <https://docs.ros.org/en/rolling/Tutorials/Intermediate/Launch/Creating-Launch-Files.html>`_. |
| 10 | + |
| 11 | +Loading the MoveIt Configuration |
| 12 | +-------------------------------- |
| 13 | + |
| 14 | +MoveIt requires several configuration parameters to include the robot description and semantic description files (:ref:`URDF and SRDF`), motion planning and kinematics plugins, trajectory execution, and more. |
| 15 | +These parameters are usually contained in a :ref:`MoveIt Configuration` package. |
| 16 | + |
| 17 | +A handy way to refer to a MoveIt configuration package is to use the ``MoveItConfigsBuilder`` utility in your Python launch files as follows: |
| 18 | + |
| 19 | +.. code-block:: python |
| 20 | +
|
| 21 | + from moveit_configs_utils import MoveItConfigsBuilder |
| 22 | +
|
| 23 | + # Define xacro mappings for the robot description file |
| 24 | + launch_arguments = { |
| 25 | + "robot_ip": "xxx.yyy.zzz.www", |
| 26 | + "use_fake_hardware": "true", |
| 27 | + "gripper": "robotiq_2f_85", |
| 28 | + "dof": "7", |
| 29 | + } |
| 30 | +
|
| 31 | + # Load the robot configuration |
| 32 | + moveit_config = ( |
| 33 | + MoveItConfigsBuilder( |
| 34 | + "gen3", package_name="kinova_gen3_7dof_robotiq_2f_85_moveit_config" |
| 35 | + ) |
| 36 | + .robot_description(mappings=launch_arguments) |
| 37 | + .trajectory_execution(file_path="config/moveit_controllers.yaml") |
| 38 | + .planning_scene_monitor( |
| 39 | + publish_robot_description=True, publish_robot_description_semantic=True |
| 40 | + ) |
| 41 | + .planning_pipelines( |
| 42 | + pipelines=["ompl", "stomp", "pilz_industrial_motion_planner"] |
| 43 | + ) |
| 44 | + .to_moveit_configs() |
| 45 | + ) |
| 46 | +
|
| 47 | +Launching Move Group |
| 48 | +-------------------- |
| 49 | + |
| 50 | +Once all the MoveIt configuration parameters have been loaded, you can launch the :ref:`Move Group Interface` using the entire set of loaded MoveIt parameters. |
| 51 | + |
| 52 | +.. code-block:: python |
| 53 | +
|
| 54 | + from launch_ros.actions import Node |
| 55 | +
|
| 56 | + run_move_group_node = Node( |
| 57 | + package="moveit_ros_move_group", |
| 58 | + executable="move_group", |
| 59 | + output="screen", |
| 60 | + parameters=[moveit_config.to_dict()], |
| 61 | + ) |
| 62 | +
|
| 63 | +Visualizing with RViz |
| 64 | +--------------------- |
| 65 | + |
| 66 | +As discussed in the :ref:`Quickstart in RViz` tutorial, you can visualize your robot model and perform motion planning tasks using RViz. |
| 67 | + |
| 68 | +The following code uses a launch argument to receive an RViz configuration file name, packages it up as a relative path to a known package directory, and specifies it as an argument when launching the RViz executable. |
| 69 | + |
| 70 | +.. code-block:: python |
| 71 | +
|
| 72 | + from launch.actions import DeclareLaunchArgument |
| 73 | + from launch.substitutions import LaunchConfiguration, PathJoinSubstitution |
| 74 | + from launch_ros.substitutions import FindPackageShare |
| 75 | +
|
| 76 | + # Get the path to the RViz configuration file |
| 77 | + rviz_config_arg = DeclareLaunchArgument( |
| 78 | + "rviz_config", |
| 79 | + default_value="kinova_moveit_config_demo.rviz", |
| 80 | + description="RViz configuration file", |
| 81 | + ) |
| 82 | + rviz_base = LaunchConfiguration("rviz_config") |
| 83 | + rviz_config = PathJoinSubstitution( |
| 84 | + [FindPackageShare("moveit2_tutorials"), "launch", rviz_base] |
| 85 | + ) |
| 86 | +
|
| 87 | + # Launch RViz |
| 88 | + rviz_node = Node( |
| 89 | + package="rviz2", |
| 90 | + executable="rviz2", |
| 91 | + name="rviz2", |
| 92 | + output="log", |
| 93 | + arguments=["-d", rviz_config], |
| 94 | + parameters=[ |
| 95 | + moveit_config.robot_description, |
| 96 | + moveit_config.robot_description_semantic, |
| 97 | + moveit_config.robot_description_kinematics, |
| 98 | + moveit_config.planning_pipelines, |
| 99 | + moveit_config.joint_limits, |
| 100 | + ], |
| 101 | + ) |
| 102 | +
|
| 103 | +Publishing Transforms to ``tf2`` |
| 104 | +-------------------------------- |
| 105 | + |
| 106 | +Many tools in the ROS ecosystem use the `tf2 <https://docs.ros.org/en/rolling/Concepts/Intermediate/About-Tf2.html>`_ library to represent coordinate transforms, which are an important part of motion planning with MoveIt. |
| 107 | + |
| 108 | +As such, our launch file includes nodes that publish both fixed (static) and dynamic names to ``tf2``. |
| 109 | +In our case, we need: |
| 110 | + |
| 111 | +* A static transform between the ``world`` frame and the base frame of our robot description, ``base_link``. |
| 112 | +* A `robot state publisher <https://github.com/ros/robot_state_publisher>`_ node that listens to the robot's joint states, calculates frame transforms using the robot's URDF model, and publishes them to ``tf2``. |
| 113 | + |
| 114 | +.. code-block:: python |
| 115 | +
|
| 116 | + # Static TF |
| 117 | + static_tf = Node( |
| 118 | + package="tf2_ros", |
| 119 | + executable="static_transform_publisher", |
| 120 | + name="static_transform_publisher", |
| 121 | + output="log", |
| 122 | + arguments=["--frame-id", "world", "--child-frame-id", "base_link"], |
| 123 | + ) |
| 124 | +
|
| 125 | + # Publish TF |
| 126 | + robot_state_publisher = Node( |
| 127 | + package="robot_state_publisher", |
| 128 | + executable="robot_state_publisher", |
| 129 | + name="robot_state_publisher", |
| 130 | + output="both", |
| 131 | + parameters=[moveit_config.robot_description], |
| 132 | + ) |
| 133 | +
|
| 134 | +Setting up ``ros2_control`` for trajectory execution |
| 135 | +---------------------------------------------------- |
| 136 | + |
| 137 | +MoveIt normally generates joint trajectories that can then be executed by sending them to a robot with a controller capable of executing these trajectories. |
| 138 | +Most commonly, we connect to the `ros2_control <https://control.ros.org/master/index.html>`_ library to achieve this. |
| 139 | + |
| 140 | +While ``ros2_control`` allows you to connect to real robot hardware, or robots in a physics-based simulator like Gazebo or NVIDIA Isaac Sim, it also exposes a `mock components <https://control.ros.org/master/doc/ros2_control/hardware_interface/doc/mock_components_userdoc.html>`_ capability for simple, idealized simulations. |
| 141 | +In our example, this is configured at the URDF level using the ``use_fake_hardware`` xacro parameter defined earlier on. |
| 142 | +The key idea is that regardless of which hardware (simulated or real) is launched, the ``ros2_control`` launch remains the same. |
| 143 | + |
| 144 | +Starting ``ros2_control`` involves launching a controller manager node, and then spawning a list of controllers necessary for trajectory execution. |
| 145 | +In our example, we have: |
| 146 | + |
| 147 | +* A joint state broadcaster, which publishes the joint states necessary for the robot state publisher to send frames to ``tf2``. |
| 148 | +* A joint trajectory controller for the arm actuators. |
| 149 | +* A gripper action controller for the parallel-jaw gripper. |
| 150 | + |
| 151 | +.. code-block:: python |
| 152 | +
|
| 153 | + ros2_controllers_path = os.path.join( |
| 154 | + get_package_share_directory("kinova_gen3_7dof_robotiq_2f_85_moveit_config"), |
| 155 | + "config", |
| 156 | + "ros2_controllers.yaml", |
| 157 | + ) |
| 158 | + ros2_control_node = Node( |
| 159 | + package="controller_manager", |
| 160 | + executable="ros2_control_node", |
| 161 | + parameters=[moveit_config.robot_description, ros2_controllers_path], |
| 162 | + output="both", |
| 163 | + ) |
| 164 | +
|
| 165 | + joint_state_broadcaster_spawner = Node( |
| 166 | + package="controller_manager", |
| 167 | + executable="spawner", |
| 168 | + arguments=[ |
| 169 | + "joint_state_broadcaster", |
| 170 | + "--controller-manager", |
| 171 | + "/controller_manager", |
| 172 | + ], |
| 173 | + ) |
| 174 | +
|
| 175 | + arm_controller_spawner = Node( |
| 176 | + package="controller_manager", |
| 177 | + executable="spawner", |
| 178 | + arguments=["joint_trajectory_controller", "-c", "/controller_manager"], |
| 179 | + ) |
| 180 | +
|
| 181 | + hand_controller_spawner = Node( |
| 182 | + package="controller_manager", |
| 183 | + executable="spawner", |
| 184 | + arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], |
| 185 | + ) |
| 186 | +
|
| 187 | +Launching all the nodes |
| 188 | +----------------------- |
| 189 | + |
| 190 | +Finally, we can tell our launch file to actually launch everything we described in the previous sections. |
| 191 | + |
| 192 | +.. code-block:: python |
| 193 | +
|
| 194 | + # ... all our imports go here |
| 195 | +
|
| 196 | + def generate_launch_description(): |
| 197 | +
|
| 198 | + # ... all our other code goes here |
| 199 | +
|
| 200 | + return LaunchDescription( |
| 201 | + [ |
| 202 | + rviz_config_arg, |
| 203 | + rviz_node, |
| 204 | + static_tf, |
| 205 | + robot_state_publisher, |
| 206 | + run_move_group_node, |
| 207 | + ros2_control_node, |
| 208 | + joint_state_broadcaster_spawner, |
| 209 | + arm_controller_spawner, |
| 210 | + hand_controller_spawner, |
| 211 | + ] |
| 212 | + ) |
0 commit comments