How to Add and Plan Around Objects Using MoveIt 2

planning-around-obstacles-rviz-moveit

In this tutorial, we will build upon our previous “Hello World” project to learn how to plan around objects in a simulated environment using C++ and MoveIt 2. I will guide you through the process of inserting collision objects into the planning scene and programming the robot to plan its movements while avoiding these obstacles.

The official instructions for this tutorial are available on the MoveIt 2 website, but we’ll walk through everything together, step by step. We’ll cover how to create and add collision objects to the scene, update target poses, and use the Planning Scene Interface to communicate changes to MoveGroup.

By the end of this tutorial, you will be able to create a program that allows a robot to plan and execute movements while avoiding obstacles in its environment. The result will look something like this:

planning-around-objects-moveit

Prerequisites

All my code for this project is located here on GitHub.

Add moveit_visual_tools as a Dependency

First let’s add moveit_visual_tools as a dependency inside the hello_moveit package.

The moveit_visual_tools package is a visualization toolkit for MoveIt that provides tools to visualize robot states, trajectories, collision objects, and planning scenes in ROS 2. It integrates with RViz and is particularly useful for debugging motion planning issues, offering functions to generate markers, display text, and visualize trajectories in the 3D environment.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/
gedit package.xml

Add this dependency:

<depend>moveit_visual_tools</depend>

Save the file, and close it.

gedit CMakeLists.txt

Add this:

find_package(moveit_visual_tools REQUIRED)

ament_target_dependencies(
  moveit_ros_planning_interface
  moveit_visual_tools
  rclcpp
)

Save the file, and close it.

Now let’s build the package.

cd ~/ros2_ws/
rosdep install --from-paths src --ignore-src -y

You will see a prompt to install moveit-visual-tools.

1-prompt-install-moveit-visual-tools

Type your password, and press Enter.

I got an error that the ros-<ros_distro>-warehouse-ros-mongo package wasn’t found. Don’t worry about it.

colcon build
source ~/.bashrc

Create a New File

We’ll start our tutorial by creating a new C++ source file in the appropriate directory of our ROS 2 workspace. Here’s how to do it…

Open a terminal window on your computer. Navigate to the correct directory in your ROS 2 workspace by entering this command:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/src

Now that we’re in the right location, let’s create our new file. We’ll name it “plan_around_objects.cpp”. You can create this file using a text editor of your choice. For example, if you’re using the gedit text editor, you would type:

gedit plan_around_objects.cpp

With our new file open, add this code:

/**
 * @file plan_around_objects.cpp
 * @brief Demonstrates basic usage of MoveIt with ROS 2 and collision avoidance.
 *
 * This program initializes a ROS 2 node, sets up a MoveIt interface for a robot manipulator,
 * creates a collision object in the scene, plans a motion to a target pose while avoiding
 * the collision object, and executes the planned motion.
 *
 * @author Addison Sears-Collins
 * @date August 8, 2024
 */

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>

/**
 * @brief The main function that starts our program.
 *
 * This function sets up our ROS 2 environment and prepares it for robot control.
 *
 * @param argc The number of input arguments our program receives.
 * @param argv The list of input arguments our program receives.
 * @return int A number indicating if our program finished successfully (0) or not.
 */
int main(int argc, char * argv[])
{
  // Start up ROS 2
  rclcpp::init(argc, argv);
  
  // Creates a node named "plan_around_objects". The node is set up to automatically
  // handle any settings (parameters) we might want to change later without editing the code.
  auto const node = std::make_shared<rclcpp::Node>(
    "plan_around_objects",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Creates a "logger" that we can use to print out information or error messages
  // as our program runs.
  auto const logger = rclcpp::get_logger("plan_around_objects");
  
  // This code creates a separate thread, which is like a mini-program running alongside our main program. 
  // This thread uses a ROS 2 "executor" to continuously process information about the robot's state. 
  // By running this in its own thread, our ROS 2 node can keep getting updates about the robot without 
  // interrupting the main flow of our program.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interfaces
  // These interfaces are used to plan and execute movements, set target poses,
  // and perform other motion-related tasks for each respective part of the robot.
  // The use of auto allows the compiler to automatically deduce the type of variable.
  // Source: https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
  using moveit::planning_interface::MoveGroupInterface;
  auto arm_group_interface = MoveGroupInterface(node, "arm");

  // Specify a planning pipeline to be used for further planning 
  arm_group_interface.setPlanningPipelineId("ompl");
  
  // Specify a planner to be used for further planning
  arm_group_interface.setPlannerId("RRTConnectkConfigDefault");  

  // Specify the maximum amount of time in seconds to use when planning
  arm_group_interface.setPlanningTime(1.0);
  
  // Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1].
  arm_group_interface.setMaxVelocityScalingFactor(1.0);
  
  //  Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1].
  arm_group_interface.setMaxAccelerationScalingFactor(1.0);

  // Display helpful logging messages on the terminal
  RCLCPP_INFO(logger, "Planning pipeline: %s", arm_group_interface.getPlanningPipelineId().c_str());    
  RCLCPP_INFO(logger, "Planner ID: %s", arm_group_interface.getPlannerId().c_str());
  RCLCPP_INFO(logger, "Planning time: %.2f", arm_group_interface.getPlanningTime());
  
  // Construct and initialize MoveItVisualTools
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              arm_group_interface.getRobotModel() };
											  
  // Erase all existing visual markers from the RViz visualization. 
  moveit_visual_tools.deleteAllMarkers();

  // Load the remote control interface for MoveIt visual tools. 
  moveit_visual_tools.loadRemoteControl();
  
  // Lambda function to draw a title in the RViz visualization
  auto const draw_title = [&moveit_visual_tools](auto text) {
    // Nested lambda to create a pose for the text
    auto const text_pose = [] {
        // Create an identity transform
        auto msg = Eigen::Isometry3d::Identity();
        // Set the z-coordinate to 1.0 (1 meter above the origin)
        msg.translation().z() = 1.0;
        return msg;
    }();
    
    // Publish the text to RViz
    // Parameters: pose, text content, color (WHITE), and size (XLARGE)
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
  };
 
  // Lambda function to display a prompt message in RViz
  auto const prompt = [&moveit_visual_tools](auto text) { 
    moveit_visual_tools.prompt(text); 
  };

  // Lambda function to visualize a trajectory as a line in RViz
  auto const draw_trajectory_tool_path = 
    [&moveit_visual_tools, jmg = arm_group_interface.getRobotModel()->getJointModelGroup("arm")](
	  auto const trajectory) {moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);};

  // Set a target pose for the end effector of the arm 
  auto const arm_target_pose = [&node]{
    geometry_msgs::msg::PoseStamped msg;
    msg.header.frame_id = "base_link";
    msg.header.stamp = node->now(); 
    msg.pose.position.x = 0.128;
    msg.pose.position.y = -0.266;
    msg.pose.position.z = 0.111;
    msg.pose.orientation.x = 0.635;
    msg.pose.orientation.y = -0.268;
    msg.pose.orientation.z = 0.694;
    msg.pose.orientation.w = 0.206;
    return msg;
  }();
  arm_group_interface.setPoseTarget(arm_target_pose);
  
  // Create collision object for the robot to avoid
  auto const collision_object = [frame_id = arm_group_interface.getPlanningFrame(), &node, &logger] {
  
    // Print the planning frame for debugging purposes
    RCLCPP_INFO(logger, "Planning frame: %s", frame_id.c_str());

    // Initialize a CollisionObject message
    moveit_msgs::msg::CollisionObject collision_object;
  
    // Set the frame ID for the collision object
    collision_object.header.frame_id = frame_id;
  
    // Set the timestamp to the current time
    collision_object.header.stamp = node->now();
  
    // Assign a unique ID to the collision object
    collision_object.id = "box1";

    // Define the shape of the collision object as a box
    shape_msgs::msg::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
  
    // Set the dimensions of the box (in meters)
    primitive.dimensions[primitive.BOX_X] = 0.2;  // Width
    primitive.dimensions[primitive.BOX_Y] = 0.05;  // Depth
    primitive.dimensions[primitive.BOX_Z] = 0.50;  // Height

    // Define the pose (position and orientation) of the box
    geometry_msgs::msg::Pose box_pose;
  
    // Set the position of the box center
    box_pose.position.x = 0.22;  // meters in x-direction
    box_pose.position.y = 0.0;   // Centered in y-direction
    box_pose.position.z = 0.25;  // meters in z-direction
  
    // Set the orientation of the box (no rotation in this case)
    box_pose.orientation.x = 0.0;
    box_pose.orientation.y = 0.0;
    box_pose.orientation.z = 0.0;
    box_pose.orientation.w = 1.0;

    // Add the shape and pose to the collision object
    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
  
    // Set the operation to add the object to the planning scene
    collision_object.operation = collision_object.ADD;

    // Log information about the created collision object
    RCLCPP_INFO(logger, "Created collision object: %s", collision_object.id.c_str());
	
    RCLCPP_INFO(logger, "Box dimensions: %.2f x %.2f x %.2f", 
      primitive.dimensions[primitive.BOX_X],
      primitive.dimensions[primitive.BOX_Y],
      primitive.dimensions[primitive.BOX_Z]);
	  
    RCLCPP_INFO(logger, "Box position: (%.2f, %.2f, %.2f)",
      box_pose.position.x, box_pose.position.y, box_pose.position.z);

    // Return the fully defined collision object
    return collision_object;
  }();
  
  // Set up a virtual representation of the robot's environment
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  
  // Add an object to this virtual environment that the robot needs to avoid colliding with
  planning_scene_interface.applyCollisionObject(collision_object);
  
  // Ask the user to press 'next' to continue
  prompt("Press 'next' in the RvizVisualToolsGui window to plan");
  
  // Set up a title for the next visualization step
  draw_title("Planning");
  
  // Update the visualization to show the new title and wait for user input
  moveit_visual_tools.trigger();

  // Create a plan to that target pose
  // This will give us two things:
  // 1. Whether the planning was successful (stored in 'success')
  // 2. The actual motion plan (stored in 'plan')
  auto const [success, plan] = [&arm_group_interface] {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    auto const ok = static_cast<bool>(arm_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();

  // Try to execute the movement plan if it was created successfully
  // If the plan wasn't successful, report an error
  // Execute the plan
  if (success)
  {
	// Visualize the planned trajectory in RViz
    draw_trajectory_tool_path(plan.trajectory);
	
    // Trigger the visualization update
    moveit_visual_tools.trigger();
	
    // Prompt the user to continue to execution
    prompt("Press 'next' in the RvizVisualToolsGui window to execute");
	
    // Update the title in RViz to show we're executing the plan
    draw_title("Executing");
	
    // Trigger another visualization update
    moveit_visual_tools.trigger();
	
    // Execute the planned motion
    arm_group_interface.execute(plan);
  }
  else
  {
    // If planning failed, update the title in RViz
    draw_title("Planning Failed!");
	
    // Trigger the visualization update
    moveit_visual_tools.trigger();
	
    // Log an error message
    RCLCPP_ERROR(logger, "Planning failed!");
  }
  
  // Clean up and shut down the ROS 2 node
  rclcpp::shutdown();
  
  // Wait for the spinner thread to finish
  spinner.join();
  
  // Exit the program
  return 0;
}

Save the file, and close it.

Understanding the Plan Around Objects Code

Let’s walk through the code before we see it in action.

The code starts with some introductory comments explaining what the program does. 

Next, we see a bunch of include statements. These include statements tell the program which tools it needs to use. In this case, it’s bringing in libraries for controlling the robot, planning movements, and visualizing what’s happening.

The main function is where the actual program starts. It begins by initializing ROS 2.

The code sets up a separate thread, which is like a mini-program running alongside our main one. This thread keeps track of what’s happening with the robot in real-time.

Next, it creates interfaces for controlling the robot arm. These interfaces are how our program talks to the robot, telling it where to move and how to plan its movements. The code specifies some settings for these movements, like how fast the arm can move and how long it can take to plan a path.

The program then sets up tools for visualizing what’s happening. This allows us to see a 3D representation of the robot and its planned movements in RViz.

After that, the code specifies a target pose for the robot arm. This is the position and orientation we want the arm to move to.

One of the key parts of this program is creating a virtual obstacle. It defines a box in the robot’s workspace that the arm needs to avoid. This box is given a specific size and position.

With the target and obstacle set up, the program then asks the robot to plan a path to the target that avoids the obstacle. If it successfully creates a plan, it shows this plan in RViz and then tells the robot to execute the movement.

Throughout this process, the code includes various prompts and visual cues to keep you informed about what’s happening. It also has error handling to deal with situations where the robot can’t find a safe path to the target.

Finally, the program cleans up after itself, shutting down ROS 2 and ending the separate thread it created at the beginning.

Understanding Virtual Joints

One more topic before we run our code. Let’s discuss virtual joints. 

If you look at my code, you will see the following call:

move_group_interface.getPlanningFrame()

This code retrieves the parent frame of the “virtual joint” defined in the SRDF file. In our case, this is the “world” frame.

A virtual joint is like an imaginary connection between your robot and the world around it. It’s not a real, physical joint, but rather a way to tell the robot how it can move in relation to its environment.

Think of it like this: Imagine you have a toy robot on a table. The robot itself can move its arms and legs, but it can also slide around on the table. The sliding motion isn’t due to any actual joint in the robot, but you need a way to describe this movement. That’s where a virtual joint comes in.

For a mobile manipulator, you will often see the following in an SRDF:

<virtual_joint name="virtual_joint" type="planar" parent_frame="odom" child_link="base_footprint" />

“odom” is used as the world frame because it represents the robot’s odometry – essentially, its position in the world as the robot understands it based on its movement.

“base_footprint” is often chosen as the child link because it represents the base of the robot – imagine the point where the robot touches the ground.

The planar virtual joint tells MoveIt that the robot’s base can move in a plane. It doesn’t actually move the robot, but allows MoveIt to consider the base’s potential positions when planning arm movements.

Also, remember that MoveIt maintains a planning scene that includes the robot’s current state and the environment.

The virtual joint allows the planning scene to be updated with base movements, even though MoveIt isn’t controlling the base directly.

I will also note that for a mobile manipulator, the URDF describes just the physical structure of the robot. It does not include the virtual joint information. The URDF would typically start with the base_footprint link and describe the physical joints and links from there.

The SRDF is where the virtual joint is defined for use with MoveIt. It adds semantic information on top of the URDF, including things like virtual joints, group definitions, and end effectors (i.e. grippers).

Edit CMakeLists.txt

Now let’s build our code.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/
gedit CMakeLists.txt

Add this code.

Save the file, and close it.

Edit package.xml

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/
gedit package.xml

Add this code.

Save the file, and close it.

Build the Package

cd ~/ros2_ws
colcon build 
source ~/.bashrc

Run the Program

Now that we’ve set up our target pose and added a collision object to the planning scene, it’s time to run our program and see the results.

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false

Launch MoveIt and RViz.

ros2 launch mycobot_moveit_config_manual_setup move_group.launch.py

Run your node:

ros2 run hello_moveit plan_around_objects

When you run the program, you should observe the following in RViz:

  • The robot model should be visible.
  • A box representing your collision object should appear in the planning scene.
2-collision-object

Now click Next under RVizVisualToolsGui on the bottom-left of RViz.

3-click-next

The robot should plan a path to the target pose you specified, avoiding the collision object.

If the planning is successful, you’ll see a visualization of the planned path in RViz. The path should clearly avoid the collision object you added.

You will also see a “Planning request complete!” message in the logs.

4-planning-request-complete

If you don’t see the collision object or if the robot’s planned path seems to ignore it, double-check that your collision object’s dimensions and position are appropriate for your robot’s workspace.

Now type Next again to execute.

You should see your robot move to the goal.

5-robot-reached-the-goal-successfully-moveit-rviz
6-execution-success

That’s it. Keep building!