The Complete Guide to Docker for ROS 2 Jazzy Projects

In this tutorial, we’ll learn how to use Docker to make working with ROS 2 projects easier and more reliable.

When developing robotics software, setting up your environment can be frustrating—installing ROS 2, managing dependencies, and making sure everything works the same on different computers. Docker solves this by creating containers—lightweight, isolated environments that include everything your ROS 2 project needs to run.

A Docker container contains:

  • ROS 2 (and any version you choose)
  • All necessary dependencies and libraries
  • Your project files and code

Because everything is bundled together, Docker containers make sure your project runs the same way, whether on your laptop, a robot, or a cloud server. No more “it works on my machine” issues!

In this tutorial, I’ll guide you step by step—from understanding Docker basics to creating custom containers for ROS 2 projects. You’ll also learn how to handle more advanced setups like running multiple containers and using graphical tools such as RViz.

By the end, you’ll know how to use Docker to build more portable, consistent, and manageable robotics environments. Let’s get started!

Prerequisites

You have completed this tutorial: Pick and Place Using MoveIt 2 and Perception – ROS 2 Jazzy.

What is a Docker Container?

Think of a Docker container as a lightweight, standalone, and executable package that includes everything needed to run a piece of software, including the code, runtime, system tools, libraries, and settings.

Here’s an analogy to help understand Docker:

Imagine you’re moving to a new house. Instead of packing all your belongings into random boxes, you pack each room into its own shipping container. Each container has everything that room needs to function – furniture, appliances, decor, even the right temperature and humidity settings. When you arrive at your new house, you can unload each container, and each room is instantly set up exactly as it was, regardless of what the new house is like.

In the same way, Docker containers package up not just your application, but also all its dependencies, configurations, and even the operating system it needs. This containerized application can then run on any computer that has Docker installed, regardless of that computer’s specific setup.

Key benefits of Docker for ROS 2 development include:

  1. Consistency: Your ROS 2 application will run the same way on any machine with Docker, eliminating “it works on my machine” problems.
  2. Isolation: Each container runs in its own environment, preventing conflicts between different projects or versions of ROS 2.
  3. Portability: You can share  your entire ROS 2 setup with others or deploy it to different machines.
  4. Version Control: You can manage different versions of ROS 2 or your application in separate containers.
  5. Resource Efficiency: Containers are more lightweight than traditional virtual machines, using fewer system resources.

About Docker Images

Docker images are the foundation of containerization, serving as blueprints for creating containers. Think of a Docker image as a recipe for a gourmet meal. Just as a recipe contains all the ingredients and instructions needed to prepare a dish, a Docker image includes all the components and instructions required to run a specific application or environment.

An Docker image is a snapshot of a Docker container, capturing the application code, runtime, libraries, environment variables, and configuration files. When you’re working with ROS 2, your image might start with a base that includes the ROS 2 framework, then add your own robotics code and any additional dependencies on top.

Docker images are read-only, meaning once created, they don’t change. If you need to make modifications, you create a new image, much like creating a new version of a recipe. This immutability ensures consistency across different environments, from development to production.

Images can be shared with others through registries like Docker Hub, similar to how you might share a favorite recipe with friends. 

Exploring Public ROS 2 Docker Images

Docker Hub is the official repository for Docker images, serving as a central location where developers can find, share, and download pre-built Docker images. For ROS 2 developers, it’s the go-to place to find official ROS 2 Docker images maintained by the ROS community.

On Docker Hub, you can find a wide variety of ROS 2 images for different distributions and configurations. The official ROS 2 images are hosted under the “ros” repository. To explore these images, you can visit this page. 

At that page, you’ll see a list of available ROS 2 images with different tags. These tags represent various ROS 2 distributions and configurations. For example, you might see tags like:

  • jazzy
  • jazzy-ros-core
  • jazzy-perception
  • jazzy-ros-base

The page features a “Filter Tags” search bar, which is useful for finding specific ROS 2 distributions or configurations. 

For example, if you’re looking for Jazzy-specific images, you can type “jazzy” in the search bar to filter the results.

Each image tag provides information about the operating system it supports, potential vulnerabilities, and the compressed size of the image. This information helps you choose the right image for your project and system requirements.

Later in this tutorial, I will show you how we pull an image from here automatically using what is called a Dockerfile.

Install Docker

Let’s go ahead and install Docker.

I will walk you through the entire process, but you can use this page as a reference.

Uninstall Old Versions of Docker

The first thing you need to do is to uninstall any outdated versions of Docker that might be lurking on your computer.

Open a terminal window, and type the following command:

for pkg in docker.io docker-doc docker-compose docker-compose-v2 podman-docker containerd runc; do sudo apt-get remove $pkg; done

You might get a message back saying that you have none of those packages installed. That is just fine.

Set Up the Apt Repository

Run these commands in order:

sudo apt-get update
sudo apt-get install ca-certificates curl
sudo install -m 0755 -d /etc/apt/keyrings
sudo curl -fsSL https://download.docker.com/linux/ubuntu/gpg -o /etc/apt/keyrings/docker.asc
sudo chmod a+r /etc/apt/keyrings/docker.asc

Add the repository to Apt sources:

echo \
  "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.asc] https://download.docker.com/linux/ubuntu \
  $(. /etc/os-release && echo "$VERSION_CODENAME") stable" | \
  sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
sudo apt-get update

Install Docker

sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin docker-compose

Verify that Docker Installed

sudo docker run hello-world
1-docker-hello-world

Post-Installation Configuration

Here are some recommended steps to take after you have successfully installed Docker.

Create the docker group.

sudo groupadd docker

Add your user to the docker group.

sudo usermod -aG docker $USER

Reboot your computer.

sudo reboot

Or you could have activated changes to groups instead of rebooting:

newgrp docker

To test that you can run Docker without using “sudo”, type the following command:

docker run hello-world
2-without-sudo

Now configure Docker to start on boot:

sudo systemctl enable docker.service
sudo systemctl enable containerd.service

You can stop this startup on boot at any time, by typing:

sudo systemctl disable docker.service
sudo systemctl disable containerd.service

Now you’re all set.

Create an entrypoint.sh

After successfully installing Docker on your system, the next step is to create an entrypoint.sh script. This script is important for setting up the proper environment inside your Docker container.

The entrypoint.sh script is the first script that runs when your Docker container starts. It is responsible for setting up the ROS 2 environment, ensuring all necessary variables are set, and preparing your workspace. Let’s create this script in your project directory.

Create a new folder called docker.

cd ~/ros2_ws/src/mycobot_ros2
mkdir -p docker && cd docker

Create a new script.

touch entrypoint.sh

Add the following content.

Save the file, and close it.

This script performs several important tasks:

  1. It sets up important environment variables.
  2. It manages the ROS_DOMAIN_ID, which is important for multi-robot communication.
  3. It sources the necessary ROS 2 setup files.
  4. It builds your ROS 2 workspace using colcon.

After creating the file, make it executable by running:

chmod +x ~/ros2_ws/src/mycobot_ros2/docker/entrypoint.sh

This entrypoint.sh script will be used in your Dockerfile, which we’ll create in the next steps. It ensures that every time you start a container based on your image, the ROS 2 environment is properly configured and ready for development.

Create a workspace.sh

After setting up our entrypoint.sh, the next step is to create a workspace.sh script. This script is responsible for setting up our ROS 2 workspace inside the Docker container, including installing any third-party packages we might need for our robotics project.

Let’s create this script in the same directory as our entrypoint.sh. 

Open a terminal window.

cd ~/ros2_ws/src/mycobot_ros2/docker
touch workspace.sh 

Copy the this content into the file.

Save the file, and close it.

This script does the following:

  1. Sources the ROS 2 setup file to ensure we’re working in a ROS 2 environment.
  2. Navigates to the src directory of our ROS 2 workspace.
  3. Clones third-party packages.
  4. Installs all ROS 2 dependencies for the newly added packages.
  5. Builds the workspace using colcon.

After creating the file, make it executable by running:

chmod +x ~/ros2_ws/src/mycobot_ros2/docker/workspace.sh

This workspace.sh script will be used in your Dockerfile to set up your ROS 2 workspace during the image build process. It allows you to add or remove packages from your workspace by modifying this script, ensuring that your Docker image contains all the necessary packages for your robotics project.

Create a build.sh File

After setting up our entrypoint.sh and workspace.sh scripts, the next step is to create a build.sh file. This script automates the process of building our Docker image and setting up necessary directories on the host system. Let’s create this script in the same directory as our previous scripts.

Open a terminal window.

cd ~/ros2_ws/src/mycobot_ros2/docker
touch build.sh 

Copy this content into the file:

#!/bin/bash

# Get the absolute path of the directory containing this script (docker directory)
SCRIPT_PATH=$(dirname $(realpath "$0"))

# Get the parent directory path (mycobot_ros2 directory)
# This is where our actual ROS 2 package code lives
PARENT_PATH=$(dirname "$SCRIPT_PATH")

# Function to build the Docker image
# This function handles the actual Docker build process
build_docker_image()
{
    # Set a log message for the build process
    LOG="Building Docker image manipulation:latest ..."

    # Print the log message using our debug function
    print_debug

    # Build the Docker image
    # -f $SCRIPT_PATH/Dockerfile: Specify the path to the Dockerfile in the docker directory
    # -t manipulation:latest: Tag the image as manipulation:latest
    # $PARENT_PATH: Use the parent directory as the build context, allowing access to all package files
    # --no-cache: Build the image without using the cache, ensuring fresh dependencies
    sudo docker image build -f $SCRIPT_PATH/Dockerfile -t manipulation:latest $PARENT_PATH --no-cache
}

# Function to create a shared folder
# This folder will be used to share files between the host and the Docker container
create_shared_folder()
{
    # Check if the directory doesn't exist
    if [ ! -d "$HOME/automaticaddison/shared/ros2" ]; then
        # Set a log message for folder creation
        LOG="Creating $HOME/automaticaddison/shared/ros2 ..."

        # Print the log message
        print_debug

        # Create the directory and its parent directories if they don't exist
        # -p flag creates parent directories as needed
        mkdir -p $HOME/automaticaddison/shared/ros2
    fi
}

# Function to print debug messages
# This provides consistent formatting for our log messages
print_debug()
{
    # Print an empty line for readability
    echo ""

    # Print the log message
    echo $LOG

    # Print another empty line for readability
    echo ""
}

# Main execution flow

# First, create the shared folder that will be mounted in the container
create_shared_folder

# Then, build the Docker image
build_docker_image

Save the file, and close it.

This script does the following:

  1. Determines the script’s directory path.
  2. Defines a function to build the Docker image using the Dockerfile in the same directory.
  3. Defines a function to create a shared folder on the host system.
  4. Executes the functions to create the shared folder and build the Docker image.

After creating the file, make it executable by running:

chmod +x ~/ros2_ws/src/mycobot_ros2/docker/build.sh

This build.sh script serves several important purposes:

  1. It automates the Docker image building process, ensuring consistency each time you build.
  2. It creates a shared folder on your host system, which can be used to exchange files between your host and the Docker container.
  3. It uses the –no-cache option when building the Docker image, ensuring that you always get the latest versions of packages and dependencies.

This will create the necessary shared folder and build your Docker image based on the Dockerfile we’ll create in the next steps. The script provides a convenient way to rebuild your image whenever you make changes to your Dockerfile or other configuration files.

Create a bash_aliases.txt

After setting up our build scripts, it’s helpful to create a bash_alias.txt file. This file will contain useful aliases that can simplify common tasks in our ROS 2 development workflow. Aliases are shorthand commands that can represent longer, more complex commands.

Let’s create this file now.

cd ~/ros2_ws/src/mycobot_ros2/docker
touch bash_aliases.txt

Copy this content into the file:

# Aliases for the ~/.bashrc file
alias build='cd ~/ros2_ws/ && colcon build && source ~/.bashrc'
alias cleanup='echo "Cleaning up..." && \
sleep 5.0 && \
pkill -9 -f "ros2|gazebo|gz|nav2|amcl|bt_navigator|nav_to_pose|rviz2|assisted_teleop|cmd_vel_relay|robot_state_publisher|joint_state_publisher|move_to_free|mqtt|autodock|cliff_detection|moveit|move_group|basic_navigator"'
alias elephant='ros2 launch mycobot_description robot_state_publisher.launch.py'
alias moveit='bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_gazebo_and_moveit.sh'
alias mtc_demos='bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_mtc_demos.sh'
alias pick='bash ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/scripts/robot.sh'
alias pointcloud='bash ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/scripts/pointcloud.sh'
alias robot='bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_gazebo.sh'

Save the file, and close it.

To use these aliases in your Docker container, you’ll need to include this file in your Docker image and source it in the .bashrc file. We’ll cover how to do this when we create the Dockerfile in a future step.

Create a docker-compose.yml File

After setting up our build scripts and aliases, the next step is to create a docker-compose.yml file. This file defines how Docker should run our containers, allowing us to manage multi-container applications.

Let’s create this file now.

cd ~/ros2_ws/src/mycobot_ros2/docker
touch docker-compose.yml

Add this code. 

Save the file, and close.

This docker-compose.yml file does several important things:

  1. It defines a service named ‘manipulation’ based on our Docker image.
  2. It sets up the container to use the host’s network and IPC namespace.
  3. It grants the container privileged access, which is often necessary for robotics applications.
  4. It sets up environment variables and volume mappings for GUI applications and file sharing.
  5. It includes commented-out examples of how to set up multiple containers for different ROS 2 nodes.

Write a Dockerfile From Scratch

After setting up our support files, the next important step is to create our Dockerfile. This file contains instructions for building our Docker image, which will serve as the environment for our ROS 2 development.

Create a new file named Dockerfile.

cd ~/ros2_ws/src/mycobot_ros2/docker
touch Dockerfile

Add this content.

# Set the ROS distribution as an argument, defaulting to 'jazzy'
ARG ROS_DISTRO=jazzy

# The base image comes from the official ROS repository hosted on Docker Hub
# You can find available ROS images here: https://hub.docker.com/_/ros/tags
# We're using the ros-base image which includes core ROS 2 packages
FROM ros:${ROS_DISTRO}-ros-base

# Set the maintainer information for this Dockerfile
LABEL maintainer="AutomaticAddison<automatic_addison@todo.com>"

# Set environment variables
ENV PIP_BREAK_SYSTEM_PACKAGES=1
ENV DEBIAN_FRONTEND=noninteractive

# Set the default shell to bash for RUN commands
# This ensures all RUN commands use bash instead of sh
SHELL ["/bin/bash", "-c"]

# Update the system and install essential tools
# This step upgrades all packages and installs utilities needed for development
RUN apt-get update -q && \
    apt-get upgrade -yq && \
    apt-get install -yq --no-install-recommends apt-utils wget curl git build-essential \
    vim sudo lsb-release locales bash-completion tzdata gosu gedit htop nano libserial-dev

# Install additional tools required for ROS 2 development
# These packages help with building and managing ROS 2 workspaces
RUN apt-get update -q && \
    apt-get install -y gnupg2 iputils-ping usbutils \
    python3-argcomplete python3-colcon-common-extensions python3-networkx python3-pip python3-rosdep python3-vcstool

# Set up the ROS 2 environment
# This ensures that ROS 2 commands are available in the shell
# rosdep is a tool for installing system dependencies for ROS packages
RUN rosdep update && \
    grep -F "source /opt/ros/${ROS_DISTRO}/setup.bash" /root/.bashrc || echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc && \
    grep -F "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" /root/.bashrc || echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> /root/.bashrc

# Install additional ROS 2 packages
RUN apt-get update && \
    apt-get install -y \
    ros-${ROS_DISTRO}-joint-state-publisher-gui \
    ros-${ROS_DISTRO}-xacro \
    ros-${ROS_DISTRO}-demo-nodes-cpp \
    ros-${ROS_DISTRO}-demo-nodes-py \
    ros-${ROS_DISTRO}-rviz2 \
    ros-${ROS_DISTRO}-rqt-reconfigure

# Install Mesa graphics drivers
# Mesa is an open-source implementation of OpenGL and other graphics APIs
# It's crucial for 3D rendering in many applications, including RViz in ROS
RUN apt-get update && \
    apt-get install -y software-properties-common && \
    DEBIAN_FRONTEND=noninteractive add-apt-repository ppa:kisak/kisak-mesa

# Create necessary directories
RUN mkdir -p /etc/udev/rules.d && \
    mkdir -p /root/ros2_ws/src/mycobot_ros2

# Copy the entire project into the container
# Since we're using the parent directory as context, we can copy everything
COPY . /root/ros2_ws/src/mycobot_ros2

# Copy configuration files from docker directory
# Note the docker/ prefix since we're in the parent context
COPY docker/workspace.sh /root/
COPY docker/entrypoint.sh /root/
COPY docker/bash_aliases.txt /root/.bashrc_aliases

# Make scripts executable
RUN chmod +x /root/workspace.sh /root/entrypoint.sh

# Add custom bash aliases
RUN cat /root/.bashrc_aliases >> /root/.bashrc

# Run the workspace setup script
# This typically installs workspace dependencies and builds the ROS 2 packages
WORKDIR /root
RUN ./workspace.sh

# Ensure the ROS 2 workspace is sourced in every new shell
RUN echo "source /root/ros2_ws/install/setup.bash" >> /root/.bashrc

# Set the entrypoint for the container
# This script will be run every time the container starts
ENTRYPOINT ["/root/entrypoint.sh"]

# Set the default command
# This keeps the container running indefinitely, allowing you to exec into it
CMD ["/bin/bash", "-c", "tail -f /dev/null"]

Save the file, and close.

This Dockerfile does several important things:

  1. It starts from a base ROS 2 image, allowing you to specify the ROS distribution.
  2. It installs necessary system packages and ROS 2 tools.
  3. It sets up the ROS 2 environment and installs additional ROS 2 packages.
  4. It copies your workspace files into the container.
  5. It runs the workspace.sh script to set up your ROS 2 workspace.
  6. It sets up the entrypoint script to run when the container starts.

Building a Docker Image

Now that we have our Dockerfile and supporting scripts ready, it’s time to build our Docker image. We’ll use the build.sh script we created earlier to automate this process.

To build the Docker image, follow these steps…

Open a terminal window.

Navigate to the directory containing your build.sh script:

cd ~/ros2_ws/src/mycobot_ros2/docker

Run the build script:

bash build.sh

When you run this command, the script will perform the following actions:

  1. Create the shared folder ~/automaticaddison/shared/ros2 if it doesn’t already exist.
  2. Build the Docker image using the Dockerfile in the current directory.
  3. Tag the image as manipulation:latest.

The build process may take several minutes, depending on your internet connection and computer speed. You’ll see output in the terminal as Docker executes each instruction in the Dockerfile.

Once the build process is complete, you can verify that your image was created successfully by running:

docker images

You should see an entry for manipulation:latest in the list of images.

3-docker-images

If you encounter any errors during the build process, check the error messages in the terminal output. Common issues include network problems, insufficient disk space, or syntax errors in the Dockerfile.

Keep an eye on your system storage to make sure you have enough memory for the Docker image:

df -h

If you run out of storage, you can type:

docker system prune

Remember, you can re-run the build.sh script whenever you make changes to your Dockerfile or want to ensure you have the latest versions of all packages in your image.

Useful Commands 

You don’t need to run these commands. This is just for your future reference.

If at any stage you want to remove the image you just created, type:

docker rmi manipulation:latest

To remove unused images, you would type:

docker image prune

To remove all images, you would do this:

docker image prune -a

You can also free up space on your computer using this command (I run this command regularly):

docker system prune

To inspect an image’s details:

docker image inspect manipulation:latest

To view the history of an image’s layers:

docker history manipulation:latest

To save an image as an archive:

docker save manipulation:latest > manipulation.tar

To load an image from a tar archive:

docker load < manipulation.tar

Creating and Running a Docker Container

After building our Docker image, the next step is to create and run a container from it. This process brings our ROS 2 environment to life, allowing us to interact with it and run our robotics applications.

Here’s how to create and run a Docker container using our newly built image.

Ensure you’re in the directory containing the docker-compose.yml file:

cd ~/ros2_ws/src/mycobot_ros2/docker

Start the container using Docker Compose:

docker-compose up -d manipulation

Here are some useful commands (you don’t need to run these now)…

To run all services:

docker-compose up -d

To run specific services:

docker-compose up -d manipulation
4-up-d-manipulation

After starting your container using docker-compose, it’s a good practice to check if it is running as expected. Here’s how you can do that:

docker container ps -a

This command is particularly useful as it shows all containers, regardless of their state (running, stopped, or exited). It helps you verify that your containers were created successfully, even if they’ve stopped for some reason.

The output will look something like this:

5-docker-container-ps-a

This output shows:

  • The container ID
  • The image used to create the container
  • The command running in the container
  • When it was created
  • Its current status
  • Any mapped ports
  • The container name

If you see your container listed with a status of “Up”, it means it is running correctly. If you see a different status or don’t see your container at all, you may need to troubleshoot your docker-compose setup or container configuration.

Remember, you can always use docker-compose logs to check for any error messages if your container is not running as expected.

To view the logs of a specific service:

docker-compose logs -f manipulation

Running ROS 2 Nodes Inside a Docker Container

Now let’s go inside the manipulation container and see if we can run an arbitrary command:

Type the following command in the host machine to enable the GUI (we will need this later for RViz):

xhost + 
6-xhost-plus
docker-compose exec manipulation bash

This opens a bash shell in the ‘manipulation’ container.

Let’s open another instance of this container.

Open a new terminal window, and type:

docker-compose exec manipulation bash

Let’s check out the robotic arm description. Type:

elephant

You could also type:

ros2 launch mycobot_description robot_state_publisher.launch.py
7-elephant

To stop the program:

Press CTRL + C to stop the nodes.

You will be unable to run anything with Gazebo due to the limited memory of our machine, but you can see that we can run RViz with no issues since it is much more lightweight.

Now let’s test how we can share files between the Docker container and the host computer.

Inside your Docker container, type this:

echo "Hello from Docker" > /root/shared/ros2/test.txt

On your host machine, check the contents of the file:

cat ~/automaticaddison/shared/ros2/test.txt

You should see the message “Hello from Docker”.

8-check-the-contents

To exit the container shells, type:

exit

To stop the manipulation container:

docker-compose stop manipulation

This command preserves the container and its state, allowing you to quickly start it again later without recreating everything from scratch.

If you want to start the container again later, you type:

docker-compose up -d manipulation

Then to get back into the container, type:

docker-compose exec manipulation bash

Creating Separate Docker Containers That Communicate With Each Other Using docker-compose.yaml

Now let’s run multiple Docker containers and see how they communicate with each other.

First, open the docker-compose.yml file in a text editor:

gedit docker-compose.yml

Uncomment the ‘minimal_publisher’ and ‘minimal_subscriber’ services in the docker-compose.yml file. It should look like this:

version: '3.3'
services:
  manipulation: &manipulation
    container_name: manipulation
    image: manipulation:latest
    ipc: host
    network_mode: host
    privileged: true
    environment:
      - DISPLAY
      - XAUTHORITY=/tmp/.Xauthority
    volumes:
      - $HOME/automaticaddison/shared/ros2:/root/shared/ros2
      - $HOME/.Xauthority:/tmp/.Xauthority
      - /tmp/.X11-unix:/tmp/.X11-unix:rw
      - /dev:/dev
    command: ["tail", "-f", "/dev/null"]

  # The following services are commented out for this tutorial.
  # They demonstrate how to create multiple containers from the same Docker image,
  # each running a specific ROS 2 node. These containers can communicate with each other
  # because they share the same ROS_DOMAIN_ID.
  # Publisher service: Runs the demo talker node
  minimal_publisher:
    <<: *manipulation  # This uses all the same settings as the 'manipulation' service
    container_name: minimal_publisher
    command: ["ros2", "run", "demo_nodes_cpp", "talker"]

  # Subscriber service: Runs the demo listener node
  minimal_subscriber:
    <<: *manipulation  # This uses all the same settings as the 'manipulation' service
    container_name: minimal_subscriber
    command: ["ros2", "run", "demo_nodes_cpp", "listener"]

Save and exit the text editor.

Now, start all the containers:

docker-compose up -d

To verify that all containers are running:

docker ps -a

You should see three containers: manipulation, minimal_publisher, and minimal_subscriber.

Now, let’s enter the manipulation container to observe the ROS 2 system:

docker-compose exec manipulation bash

Inside the container, check the active ROS 2 nodes:

ros2 node list

You should see output similar to:

/listener
/talker

Next, check the active ROS 2 topics:

ros2 topic list

You should see output including:

/chatter
/parameter_events
/rosout

To see the messages being published, you can use:

ros2 topic echo /chatter
12-ros2-topic-echo-chatter

This will show the messages being published by the publisher node and received by the subscriber node.

To exit the topic echo, press CTRL + C.

To see the number of publishers and subscribers on the /chatter topic, type:

ros2 topic info /chatter
13-ros2-topic-info

Exit the container:

exit

When you’re done, stop all the containers:

docker-compose stop

This demonstration shows how multiple Docker containers can run different ROS 2 nodes and communicate with each other. The publisher and subscriber nodes are running in separate containers but can still interact through ROS 2 topics, thanks to the shared network configuration in the docker-compose.yml file.

This setup is useful for distributed robotics applications, allowing you to isolate different components of your system while maintaining communication between them.

You can for example have one container that runs only the LIDAR and another that is dedicated to publishing GPS data.

If you ever want to remove the manipulation container, you can type:

docker container rm manipulation

If you want to remove a container that is running you need to -f (force) flag:

docker container rm -f manipulation

That’s it! Keep building!

Pick and Place Using MoveIt 2 and Perception – ROS 2 Jazzy

In this tutorial, we will use the MoveIt Task Constructor for ROS 2 to carry out a pick and place task. Here is what you will build by the end of this tutorial:

pick-place-demo-rviz-600-fast
pick-place-gazebo-600-fast

We’ll use a depth camera to dynamically detect and locate objects in the scene.

By the end of this tutorial, you’ll have created a vision-enhanced pick and place system that demonstrates the power of combining perception with sophisticated motion planning.

Here is what skills you will learn in this tutorial:

  1. Learn how to integrate a simulated depth camera into Gazebo
  2. Learn how to process point cloud data to detect and locate objects.
    • A point cloud is a collection of data points in 3D space that represent the surface of an object or environment.
  3. Learn how to dynamically update the MoveIt planning scene based on visual information
  4. Learn how to modify the MoveIt Task Constructor pipeline to use real-time object poses (positions and orientations)

Here is a high-level overview of what our enhanced program will do:

  1. Set up the demo scene with randomly placed objects
  2. Acquire and process point cloud data from the depth camera
  3. Detect and localize objects in the scene
  4. Update the planning scene with the detected objects
  5. Define a pick sequence that includes:
    • Opening the gripper
    • Moving to a visually determined pre-grasp position
    • Approaching the target object
    • Closing the gripper
    • Lifting the object
  6. Define a place sequence that includes:
    • Moving to a designated place location
    • Lowering the object
    • Opening the gripper
    • Retreating from the placed object
  7. Plan the entire pick and place task using the updated scene information
  8. Optionally execute the planned task
  9. Provide detailed feedback on each stage of the process, including visual perception results

Prerequisites

All the code is here on my GitHub repository. Note that I am working with ROS 2 Jazzy, so the steps might be slightly different for other versions of ROS 2.

Create a Package

Let’s create a package to store our code.

cd ~/ros2_ws/src/mycobot_ros2/
ros2 pkg create \
  --build-type ament_cmake \
  --dependencies \
    generate_parameter_library \
    libpcl-all-dev \
    moveit_common \
    moveit_core \
    moveit_ros_planning \
    moveit_ros_planning_interface \
    moveit_task_constructor_core \
    moveit_task_constructor_msgs \
    pcl_conversions \
    pcl_ros \
    rclcpp \
    sensor_msgs \
    shape_msgs \
    tf2_eigen \
    tf2_geometry_msgs \
  --license BSD-3-Clause \
  --maintainer-name ubuntu \
  --maintainer-email automaticaddison@todo.com \
  --description "Pick and place demo using the MoveIt Task Constructor for motion planning and the Point Cloud Library for perception." \
  mycobot_mtc_pick_place_demo
cd ~/ros2_ws/
rosdep install --from-paths src --ignore-src -r -y

Type in your password, and install any missing dependencies.

Now build.

colcon build && source ~/.bashrc

Don’t worry about any build errors at this stage. We will fix those later.

Create a Launch File 

Now let’s create some launch files. Here are all the launch files. You will add the code for each launch file below.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/

Create a folder named launch.

mkdir launch && cd launch

Add your first launch file: 

touch pick_place_demo.launch.py

This launch file launches the MoveIt Task Constructor node responsible for pick and place with perception.

touch point_cloud_viewer.launch.py

This launch file is created for viewing point cloud data (.pcd files) in RViz. It starts a node to convert PCD files to point clouds, allows configuration of file paths and publishing intervals, and launches RViz with a specific configuration for visualizing the point cloud data.

Save the file, and close it. 

touch get_planning_scene_server.launch.py 

This launch file starts the GetPlanningSceneServer node (we’ll go over this service in detail later in this tutorial), which is responsible for providing the current planning scene. It loads configuration parameters from a YAML file.

Save the file, and close it.

Install the Packet Capture Library

Open a terminal window, and type:

sudo apt-get install libpcap-dev

Create a Parameter File

Now let’s add some parameters.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/
mkdir config && cd config
touch mtc_node_params.yaml 

Add the code, and save.

This YAML file contains configuration parameters for the MoveIt Task Constructor node. It includes settings for robot control, object manipulation, motion planning, and various timeout and scaling factors. These parameters define the behavior of the pick and place task, including grasp generation, approach distances, and cartesian motion settings.

Save the file, and close it. 

touch get_planning_scene_server.yaml 

This configuration file sets parameters for the GetPlanningSceneServer node. It includes settings for point cloud processing, plane and object segmentation, support surface detection, and various thresholds for filtering and clustering. These parameters are for processing sensor data and creating an accurate representation of the planning scene.

Save the file, and close it. 

Add the Source Code

Below I will guide you through how I set up the source code. If you want to learn more details about the implementation of each piece of code, go to the Appendix.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/src 
touch mtc_node.cpp 

Add the code.

This file implements the main MoveIt Task Constructor (MTC) node for the pick and place task. It sets up the planning scene, creates the MTC task with various stages such as move to pick, grasp, lift, and place. The file also handles the execution of the task, coordinating the robot’s movements to complete the pick and place operation.

Save the file, and close it. 

touch cluster_extraction.cpp 

This file contains functions for extracting clusters from a point cloud using a region growing algorithm. It helps in separating different objects in the scene by grouping points that likely belong to the same object. The extracted clusters can then be processed individually for object recognition or manipulation tasks.

Save the file, and close it. 

touch get_planning_scene_client.cpp 

This file implements a test client for the GetPlanningScene service. It’s responsible for requesting the current planning scene, which includes information about objects in the environment. 

Save the file, and close it. 

touch get_planning_scene_server.cpp 

This file implements the server for the GetPlanningScene service. It processes point cloud and RGB image data to generate CollisionObjects for the MoveIt planning scene. These CollisionObjects represent the obstacles and objects in the robot’s environment, allowing for accurate motion planning and object manipulation.

Save the file, and close it. 

touch normals_curvature_and_rsd_estimation.cpp 

This file contains functions to estimate normal vectors, curvature values, and Radius-based Surface Descriptor (RSD) values for each point in a point cloud. These geometric features help in understanding the shape and orientation of surfaces in the scene. The estimated features can be used for tasks such as object recognition, segmentation, and grasp planning.

Save the file, and close it. 

touch object_segmentation.cpp 

This file does most of the heavy lifting. It implements object segmentation for the input 3D point cloud. It includes functions for fitting geometric primitives (cylinders and boxes) to point cloud data, which is used to identify and represent objects in the scene.

Save the file, and close it. 

touch plane_segmentation.cpp 

This file contains functions to segment the support plane and objects from a given point cloud. It identifies the surface on which objects are placed, separating it from the objects themselves. This segmentation is important for tasks such as determining where objects can be placed.

Save the file, and close it.

Add the Include Files

Now let’s create the header files.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/include/mycobot_mtc_pick_place_demo/
touch cluster_extraction.h 

Save the file, and close it.

touch get_planning_scene_client.h 

Save the file, and close it. 

touch normals_curvature_and_rsd_estimation.h 

Save the file, and close it. 

touch object_segmentation.h 

Save the file, and close it. 

gedit plane_segmentation.h 

Save the file, and close it.

These header files define the interfaces for the corresponding source files we created earlier. They contain function declarations, class definitions, and necessary include statements. 

Add Launch Scripts

Let’s add some launch scripts.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/ 
mkdir scripts && cd scripts
touch pointcloud.sh 

Save the file, and close it. 

touch robot.sh 

Save the file, and close it. 

chmod +x pointcloud.sh 
chmod +x robot.sh

These scripts will help you launch the necessary components for viewing point clouds and running the robot simulation with Gazebo, RViz, and MoveIt 2. 

The pointcloud.sh script is designed to launch the robot in Gazebo and then view a specific point cloud file. 

The robot.sh script launches the full setup including the robot in Gazebo, RViz for visualization, and sets up MoveIt 2 for motion planning. The chmod commands at the end make both scripts executable, allowing you to run them directly from the terminal.

Add the RViz Configuration File

Now let’s add the RViz configuration file.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/ 
mkdir rviz && cd rviz
touch point_cloud_viewer.rviz

Add the RViz configuration file.

Save the file, and close it.

Visualize the PointCloud Data in RViz

1-visualize-the-point-cloud

Test Point Cloud Data Generation in Gazebo

We haven’t built our package yet (using “colcon build”), but it is helpful to know how to test point cloud data in Gazebo once everything has been built. 

To confirm the point cloud data is being generated successfully in Gazebo, you would run these commands:

bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_gazebo_and_moveit.sh
gz topic -l

See the information about a topic:

gz topic -t <topic_name> -i

Echo a topic:

gz topic -t <topic_name> -e

You can run these commands to confirm data is being generated:

ros2 topic echo /camera_head/depth/camera_info 
ros2 topic echo /camera_head/depth/color/points

To see the frequency of publishing, type:

ros2 topic hz /camera_head/depth/camera_info 
ros2 topic hz /camera_head/depth/color/points

To get more information on the topics, type:

ros2 topic info /camera_head/color/image_raw
ros2 topic info /camera_head/depth/color/points

Go to RViz and click the Add button in the Displays panel on the left.

Click the “By topic” tab.

Click /camera_head -> /depth -> /color -> /points -> PointCloud2 so you can see the point cloud in RViz.

Click OK.

You can also see the camera image topic information.

Click /camera_head -> /color -> /image_raw -> Image 

2-see-raw-camera-image-rviz

Press CTRL + C to close everything down.

Create a ROS 2 Service Interface for Point Cloud Processing for MoveIt Planning Scene Generation

Now let’s create a ROS 2 service that processes point cloud data to generate CollisionObjects for a MoveIt planning scene. This service will segment the input point cloud, fit primitive shapes to the segments, and create corresponding CollisionObjects. The service will also provide the necessary data for subsequent grasp generation should you decide to use a grasp generation strategy other than the one we will implement in this tutorial.

Here is a description of the service on a high level:

Input (Request)

  • std::string: Target object shape (e.g., “cylinder”, “box”)
  • std::vector<double>: Approximate target object dimensions (for identification)

Output (Response)

  • moveit_msgs::msg::PlanningSceneWorld: Contains CollisionObjects for all detected objects
  • sensor_msgs::msg::PointCloud2: Full scene point cloud
  • sensor_msgs::msg::Image: RGB image of the scene 
  • std::string: ID of the target object in the PlanningSceneWorld
  • std::string: ID of the support surface in the PlanningSceneWorld
  • bool: Success flag

Create a Package

Let’s create a new package called mycobot_interfaces to store our custom service definition. This package will be used across your mycobot projects for custom message and service definitions.

Here is the full package.

Navigate to your mycobot workspace:

cd ~/ros2_ws/src/mycobot_ros2

Create the new package:

ros2 pkg create \
    --build-type ament_cmake \
    --dependencies moveit_msgs sensor_msgs rclcpp \
    --license BSD-3-Clause \
    --maintainer-name ubuntu \
    --maintainer-email automaticaddison@todo.com \
    --description "Service definitions for generating MoveIt planning scenes from point cloud data, including segmentation and primitive shape fitting for CollisionObjects" \
    mycobot_interfaces

Navigate into the new package:

cd mycobot_interfaces

Create a srv directory for our service definitions:

mkdir srv

Update the package.xml file:

gedit package.xml

Make it look like what is on Github.

Save the file, and close it.

Update the CMakeLists.txt file:

gedit CMakeLists.txt

Make it look like what is on GitHub.

Just comment out these lines for now. 

#rosidl_generate_interfaces(${PROJECT_NAME}
#  "srv/GetPlanningScene.srv"
#  DEPENDENCIES moveit_msgs sensor_msgs
#)

Save the file, and close it.

Build the package to ensure everything is set up correctly:

cd ~/ros2_ws
colcon build --packages-select mycobot_interfaces
source ~/.bashrc

Create the Custom Service Interface

Now that we have our mycobot_interfaces package set up, let’s create the custom service interface for our planning scene generation service.

Navigate to the srv directory in the mycobot_interfaces package:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_interfaces/srv

Create a new file for our service definition:

touch GetPlanningScene.srv

Add this content to define our service interface:

Save and close the file.

Uncomment these lines on the CMakeLists.txt.

#rosidl_generate_interfaces(${PROJECT_NAME}
#  "srv/GetPlanningScene.srv"
#  DEPENDENCIES moveit_msgs sensor_msgs
#)

Save the file.

cd ~/ros2_ws
colcon build --packages-select mycobot_interfaces
source ~/.bashrc

Confirm Your Custom Interface

After creating our custom service interface, it’s important to verify that it has been created correctly. Follow these steps to confirm the interface creation:

Open a new terminal.

Navigate to your workspace:

cd ~/ros2_ws

Use the ros2 interface show command to display the content of our newly created service:

ros2 interface show mycobot_interfaces/srv/GetPlanningScene

Remember, whenever you make changes to your interfaces, you need to rebuild the package and source your workspace again for the changes to take effect.

Now you have created a custom service interface for planning scene generation. This service will take a target shape and dimensions as input, and return a planning scene world, full point cloud, RGB image, target object ID, support surface ID (e.g. a table), and a success flag.

To use this service in other packages, add mycobot_interfaces as a dependency in the package.xml of the mycobot_mtc_pick_place_demo package where you want to use this service:

<depend>mycobot_interfaces</depend>

In your C++ code for the get_planning_scene_server, you would include the generated header:

#include <mycobot_interfaces/srv/get_planning_scene.hpp>

You can then create service clients or servers using this interface.

This custom service interface provides a clear contract for communication between your point cloud processing node and other nodes in your system that need planning scene information.

Edit CMakeLists.txt

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

Make sure your CMakeLists.txt looks like this.

Save the file, and close it.

Edit package.xml

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

Add this code.

Save the file, and close it.

Build the Code

Let’s build the code now. We will do this in stages due to all the developer warnings I encountered when trying to build the mycobot_mtc_pick_place_demo package the first time I did this.

First build all packages other then the mycobot_mtc_pick_place_demo package.

cd ~/ros2_ws/
colcon build --packages-skip mycobot_mtc_pick_place_demo
source ~/.bashrc 

(OR source ~/ros2_ws/install/setup.bash if you haven’t set up your bashrc file to source your ROS distribution automatically with “source ~/ros2_ws/install/setup.bash”)

Now build the mycobot_mtc_pick_place_demo package:

colcon build --packages-select mycobot_mtc_pick_place_demo --cmake-args -Wno-dev

Without the Wno-dev flag, you would see this warning:

CMake Warning (dev) at /usr/lib/x86_64-linux-gnu/cmake/pcl/Modules/FindFLANN.cmake:45 (find_package):
  Policy CMP0144 is not set: find_package uses upper-case <PACKAGENAME>_ROOT
  variables.  Run "cmake --help-policy CMP0144" for policy details.  Use the
  cmake_policy command to set the policy and suppress this warning.

  CMake variable FLANN_ROOT is set to:

    /usr

  For compatibility, find_package is ignoring the variable, but code in a
  .cmake module might still use it.
Call Stack (most recent call first):
  /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:261 (find_package)
  /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:306 (find_flann)
  /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake:570 (find_external_library)
  CMakeLists.txt:55 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

Just ignore it. Build the workspace again.

source ~/.bashrc
colcon build
source ~/.bashrc

Open a terminal window, and type the following command:

sudo sed -i 's/^\(\s*\)PCL_ERROR ("\[pcl::SampleConsensusModelPlane::isSampleGood\] Sample points too similar or collinear!\\n");/\1\/\/ PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Sample points too similar or collinear!\\n");/' $(find /usr/include/pcl* -path "*/sample_consensus/impl/sac_model_plane.hpp")

This command comments out logging by the PCL library that happens during RANSAC. It silences this annoying warning:

[pcl::SampleConsensusModelPlane::isSampleGood] Sample points too similar or collinear!

Build one more time.

colcon build && source ~/.bashrc

Launch the Code

Finally…the moment you have been waiting for. Time to launch the code.

Let’s add two aliases. Open a terminal, and type these commands:

echo "alias pointcloud='bash ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/scripts/pointcloud.sh'" >> ~/.bashrc
echo "alias pick='bash ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_pick_place_demo/scripts/robot.sh'" >> ~/.bashrc

Launch the entire demo:

pick

Ignore the message that looks like this:

[Err] [Physics.cc:1773] Attempting to create a mimic constraint for joint [gripper_base_to_gripper_left2] but the chosen physics engine does not support mimic constraints, so no constraint will be created.

You can also ignore this warning:

Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.

Here is what you would see (I disabled the Marker Array in the Displays panel):

3-pick-place-here-what-should-see

Camera title angle makes a big difference in the quality of the MoveIt planning scene. You can experiment with different values by modifying your URDF for the Intel RealSense camera.

Also, if you attempt to execute the plan, and your robot stalls as it tries to pick up the cylinder, experiment with different physics engine plugins for the SDF file for the Gazebo world.

4-gazebo-pick-place

If you want to visualize the raw 3D point cloud data, you can run the pointcloud.sh script. Make sure the files are in the appropriate location.

pointcloud

That’s it!

Deep Learning Alternatives for MoveIt Planning Scene Generation

In the service we developed to generate the collision objects for the planning scene, we fit primitive shapes to the 3D point cloud generated by our RGBD camera. We could have also generated the MoveIt planning scene using modern deep learning methods (I won’t go through this in this tutorial).

For example, you could use a package like isaac_ros_foundationpose to create 3D bounding boxes around objects in the scene and then add those boxes as collision objects. The advantage of this technique is that you have the pose of the object as well as the class information (e.g. mug, plate, bowl, etc.)

Here is what a Detection3D.msg message would look like if you were to type ‘ros2 topic echo <name_of_detection3d_topic>’:

---
header:
  stamp:
    sec: 1694627500
    nanosec: 500000000
  frame_id: kitchen_camera
results:
  - class_id: mug
    score: 0.95
  - class_id: plate
    score: 0.03
  - class_id: bowl
    score: 0.02
bbox:
  center:
    position:
      x: 0.5
      y: 1.2
      z: 0.8
    orientation:
      x: 0.0
      y: 0.0
      z: -0.7071
      w: 0.7071
  size:
    x: 0.1
    y: 0.1
    z: 0.15
id: kitchen_mug_2

Appendix: ROS 2 Service: Generating a MoveIt Planning Scene from a 3D Point Cloud 

Overview

The method I used for generating the planning scene was inspired by the following paper: 

Goron, Lucian Cosmin, et al. “Robustly segmenting cylindrical and box-like objects in cluttered scenes using depth cameras.” ROBOTIK 2012; 7th German Conference on Robotics. VDE, 2012.

If you get a chance, I highly recommend you read this entire paper. It provides a robust methodology for generating object primitives (i.e. boxes and cylinders) from a 3D point cloud scene even if the depth camera can only see the objects partially (i.e. from one side).

segmentation-process-goron-et-al
Image Source: Goron, Lucian Cosmin, et al. “Robustly segmenting cylindrical and box-like objects in cluttered scenes using depth cameras.” ROBOTIK 2012; 7th German Conference on Robotics. VDE, 2012.

Let’s go through the technical details of the ROS 2 service we created. The purpose of the service is to process point cloud and RGB image data to generate CollisionObjects for a MoveIt planning scene. I will cover how we segmented the input point cloud, fit primitive shapes to the segments, created corresponding CollisionObjects, and provided the necessary data for subsequent grasp generation.

Service Definition

Name: get_planning_scene

Input (Request)

  • std::string: Target object shape (e.g., “cylinder”, “box”)
  • std::vector<double>: Approximate target object dimensions (for identification)

Output (Response)

  • moveit_msgs::msg::PlanningSceneWorld: Contains CollisionObjects for all detected objects
  • sensor_msgs::msg::PointCloud2: Full scene point cloud
  • sensor_msgs::msg::Image: RGB image of the scene 
  • std::string: ID of the target object in the PlanningSceneWorld
  • std::string: ID of the support surface in the PlanningSceneWorld
  • bool: Success flag

Implementation Details 

Point Cloud Preprocessing

Estimate the support plane for the objects in the scene and extract the points in the point cloud that are above the support plane (plane_segmentation.cpp)

The first step of the algorithm identifies the points that make up the flat surface (like a table) that objects are sitting on.

Input
  • Point cloud (pcl::PointCloud<pcl::PointXYZRGB>)
Output
  • Point cloud for the support plane
  • Point cloud for the points above the detected support plane.
Process
  • Estimate surface normals
    • A normal is a vector perpendicular to the surface at a given point. It provides information about the local orientation of the surface
    • For each point in the cloud, estimate the normal by fitting a plane to its k-nearest neighbors. 
    • Store the computed normals, keeping them aligned with the original point cloud. 
  • Identify potential support surfaces
    • Use the computed surface normals to find approximately horizontal surfaces.
    • Group points whose normals are approximately parallel to the world Z-axis (vertical). These points likely belong to horizontal surfaces like tables.
    • Perform Euclidean clustering on these points to get support surface candidate clusters.
    • Store the support surface candidate clusters
  • For each support surface candidate cluster:
    • Use RANSAC to fit a plane model
    • Validate the plane model based on the robot’s workspace limits
      • If cropping is enabled:
        • Check if the plane is within the cropped area: 
      • If cropping is disabled:
        • Skip the position check
      • Check if the plane is close to z=0:
        • Define z_tolerance (e.g., 0.05 meters)
        • Ensure the absolute value of plane_center.z is less than z_tolerance
      • Verify the plane is approximately horizontal:
        • Define up_vector as (0, 0, 1)
        • Calculate dot_product between plane_normal and up_vector
        • Define angle_tolerance based on acceptable tilt (e.g., cos of 2.5 degrees)
        • Ensure dot_product is greater than angle_tolerance
      • If all applicable conditions are met, consider the plane model valid; otherwise, reject it
  • Select the best fitting plane as the support surface
    • From the set of validated plane models, choose the best candidate based on the following criteria:
      • Inlier count:
        • Define inlier_count for each plane model as the number of points that fit the model within a specified distance threshold 
        • Prefer plane models with higher inlier_count, as they represent surfaces with more supporting points
      • Plane size:
        • Calculate the area of each plane model by finding the 2D bounding box of inlier points projected onto the plane
        • Prefer larger planes, as they are more likely to represent the main support surface
      • Distance to z=0:
        • Calculate z_distance as the absolute distance of the plane’s center to z=0 
        • Prefer planes with smaller z_distance
      • Orientation accuracy:
        • Calculate orientation_score as the dot product between the plane’s normal and the up vector (0, 0, 1) 
        • Prefer planes with higher orientation_score (closer to being perfectly horizontal)
    • Combine these factors using a weighted scoring system:
      • Define weights for each factor (e.g., w_inliers, w_size, w_distance, w_orientation) 
      • Calculate a total_score for each plane model 
      • Select the plane model with the highest total_score as the best fitting plane
    • Store the selected best_plane_model for further use in object segmentation
  • Return the results:
    • Create support_plane_cloud:
  • Extract all inlier points from the original point cloud that belong to the best_plane_model
  • Store these points in support_plane_cloud 
  • Create objects_cloud:
    • For each point in the original point cloud:
      • If the point is above the best_plane_model (use the plane equation to check)
      • And if cropping is enabled, the point is within the crop boundaries
      • Add the point to objects_cloud
  • Return both support_plane_cloud and objects_cloud

References:

  • R. Mario and V. Markus, “Grasping of Unknown Objects from a Table Top,” in Workshop on Vision in Action: Efficient strategies for cognitive agents in complex environments, 2008.
  • R. B. Rusu, N. Blodow, Z. C. Marton, and M. Beetz, “Close-range Scene Segmentation and Reconstruction of 3D Point Cloud Maps for Mobile Manipulation in Human Environments,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), St. Louis, MO, USA, October 2009.

Estimate normal vectors, curvature values, and radius descriptor values for each point in the point cloud (normals_curvature_and_rsd_estimation.cpp)

Input
  • Point cloud (of type pcl::PointCloudpcl::PointXYZRGB)
Output
  • Point Cloud with Normal Vectors, Curvature Values, and Radius Descriptor Values
    • Each point has an associated 3D vector representing its normal vector (a 3D vector indicating the direction the surface is facing at that point)
    • Each point also has an additional value representing how curved the surface is at that point.
    • Each point has Radius-based Surface Descriptor (RSD) values (the minimum and maximum surface radius that can be fitted to the point’s neighborhood). This step determines whether a point belongs to a linear or circular surface.
Process

This approach provides a method for handling boundary points and refining normal estimation using MaximumLikelihoodSampleConsensus (MLESAC). It estimates normals, curvature, and RSD values for every point, using a more conservative approach for points identified as potentially being on a boundary.

For each point p in the cloud:

  1. Find neighbors:
    • Identify the k nearest neighboring points around p (k_neighbors as input parameter)
    • These k points form the “neighborhood” of p.
    • Check if the point is a boundary point by seeing if it has fewer than k_neighbors neighbors.
    • For boundary points, adjust the neighborhood size to use a smaller number of neighbors (up to min_boundary_neighbors or whatever is available).
  2. Estimate the normal vector:
    • Perform initial Principal Component Analysis (PCA) on the neighborhood:
      1. This captures how the neighborhood points are spread out around point p.
      2. The eigenvector corresponding to the smallest eigenvalue is an initial normal estimate.
    • Use Maximum Likelihood Estimation SAmple Consensus (MLESAC) to refine the local plane estimate:
      1. MLESAC aims to refine the plane fitting by robustly estimating the best plane model and discarding outliers.
      2. It uses the initial normal estimate as a starting point.
      3. If MLESAC fails, fall back to the initial normal estimate.
    • Compute a weighted covariance matrix:
      1. Assign weights to neighbor points based on their distance to the estimated local plane
      2. Points closer to the plane and inliers from MLESAC get higher weights
      3. This step helps to reduce the influence of outliers
    • Perform PCA on this weighted covariance matrix:
      1. Compute eigenvectors and eigenvalues
      2. The eigenvector corresponding to the smallest eigenvalue is the final normal estimate
  3. Compute curvature values:
    • Use the eigenvalues from PCA on the weighted covariance matrix to calculate the curvature values according to the following formula: curvature = λ₀ / (λ₀ + λ₁ + λ₂) where λ₀ ≤ λ₁ ≤ λ₂
  4. Calculate Radius-based Surface Descriptor (RSD) values:
    • Use pcl::RSDEstimation to compute the minimum and maximum surface radius that can be fitted to the point’s neighborhood.
    • This step determines whether a point belongs to a linear or circular surface.
  5. Output creation: Point cloud that includes:
    • The original XYZ coordinates
    • The RGB color information
    • The estimated normal vector for each point
    • The estimated curvature value for each point
    • The RSD values (r_min and r_max) for each point
Key Parameters
  • k_neighbors: Number of nearest neighbors to consider
  • max_plane_error: Maximum allowed error for plane fitting in MLESAC
  • max_iterations: Maximum number of iterations for MLESAC
  • min_boundary_neighbors: Minimum number of neighbors to consider for boundary points
  • rsd_radius: Radius to use for RSD estimation
References
  1. R. B. Rusu, Z. C. Marton, N. Blodow, M. Dolha, and M. Beetz, “Towards 3D Point Cloud Based Object Maps for Household Environments,” Robotics and Autonomous Systems Journal (Special Issue on Semantic Knowledge in Robotics), vol. 56, no. 11, pp. 927–941, 30 November 2008.
  2. Z. C. Marton, D. Pangercic, N. Blodow, and M. Beetz, “Combined 2D-3D Categorization and Classification for Multimodal Perception Systems,” International Journal of Robotics Research, 2011.

Cluster the point cloud into connected components using region growing based on nearest neighbors – cluster_extraction.cpp

Input
  • Point cloud (of type pcl::PointCloud<PointXYZRGBNormalRSD>::Ptr) – Point cloud with XYZ, RGB, normal vectors, curvature values, and Radius-based Surface Descriptor (RSD) values
Output
  • Vector of point cloud clusters, where each cluster is a separate point cloud containing:
    • Points that are close to each other in 3D space.
    • Each point retains its original attributes (xyz, RGB, normal, curvature, RSD values)
Process
  1. Create a KdTree object for efficient nearest neighbor searches
  2. Extract normals from the input cloud into a separate pcl::PointCloudpcl::Normal object
  3. Create a RegionGrowing object and set its parameters:
    • Minimum and maximum cluster size
    • Number of nearest neighbors to consider
    • Smoothness threshold (angle threshold for neighboring normals)
    • Curvature threshold
  4. Apply the region growing algorithm to extract clusters
  5. For each extracted cluster:
    • Create a new point cloud
    • Copy points from the input cloud to the new cluster cloud based on the extracted indices
    • Set the cluster’s width, height, and is_dense properties
Key Points
  • Purpose: Reduce the search space for subsequent segmentation by grouping nearby points
  • Note: In cluttered scenes, objects that are touching or very close may end up in the same cluster
  • This step does not perform final object segmentation, but prepares data for later segmentation steps
  • The algorithm uses both geometric properties (normals) and curvature information for clustering
  • Smoothness threshold is converted from degrees to radians in the implementation
Parameters
  • min_cluster_size: Minimum number of points that a cluster needs to contain
  • max_cluster_size: Maximum number of points that a cluster can contain
  • smoothness_threshold: Maximum angle difference between normals (in degrees, converted to radians internally)
  • curvature_threshold: Maximum difference in curvature between neighboring points
  • nearest_neighbors: Number of nearest neighbors to consider for region growing
Additional Features
  • The implementation calculates and logs various statistics for each cluster:
    • Cluster size
    • Centroid coordinates
    • Average RGB color
    • Curvature statistics (min, 20th percentile, average, median, 80th percentile, max)
    • Average RSD (Radius-based Surface Descriptor) values
References
  • R. B. Rusu, N. Blodow, Z. C. Marton, and M. Beetz, “Close-range Scene Segmentation and Reconstruction of 3D Point Cloud Maps for Mobile Manipulation in Human Environments,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), St. Louis, MO, USA, October 2009.

Segmentation of Clutter – object_segmentation.cpp

The input into this step is the vector of point cloud clusters that was generated during the previous step.

Input
  • Vector of point cloud clusters, where each cluster is a separate point cloud containing:
    • Points that are close to each other in 3D space.
    • Each point retains its original attributes (xyz, RGB, normal, curvature, RSD values)
Output
  • moveit_msgs::CollisionObject[] collision_objects
Outer Loop

The entire process below occurs for each point cloud cluster in the input vector.

Process
Project the Point Cloud Cluster Onto the Surface Plane
  1. For each point (x,y,z,RGB,normal, curvature, RSDmin and RSDmax) in the 3D cluster:
    • Project the point onto the surface plane (assumed to be z=0). This creates a point (x, y).
    • Maintain a mapping between each 2D projected point and its original 3D point
Initialize Two Separate Parameter Spaces
  1. Create a vector to store line models for the Hough transform
    • Each line model stores rho, theta, votes, and inlier count
  2. Create a 3D Hough parameter space for circle models
    • Dimensions: center_x, center_y, radius
Understanding Hough Space

Hough space is a parameter space used for detecting specific shapes or models. Each point in Hough space represents a possible instance of the shape you’re looking for (in this case, circles or lines).

For lines:

For circles:

  • The Hough space is 3D: (center_x, center_y, radius)
  • Each point in this space represents a possible circle in the original image

The voting process in Hough space helps find shapes even if they’re partially hidden or broken in the original image.

Inner Loop (repeated num_iterations times)

RANSAC Model Fitting

  1. Line Fitting
    • Use RANSAC to fit a 2D line to the projected points. This is done to identify potential box-like objects.
  2. Circle Fitting
    • Use RANSAC to fit a 2D circle to the projected points. This is done to identify cylindrical-like objects.

Filter Inliers

For the fitted models from the RANSAC Model Fitting step, apply a series of filters to refine the corresponding set of inlier points.

Circle Filtering

  1. Euclidean Clustering
    • Use pcl::EuclideanClusterExtraction to group inliers into clusters.
    • Accept models with a maximum of two clusters (representing complete circles or two visible arcs of the same cylinder).
    • Reject models with more than the specified maximum number of clusters.
  2. Height Consistency (for two clusters only)
    • For models with exactly two clusters, check if the height difference between clusters is within the specified tolerance.
    • This ensures that the fitted circle represents a cross-section of a single, upright cylindrical object.
  3. Curvature Filtering
    • Keep inlier points with high curvature (above the specified threshold).
    • Remove inlier points with low curvature.
  4. Radius-based Surface Descriptor (RSD) Filtering
    • Compare the minimum surface radius (r_min) of each inlier point to the radius of the fitted circle.
    • Keep points where the difference is within the specified tolerance.
  5. Surface Normal Filtering
    • Calculate the angle between the point’s normal (projected onto the xy-plane) and the vector from the circle center to the point.
    • Keep points where this angle is within the specified threshold.

Line Filtering

  1. Euclidean Clustering
    • Use pcl::EuclideanClusterExtraction to group inliers into clusters.
    • Accept models with only one cluster.
    • Reject models with more than the specified maximum number of clusters.
  2. Curvature Filtering
    • Keep inlier points with low curvature (below the specified threshold).
    • Remove inlier points with high curvature.

Model Validation

For both circle and line models, check how many inlier points remain after filtering.

  • If the number of remaining inliers for the model exceeds the threshold:
    • The model is considered valid.
    • Add the model to the appropriate Hough parameter space.
  • If the number of remaining inliers is below the threshold:
    • The model is rejected.

An additional validation step compares inlier counts between circle and line models, keeping only the model type with more inliers.

Add Model to the Hough Space

If a model is valid:

  • For circles:
    • Add a vote to the 3D Hough space (center_x, center_y, radius bins)
  • For lines:
    • Add the line model (rho, theta, votes, inlier count) to the line models vector
    • rho is the perpendicular distance from the origin (0, 0) to the line
    • theta is the angle formed between the x-axis and this perpendicular vector (positive theta is counter-clockwise measured from x-axis)

Remove inliers and continue

Remove the inliers of valid models from the working point cloud and continue the inner loop until insufficient points remain or no valid models are found.

Cluster Parameter Spaces

After all iterations on a point cloud cluster:

  1. Cluster line models based on similarity in rho and theta.
  2. Cluster circle models in the 3D Hough space.
Select Model with Most Votes

Compare the top line cluster with the top circle cluster. Select the model type (line or circle) with the highest vote count.

Estimate 3D Shape

Using the parameters from the highest-vote cluster, fit the selected solid geometric primitive model type (box or cylinder) to the original 3D point cloud data.

Cylinder Fitting

  1. Use the 2D circle fit for radius and (x,y) center position.
  2. Set cylinder bottom at z=0.
  3. Set top height to the highest point in the cluster.
  4. Calculate dimensions and position of the cylinder.

Box Fitting

  1. Compute box orientation from the line angle.
  2. Project points onto the line direction and perpendicular direction to determine length and width.
  3. Use z-values of points to determine height.
  4. Calculate dimensions and position of the box.
Add Shape as Collision Object

The box or cylinder is added as a collision object (moveit_msgs::CollisionObject) to the planning scene with a unique id (e.g. box_0, box_1, cylinder_0, cylinder_1, etc).

Move to Next Cluster

Proceed to the next point cloud cluster in the vector (i.e., move to the next iteration of the Outer Loop).

Key Parameters
  • num_iterations: Number of RANSAC iterations per cluster
  • inlier_threshold: Minimum number of inliers for a model to be considered valid
  • ransac_distance_threshold: Maximum distance for a point to be considered an inlier in RANSAC
  • ransac_max_iterations: Maximum number of iterations for RANSAC
  • circle_min_cluster_size, line_min_cluster_size: Minimum size for Euclidean clusters
  • circle_max_clusters, line_max_clusters: Maximum number of allowed clusters
  • circle_height_tolerance: Maximum allowed height difference between two circle clusters
  • circle_curvature_threshold, line_curvature_threshold: Curvature thresholds for filtering
  • circle_radius_tolerance: Tolerance for RSD filtering
  • circle_normal_angle_threshold: Maximum angle between normal and radial vector for circles
  • circle_cluster_tolerance, line_cluster_tolerance: Distance threshold for Euclidean clustering
  • line_rho_threshold, line_theta_threshold: Thresholds for clustering line models in the parameter space

Get Planning Scene Server (get_planning_scene_server.cpp)

This code brings together all the previously developed components into a single, unified ROS 2 service. It integrates functions for point cloud processing, plane segmentation, object clustering, and shape fitting into a cohesive workflow. The service processes point cloud and RGB image data to generate a MoveIt planning scene, which can be called by a node using the MoveIt Task Constructor to obtain environment information for manipulation tasks. The core functionality is encapsulated in the handleService method of the GetPlanningSceneServer class.

handleService Method Walkthrough

  1. Initialize Response: The method starts by setting the success flag of the response to false.
  2. Check Data Availability: It verifies that both point cloud and RGB image data are available. If either is missing, it logs an error and returns.
  3. Validate Input Parameters and Prepare Point Cloud:
    • Checks if target_shape and target_dimensions are valid
    • Transforms the point cloud to the target frame
    • Applies optional cropping to the point cloud
  4. Convert PointCloud2 to PCL Point Cloud: Converts the ROS PointCloud2 message to a PCL point cloud for further processing.
  5. Segment Support Plane and Objects: Uses the segmentPlaneAndObjects function to separate the support surface from objects in the scene.
  6. Create CollisionObject for Support Surface: Generates a CollisionObject representing the support surface and adds it to the planning scene.
  7. Estimate Normals, Curvature, and RSD: Calls estimateNormalsCurvatureAndRSD to calculate geometric features for each point in the object cloud.
  8. Extract Clusters: Uses extractClusters to identify distinct object clusters in the point cloud.
  9. Get Collision Objects: Calls segmentObjects to convert point cloud clusters into collision objects for the planning scene.
  10. Identify Target Object: Searches through the collision objects to find one matching the requested target shape and dimensions.
  11. Assemble PlanningSceneWorld: Combines all collision objects into a complete PlanningSceneWorld structure.
  12. Fill the Response:
    • Sets the full point cloud, RGB image, target object ID, and support surface ID in the response
    • Sets the success flag to true if all critical steps were successful
    • Logs detailed information about the response contents

This method transforms raw sensor data into a structured planning scene that can be used by the MoveIt Task Constructor for motion planning and manipulation tasks.

Congratulations on reaching the end! Keep building!