Build a MoveIt! Package¶

In this exercise, we will create a MoveIt! package for an industrial robot. This package creates the configuration and launch files required to use a robot with the MoveIt! Motion-Control nodes. In general, the MoveIt! package does not contain any C++ code.

IMPORTANT: This exercise requires running MoveIt in ROS1, as MoveIt2 does not have full feature parity yet. The final section of this exercise demonstrates the process of building a MoveIt2 package using the output created in ROS1. If you don’t have a ROS1 version of a workspace, you may create one using the solutions provided in the exercises/ folder of this repository.

Motivation¶

MoveIt! is a free-space motion planning framework for ROS. It’s an incredibly useful and easy-to-use tool for planning motions between two points in space without colliding with anything. Under the hood MoveIt is quite complicated, but unlike most ROS libraries, it has a really nice GUI Wizard to get you going.

Reference Example¶

Using MoveIt with ROS-I

Further Information and Resources¶

MoveIt’s Standard Wizard Guide

Scan-N-Plan Application: Problem Statement¶

In this exercise, you will generate a MoveIt package for the UR5 workcell you built in a previous step. This process will mostly involve running the MoveIt! Setup Assistant. At the end of the exercise you should have the following:

A new package called myworkcell_moveit_config
A moveit configuration with one group (“manipulator”), that consists of the kinematic chain between the UR5’s base_link and tool0.

Scan-N-Plan Application: Guidance¶

Start the MoveIt! Setup Assistant (don’t forget auto-complete with tab):
```
roslaunch moveit_setup_assistant setup_assistant.launch
```
Select “Create New MoveIt Configuration Package”, select the workcell.xacro you created previously, then “Load File”.
Work your way through the tabs on the left from the top down.
1. Generate a self-collision matrix.
2. Add a fixed virtual base joint. e.g.
```
name = 'FixedBase' (arbitrary)
child = 'world' (should match the URDF root link)
parent = 'world' (reference frame used for motion planning)
type = 'fixed'
```
3. Add a planning group called manipulator that names the kinematic chain between base_link and tool0. Note: Follow ROS naming guidelines/requirements and don’t use any whitespace, anywhere.
  
  a. Set the kinematics solver to KDLKinematicsPlugin
4. Create a few named positions (e.g. “home”, “allZeros”, etc.) to test with motion-planning.
5. Don’t worry about adding end effectors/grippers or passive joints for this exercise.
6. Enter author / maintainer info.
  
  Yes, it’s required, but doesn’t have to be valid
7. Generate a new package and name it myworkcell_moveit_config.
  - make sure to create the package inside your catkin_ws/src directory

The outcome of these steps will be a new package that contains a large number of launch and configuration files. At this point, it’s possible to do motion planning, but not to execute the plan on any robot. To try out your new configuration:

catkin build
source ~/catkin_ws/devel/setup.bash
roslaunch myworkcell_moveit_config demo.launch

Don’t worry about learning how to use RViz to move the robot; that’s what we’ll cover in the next session!

Using MoveIt! with Physical Hardware¶

MoveIt!’s setup assistant generates a suite of files that, upon launch:

Loads your workspace description to the parameter server.
Starts a node move_group that offers a suite of ROS services & actions for doing kinematics, motion planning, and more.
An internal simulator that publishes the last planned path on a loop for other tools (like RViz) to visualize.

Essentially, MoveIt can publish a ROS message that defines a trajectory (joint positions over time), but it doesn’t know how to pass that trajectory to your hardware.

To do this, we need to define a few extra files.

Create a controllers.yaml file (myworkcell_moveit_config/config/controllers.yaml) with the following contents:

controller_list:
  - name: ""
    action_ns: joint_trajectory_action
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

Create the joint_names.yaml file (myworkcell_moveit_config/config/joint_names.yaml):

controller_joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

Fill in the existing, but blank, controller_manager launch file (myworkcell_moveit_config/launch/myworkcell_moveit_controller_manager.launch.xml):

<launch>
  <arg name="moveit_controller_manager"
       default="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
  <param name="moveit_controller_manager"
         value="$(arg moveit_controller_manager)"/>

  <rosparam file="$(find myworkcell_moveit_config)/config/controllers.yaml"/>
</launch>

Create a new myworkcell_planning_execution.launch (in myworkcell_moveit_config/launch):

<launch>
  <!-- The planning and execution components of MoveIt! configured to run -->
  <!-- using the ROS-Industrial interface. -->

  <!-- Non-standard joint names:
       - Create a file [robot_moveit_config]/config/joint_names.yaml
           controller_joint_names: [joint_1, joint_2, ... joint_N]
       - Update with joint names for your robot (in order expected by rbt controller)
       - and uncomment the following line: -->
  <rosparam command="load" file="$(find myworkcell_moveit_config)/config/joint_names.yaml"/>

  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
  <!--  - if sim=false, a robot_ip argument is required -->
  <arg name="sim" default="true" />
  <arg name="robot_ip" unless="$(arg sim)" />

  <!-- load the robot_description parameter before launching ROS-I nodes -->
  <include file="$(find myworkcell_moveit_config)/launch/planning_context.launch" >
   <arg name="load_robot_description" value="true" />
  </include>

  <!-- run the robot simulator and action interface nodes -->
  <group if="$(arg sim)">
    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />

    <!-- publish the robot state (tf transforms) -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  </group>

  <!-- run the "real robot" interface nodes -->
  <!--   - this typically includes: robot_state, motion_interface, and joint_trajectory_action nodes -->
  <!--   - replace these calls with appropriate robot-specific calls or launch files -->
  <group unless="$(arg sim)">
    <remap from="follow_joint_trajectory" to="joint_trajectory_action"/>
    <include file="$(find ur_modern_driver)/launch/ur_common.launch" >
      <arg name="robot_ip" value="$(arg robot_ip)"/>
      <arg name="min_payload" value="0.0"/>
      <arg name="max_payload" value="5.0"/>
    </include>
  </group>

  <include file="$(find myworkcell_moveit_config)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
  </include>

  <include file="$(find myworkcell_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
  </include>

</launch>

Now let’s test the new launch files we created:

roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch

Creating a ROS2 package¶

The MoveIt package created using the setup assistant in ROS1 can be significantly reused in a ROS2 system with MoveIt2, where the setup assistant is not yet available. The following steps will set up a ROS2 equivalent of the MoveIt config package you just created for ROS1. Note that the world of MoveIt is continually evolving in ROS2; this is currently just one way to get a ROS2 package going and is not presented as the “correct” way.

In a new terminal, source your ros2_ws setup file and create a new empty package:

. ~/ros_ws/install/setup.bash
cd ~/ros_ws/src
ros2 pkg create myworkcell_moveit_config --dependencies myworkcell_support

Copy the config/ subdirectory from the ROS1 MoveIt config package into the new empty ROS2 package:

cp -r ~/catkin_ws/src/myworkcell_moveit_config/config ~/ros2_ws/src/myworkcell_moveit_config/

Open the config/ompl_planning.yaml file in a text editor and add the following lines at the bottom:

planning_plugin: 'ompl_interface/OMPLPlanner'
request_adapters: >-
    default_planner_request_adapters/AddTimeOptimalParameterization
    default_planner_request_adapters/FixWorkspaceBounds
    default_planner_request_adapters/FixStartStateBounds
    default_planner_request_adapters/FixStartStateCollision
    default_planner_request_adapters/FixStartStatePathConstraints

Also place quotes around the value of the longest_valid_segment_fraction option if it doesn’t have them already. E.g., longest_valid_segment_fraction: "0.005".

Open the config/fake_controllers.yaml file and replace its contents with these lines:

fake_joint_trajectory_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    write_op_modes:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

This step allows for plans created by MoveIt2 to be executed, but only by an instance of a “fake_joint_trajectory_controller”. This will be our stand-in for a physical robot later on in the training.

Create a new file, config/moveit.yaml and place the following lines in it:

planning_scene_monitor_options:
  name: "planning_scene_monitor"
  robot_description: "robot_description"
  joint_state_topic: "/joint_states"
  attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
  publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
  monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
  wait_for_initial_state_timeout: 10.0

planning_pipelines:
  pipeline_names: ["ompl"]

plan_request_params:
  planning_attempts: 10
  planning_pipeline: ompl
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0

Open the new package’s CMakeLists.txt file and add an installation rule for the config/ directory underneath the calls to find_package:
```
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
```
Rebuild the workspace:
```
cd ~/ros2_ws
colcon build
```