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!

Reusing Motion Plans – ROS 2 Jazzy MoveIt Task Constructor

In this tutorial, we’ll explore how to create reusable motion plans for robotic arms using the MoveIt Task Constructor. We’ll build an application from scratch that demonstrates how to define a series of modular movements that can be combined and reused. This approach allows for more flexible and maintainable robot motion planning, especially useful in scenarios where similar motion sequences are repeated or slightly modified.

Here is what you will develop:

modular-moveit-task-constructor

Our application will showcase:

  1. Creation of a reusable module containing a sequence of movements
  2. Combining multiple instances of this module into a larger task
  3. Use of both Cartesian and joint space planning
  4. Integration with ROS 2 and logging of the planning process

By the end of this tutorial, you’ll have a deep understanding of how to structure complex motion plans using the MoveIt Task Constructor, making your robotics applications more modular and easier to maintain.

Here’s a high-level overview of what our program will do:

  1. Define a reusable module that includes:
    • Moving 5 cm in the positive X direction
    • Moving 2 cm in the negative Y direction
    • Rotating -18 degrees around the Z axis
    • Moving to a predefined “ready” position
  2. Create a main task that:
    • Starts from the current state
    • Moves to the “ready” position
    • Executes the reusable module five times in succession
    • Finishes by moving to the “home” position
  3. Plan and execute the task, providing detailed feedback on each stage

Real-World Use Cases

The reusable motion planning approach for robotic arms that you’ll develop in this tutorial has several practical applications:

  • Manufacturing and Assembly
    • Create modular motion sequences for pick-and-place tasks or component assembly
    • Optimize arm movements for repetitive operations, reducing cycle times (Cycle time is the total time it takes to complete one full operation, from start to finish)
  • Bin Picking and Sorting
    • Develop flexible routines for grabbing objects from bins with varying contents
    • Combine basic movement modules to handle different object shapes and orientations
  • Welding and Surface Treatment
    • Build libraries of arm motions for welding or spray painting different part shapes

By mastering these techniques, you’ll be able to create more flexible and efficient robotic arm systems. This modular approach allows you to more efficiently develop and adapt arm motions for various industries.

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 the Code

If you don’t already have modular.cpp, open a new terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/src/
gedit modular.cpp

Add this code

/**
 * @file modular.cpp
 * @brief Demonstrates the use of MoveIt Task Constructor for robot motion planning.
 *
 * This program creates a reusable task for a robot arm using MoveIt Task Constructor.
 * It defines a series of movements including Cartesian paths and joint space motions.
 *
 * Key Concept:
 *   SerialContainer: This is a type of container in MoveIt Task Constructor that holds
 *     multiple movement stages. These stages are executed in sequence, one after another.
 *     Think of it like a to-do list for the robot, where each item must be completed
 *     before moving on to the next one.
 *
 * @author Addison Sears-Collins
 * @date December 19, 2024
 */

// Include necessary headers
#include <rclcpp/rclcpp.hpp>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/container.h>
#include <moveit/planning_scene/planning_scene.h>

// Use the moveit::task_constructor namespace for convenience
using namespace moveit::task_constructor;

/**
 * @brief Creates a reusable module for robot movement.
 *
 * @param group The name of the robot group to move.
 * @return std::unique_ptr<SerialContainer> A container with a series of movement stages.
 */
std::unique_ptr<SerialContainer> createModule(const std::string& group) {
  // Create a new SerialContainer to hold our movement stages
  auto c = std::make_unique<SerialContainer>("Cartesian Path");
  c->setProperty("group", group);

  RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Creating module for group: %s", group.c_str());

  // Create solvers for Cartesian and joint space planning
  auto cartesian = std::make_shared<solvers::CartesianPath>();
  auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();

  // Stage 1: Move 5 cm in the positive X direction
  {
    auto stage = std::make_unique<stages::MoveRelative>("x +0.05", cartesian);
    stage->properties().configureInitFrom(Stage::PARENT, { "group" });
    geometry_msgs::msg::Vector3Stamped direction;
    direction.header.frame_id = "base_link";
    direction.vector.x = 0.05;
    stage->setDirection(direction);
    c->insert(std::move(stage));
    RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Added stage: Move 5 cm in +X direction");
  }

  // Stage 2: Move 2 cm in the negative Y direction
  {
    auto stage = std::make_unique<stages::MoveRelative>("y -0.02", cartesian);
    stage->properties().configureInitFrom(Stage::PARENT);
    geometry_msgs::msg::Vector3Stamped direction;
    direction.header.frame_id = "base_link";
    direction.vector.y = -0.02;
    stage->setDirection(direction);
    c->insert(std::move(stage));
    RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Added stage: Move 2 cm in -Y direction");
  }

  // Stage 3: Rotate -18 degrees around the Z axis
  {
    auto stage = std::make_unique<stages::MoveRelative>("rz -18°", cartesian);
    stage->properties().configureInitFrom(Stage::PARENT);
    geometry_msgs::msg::TwistStamped twist;
    twist.header.frame_id = "base_link";
    twist.twist.angular.z = -M_PI / 10.; // 18 degrees in radians
    stage->setDirection(twist);
    c->insert(std::move(stage));
    RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Added stage: Rotate -18 degrees around Z axis");
  }

  // Stage 4: Move to the "ready" position
  {
    auto stage = std::make_unique<stages::MoveTo>("moveTo ready", joint_interpolation);
    stage->properties().configureInitFrom(Stage::PARENT);
    stage->setGoal("ready");
    c->insert(std::move(stage));
    RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Added stage: Move to 'ready' position");
  }

  RCLCPP_INFO(rclcpp::get_logger("modular_demo"), "Module creation completed with 4 stages");
  return c;
}

/**
 * @brief Creates the main task for robot movement.
 *
 * @param node The ROS2 node to use for loading the robot model.
 * @return Task The complete task for robot movement.
 */
Task createTask(const rclcpp::Node::SharedPtr& node) {
  Task t;
  t.loadRobotModel(node);
  t.stages()->setName("Reusable Containers");

  RCLCPP_INFO(node->get_logger(), "Creating task: %s", t.stages()->name().c_str());

  // Add the current state as the starting point
  t.add(std::make_unique<stages::CurrentState>("current"));
  RCLCPP_INFO(node->get_logger(), "Added current state as starting point");

  // Define the robot group to move
  const std::string group = "arm";

  // Add a stage to move to the "ready" position
  {
    auto stage = std::make_unique<stages::MoveTo>("move to ready", std::make_shared<solvers::JointInterpolationPlanner>());
    stage->setGroup(group);
    stage->setGoal("ready");
    t.add(std::move(stage));
    RCLCPP_INFO(node->get_logger(), "Added stage: Move to 'ready' position");
  }

  // Add five instances of our reusable module
  // This creates a sequence of movements that the robot will perform,
  // repeating the same set of actions five times in a row.
  RCLCPP_INFO(node->get_logger(), "Adding 5 instances of the reusable module");
  for (int i = 1; i <= 5; ++i) {
    t.add(createModule(group));
    RCLCPP_INFO(node->get_logger(), "Added module instance %d", i);
  }

  // Add a stage to move to the "home" position
  {
    auto stage = std::make_unique<stages::MoveTo>("move to home", std::make_shared<solvers::JointInterpolationPlanner>());
    stage->setGroup(group);
    stage->setGoal("home");
    t.add(std::move(stage));
    RCLCPP_INFO(node->get_logger(), "Added stage: Move to 'home' position");
  }

  RCLCPP_INFO(node->get_logger(), "Task creation completed with 5 module instances");
  return t;
}

/**
 * @brief Main function to set up and execute the robot task.
 *
 * @param argc Number of command-line arguments.
 * @param argv Array of command-line arguments.
 * @return int Exit status of the program.
 */
int main(int argc, char** argv) {
  // Initialize ROS2
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("modular_demo");
  auto logger = node->get_logger();

  RCLCPP_INFO(logger, "Starting modular demo");

  // Start a separate thread for ROS2 spinning
  std::thread spinning_thread([node] { rclcpp::spin(node); });

  // Create and plan the task
  auto task = createTask(node);
  try {
    RCLCPP_INFO(logger, "Starting task planning");

    // Plan the task
    moveit::core::MoveItErrorCode error_code = task.plan();

    // Log the planning result
    if (error_code == moveit::core::MoveItErrorCode::SUCCESS) {
      RCLCPP_INFO(logger, "Task planning completed successfully");
      RCLCPP_INFO(logger, "Found %zu solutions", task.numSolutions());

      // Use printState to log the task state
      std::ostringstream state_stream;
      task.printState(state_stream);
      RCLCPP_INFO(logger, "Task state:\n%s", state_stream.str().c_str());

      // If planning succeeds, publish the solution
      task.introspection().publishSolution(*task.solutions().front());
      RCLCPP_INFO(logger, "Published solution");
    } else {
      RCLCPP_ERROR(logger, "Task planning failed with error code: %d", error_code.val);

      // Use explainFailure to log the reason for failure
      std::ostringstream failure_stream;
      task.explainFailure(failure_stream);
      RCLCPP_ERROR(logger, "Failure explanation:\n%s", failure_stream.str().c_str());
    }

    // Log a simple summary of each stage
    RCLCPP_INFO(logger, "Stage summary:");
    for (size_t i = 0; i < task.stages()->numChildren(); ++i) {
      const auto* stage = task.stages()->operator[](i);
      RCLCPP_INFO(logger, "  %s: %zu solutions, %zu failures",
                  stage->name().c_str(), stage->solutions().size(), stage->failures().size());
    }

  } catch (const InitStageException& ex) {
    RCLCPP_ERROR(logger, "InitStageException caught during task planning: %s", ex.what());
    std::ostringstream oss;
    oss << task;
    RCLCPP_ERROR(logger, "Task details:\n%s", oss.str().c_str());
  }

  RCLCPP_INFO(logger, "Modular demo completed");

  // Wait for the spinning thread to finish
  spinning_thread.join();

  return 0;
}

Save the file, and close it.

Build the Code

cd ~/ros2_ws/
colcon build
source ~/.bashrc

Launch

Launch everything:

bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_mtc_demos.sh modular

OR

mtc_demos modular

Here is what you should see:

modular-moveit-task-constructor

Understanding the Motion Planning Results

RViz – “Motion Planning Tasks” Panel

The “Motion Planning Tasks” panel in RViz provides a detailed breakdown of our reusable motion planning task. It presents a hierarchical view with “Motion Planning Tasks” at the root, followed by “Reusable Containers”.

2-motion-planning-tasks-panel

Under “Reusable Containers“, we can see the following stages:

  1. current“: This represents the initial state of the robot.
  2. move to ready“: The first movement to get the robot into a ready position.
  3. Five “Cartesian Path” stages: These correspond to our reusable module, each containing:
    • “x +0.05”: Moving 5cm in the positive X direction
    • “y -0.02”: Moving 2cm in the negative Y direction
    • “rz -18°”: Rotating -18 degrees around the Z axis
    • “moveTo ready”: Returning to the ready position
  4. move to home“: The final movement to return the robot to its home position.

The second column shows green checkmarks and the number “1” for each stage, indicating that every step of the plan was successfully computed with one solution.

The “time” column displays the computational time for each component. We can see that the entire “Reusable Containers” task took 0.0383 seconds to compute, with individual stages taking milliseconds.

The “cost” column in this context represents a metric used by the motion planner. For most stages, it’s a very small value (0.0004 to 0.0017), meaning these movements are considered efficient or low-cost by the planner.

The “#” column consistently shows “1”, indicating that each stage has one solution.

The yellow highlighting on the “move to home” stage indicates that this is the currently selected or focused stage in the RViz interface.

This breakdown allows us to verify that our reusable module is indeed being repeated five times as intended, and that the overall motion plan is structured correctly with initial and final movements to ready and home positions.

Terminal Window – Planning Results

If you look at the terminal window, you’ll see the detailed planning results. Let’s interpret these outputs.

MoveIt Task Constructor uses a hierarchical planning approach. This means it breaks down the overall task into smaller, manageable stages and plans each stage individually while considering the connections between them.

  • Stage Creation: The terminal output shows each stage being added to the task, including the creation of the reusable module and its five instances.
  • Planning Process: After all stages are added, the planning process begins.

Arrow Interpretation in the Task State:

  • → (Right Arrow): Represents the forward flow of results from one stage to the next. This means that a stage has successfully generated a result, and it is passing that result to the next stage for further processing.
  • ← (Left Arrow): Indicates a backward flow of results. In MTC, some stages may require feedback from later stages to adjust their own results or to optimize the plan.
  • – (Dash): A dash indicates no information flowed in that direction.

Let’s analyze the task state output:

  1. The root “Reusable Containers” stage shows 1 – ← 1 → – 1, indicating one solution was found and propagated both forward and backward.
  2. For each stage, we see a pattern like this: – 0 → 1 → – 0 or – 0 → 1 → – 1
    • The first “0” means no solutions were propagated backward to this stage.
    • The “1” in the middle indicates one solution was found for this stage.
    • The last number (0 or 1) shows whether this solution was propagated forward to the next stage.
  3. The “Cartesian Path” stages, representing our reusable module, each show – 1 → 1 → – 1, meaning they received a solution from the previous stage, found their own solution, and passed it to the next stage.
  4. The individual movement stages (x +0.05, y -0.02, rz -18°) within each Cartesian Path show – 0 → 1 → – 0, indicating they found a solution but didn’t need to propagate it directly.
  5. The “moveTo ready” stages at the end of each Cartesian Path show – 0 → 1 → – 1, meaning they found a solution and passed it forward to the next module or final stage.

These results demonstrate that our planner effectively generated solutions for each stage of the task, including five repetitions of our reusable module. The hierarchical structure allowed the planner to solve each small part of the problem independently while maintaining the overall sequence of movements.

The Stage summary at the end confirms that each major stage (current, move to ready, five Cartesian Paths, and move to home) found one solution with no failures. This indicates a successful planning process for our entire reusable motion sequence.

5-published-solution

By examining these results, we can see how the modular approach allows for efficient planning of complex, repetitive tasks. Each instance of the reusable module is planned independently, but within the context of the overall task, ensuring a cohesive and executable motion plan for the robot arm.

Analysis of the Results

Let’s break down what we did and what we learned from this project.

Our Modular Approach

We created a reusable module consisting of four stages:

  1. Move 5 cm in +X direction
  2. Move 2 cm in -Y direction
  3. Rotate -18 degrees around Z axis
  4. Move to ‘ready’ position

This module was then repeated five times in our overall task, bookended by initial and final movements.

The Results: A Stage-by-Stage Breakdown

Looking at our terminal output and RViz Motion Planning Tasks panel, here’s what we observed:

Task Creation:

  • Successfully added all stages, including five instances of our reusable module
  • Each module creation was completed with 4 stages as designed

Planning Process:

  • The task planning completed successfully
  • Found 1 solution for the entire task

Detailed Task State:

  1. Root “Reusable Containers”: 1 – ← 1 → – 1
    • Indicates one solution was found and propagated both ways
  2. Individual Stages:
    • “current” and “move to ready”: – 0 → 1 → – 1
      • Successfully found a solution and passed it forward
    • Cartesian Path (reusable module): – 1 → 1 → – 1
      • Received a solution, found its own, and passed it forward
    • Individual movements (x, y, rz): – 0 → 1 → – 0
      • Found solutions but didn’t need to propagate directly
    • “moveTo ready” within modules: – 0 → 1 → – 1
      • Found a solution and passed it to the next stage
  3. Final “move to home”: – 0 → 1 → – 1
    • Successfully planned the final movement

Stage Summary

  • All stages (current, move to ready, five Cartesian Paths, move to home) found 1 solution with 0 failures.

The Big Picture

This experiment demonstrates several key advantages of our modular approach:

  1. Reusability: We successfully created a module that could be repeated multiple times within the larger task. This showcases the power of modular design in robotic motion planning.
  2. Efficiency: Each instance of our reusable module was planned independently, yet within the context of the overall task. This allows for efficient planning of complex, repetitive tasks.
  3. Robustness: The successful planning of all stages with no failures indicates that our modular approach is robust and can handle multiple repetitions of the same movement sequence.
  4. Flexibility: By breaking down the task into smaller, reusable components, we create a system that isadaptable. New movements or sequences can be added or modified without redesigning the entire task.
  5. Scalability: The ability to repeat our module five times without issues suggests that this approach could scale to even more complex sequences of movements.

By structuring our motion planning this way, we achieve a balance of simplicity and power. The reusable modules allow for faster development of complex tasks, while the hierarchical planning ensures that each part fits smoothly into the whole. 

Detailed Code Walkthrough

Now for the C++ part. Let’s go through each piece of this code, step by step.

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/
gedit modular.cpp

File Header and Includes

The code begins with a comprehensive comment block outlining the file’s purpose: demonstrating the use of MoveIt Task Constructor for robot motion planning. It introduces the key concept of SerialContainer, which is used to create reusable modules of movement stages. The file includes necessary headers for ROS 2, MoveIt, and the Task Constructor library, establishing the foundation for our modular motion planning demo.

createModule Function

This function creates a reusable module for robot movement:    

It sets up a SerialContainer named “Cartesian Path” and configures it with four stages:

  1. Move 5 cm in the positive X direction
  2. Move 2 cm in the negative Y direction
  3. Rotate -18 degrees around the Z axis
  4. Move to the “ready” position

Each stage is created using either stages::MoveRelative or stages::MoveTo, configured with the appropriate movement parameters, and added to the container.

createTask Function

This function creates the main task for robot movement:

It sets up the task with the following structure:

  • Add the current state as the starting point
  • Move to the “ready” position
  • Add five instances of the reusable module created by createModule
  • Move to the “home” position

This structure creates a sequence of movements that the robot will perform, repeating the same set of actions five times in a row.

Main Function

The main function orchestrates the entire demo.

ROS 2 Initialization and Node Setup

ROS 2 is initialized, and a node named “modular_demo” is created.

Spinning Thread

A separate thread is created to handle ROS 2 callbacks, allowing the node to process incoming messages and services.

Task Creation and Execution

The task is created using the createTask function. The code then attempts to plan the task.

Result Handling and Logging

The code includes comprehensive logging of the planning results, including the number of solutions found, the task state, and a summary of each stage’s performance.

Error Handling

The code includes error handling to catch and report any exceptions that occur during the planning process, including detailed task information in case of failure.

Completion

The program waits for the ROS 2 spinning thread to finish before exiting.

That’s it. Keep building!

Inverse Kinematics – ROS 2 Jazzy MoveIt Task Constructor

In this tutorial, we’ll explore how to implement inverse kinematics (IK) with clearance cost optimization using the MoveIt Task Constructor. We’ll create an application from scratch that demonstrates how to plan movements for a robotic arm while considering obstacle clearance. The output of your application will provide detailed insights into the planning process, including the number of solutions found and the performance of each stage.

Here is what your final output will look like (I am flipping back and forth between the two successfully found inverse kinematics solutions):

inverse-kinematics-solver-moveit-task-constructor

On a high level, your program will demonstrate a sophisticated approach to motion planning that does the following:

  1. Sets up a scene with the mycobot_280 robot and a spherical obstacle
  2. Defines a target pose for the robot’s gripper (end-effector)
  3. Uses the ComputeIK stage to find valid arm configurations reaching the target
  4. Applies a clearance cost term to favor solutions that keep the robot farther from obstacles
  5. Uses ROS 2 parameters to control the behavior of the clearance cost calculation

While OMPL and Pilz are motion planners that generate full trajectories, they rely on IK solutions like those computed in this code to determine feasible goal configurations for the robot. In a complete motion planning pipeline, this IK solver would typically be used to generate goal states, which OMPL or Pilz would then use to plan full, collision-free paths from the robot’s current position to the desired end-effector pose.

Real-World Use Cases

The code you will develop in this tutorial can serve as a foundation for various practical applications:

  • Robotic Assembly in Cluttered Environments
    • Generate arm configurations that avoid collisions with nearby parts or fixtures
    • Optimize for paths that maintain maximum clearance from obstacles
  • Bin Picking and Sorting
    • Plan motions that safely navigate around the edges of bins and other items
    • Minimize the risk of collisions in tight spaces
  • Collaborative Robot Operations
    • Ensure the robot maintains a safe distance from human work areas
    • Dynamically adjust paths based on changing obstacle positions
  • Quality Inspection Tasks
    • Generate smooth, collision-free paths for sensors or cameras to inspect parts
    • Optimize for viewpoints that balance clearance and inspection requirements

By the end of this tutorial, you’ll have a solid understanding of how to implement IK solutions with clearance cost optimization in your motion planning tasks. This approach will make your robotic applications more robust, efficient, and capable of operating safely in complex environments.

Let’s dive into the code and explore how to build this advanced motion planning application!

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 the Code

If you don’t already have ik_clearance_cost.cpp, open a new terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/src/
gedit ik_clearance_cost.cpp

Add this code

/**
 * @file ik_clearance_cost.cpp
 * @brief Demonstrates using MoveIt Task Constructor for motion planning with collision avoidance.
 *
 * This program sets up a motion planning task for a mycobot_280 robot arm using MoveIt Task Constructor.
 * It creates a scene with an obstacle, computes inverse kinematics (IK) solutions, and plans a motion
 * while considering clearance from obstacles.
 *
 * @author Addison Sears-Collins
 * @date December 19, 2024
 */

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/cost_terms.h>
#include "ik_clearance_cost_parameters.hpp"

// Use the moveit::task_constructor namespace for convenience
using namespace moveit::task_constructor;

/* ComputeIK(FixedState) */
int main(int argc, char** argv) {
  // Initialize ROS 2
  rclcpp::init(argc, argv);

  // Create a ROS 2 node
  auto node = rclcpp::Node::make_shared("ik_clearance_cost_demo");

  // Create a logger
  auto logger = node->get_logger();
  RCLCPP_INFO(logger, "Starting IK Clearance Cost Demo");

  // Start a separate thread to handle ROS 2 callbacks
  std::thread spinning_thread([node] { rclcpp::spin(node); });

  // Create a parameter listener for IK clearance cost parameters
  const auto param_listener = std::make_shared<ik_clearance_cost_demo::ParamListener>(node);
  const auto params = param_listener->get_params();
  RCLCPP_INFO(logger, "Parameters loaded: cumulative=%s, with_world=%s",
              params.cumulative ? "true" : "false",
              params.with_world ? "true" : "false");

  // Create a Task object to hold the planning stages
  Task t;
  t.stages()->setName("clearance IK");
  RCLCPP_INFO(logger, "Task created: %s", t.stages()->name().c_str());

  // Wait for 500 milliseconds to ensure ROS 2 is fully initialized
  rclcpp::sleep_for(std::chrono::milliseconds(500));

  // Load the robot model (mycobot_280)
  t.loadRobotModel(node);
  assert(t.getRobotModel()->getName() == "mycobot_280");
  RCLCPP_INFO(logger, "Robot model loaded: %s", t.getRobotModel()->getName().c_str());

  // Create a planning scene
  auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
  RCLCPP_INFO(logger, "Planning scene created");

  // Set the robot to its default state
  auto& robot_state = scene->getCurrentStateNonConst();
  robot_state.setToDefaultValues();
  RCLCPP_INFO(logger, "Robot state set to default values");

  // Set the arm to its "ready" position
  [[maybe_unused]] bool found =
      robot_state.setToDefaultValues(robot_state.getJointModelGroup("arm"), "ready");
  assert(found);
  RCLCPP_INFO(logger, "Arm set to 'ready' position");

  // Create an obstacle in the scene
  moveit_msgs::msg::CollisionObject co;
  co.id = "obstacle";
  co.primitives.emplace_back();
  co.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
  co.primitives[0].dimensions.resize(1);
  co.primitives[0].dimensions[0] = 0.1;
  co.header.frame_id = t.getRobotModel()->getModelFrame();
  co.primitive_poses.emplace_back();
  co.primitive_poses[0].orientation.w = 1.0;
  co.primitive_poses[0].position.x = -0.183;
  co.primitive_poses[0].position.y = -0.14;
  co.primitive_poses[0].position.z = 0.15;
  scene->processCollisionObjectMsg(co);
  RCLCPP_INFO(logger, "Obstacle added to scene: sphere at position (%.2f, %.2f, %.2f) with radius %.2f",
              co.primitive_poses[0].position.x, co.primitive_poses[0].position.y,
              co.primitive_poses[0].position.z, co.primitives[0].dimensions[0]);

  // Create a FixedState stage to set the initial state
  auto initial = std::make_unique<stages::FixedState>();
  initial->setState(scene);
  initial->setIgnoreCollisions(true);
  RCLCPP_INFO(logger, "FixedState stage created");

  // Create a ComputeIK stage for inverse kinematics
  auto ik = std::make_unique<stages::ComputeIK>();
  ik->insert(std::move(initial));
  ik->setGroup("arm");

  // Set the target pose
  ik->setTargetPose(Eigen::Translation3d(-.183, 0.0175, .15) * Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitX()));

  ik->setTimeout(1.0);
  ik->setMaxIKSolutions(100);

  // Set up the clearance cost term
  auto cl_cost{ std::make_unique<cost::Clearance>() };
  cl_cost->cumulative = params.cumulative;
  cl_cost->with_world = params.with_world;
  ik->setCostTerm(std::move(cl_cost));
  RCLCPP_INFO(logger, "Clearance cost term added to ComputeIK stage");

  // Add the ComputeIK stage to the task
  t.add(std::move(ik));
  RCLCPP_INFO(logger, "ComputeIK stage added to task");

  // Attempt to plan the task
  try {
    RCLCPP_INFO(logger, "Starting task planning");

    // Plan the task
    moveit::core::MoveItErrorCode error_code = t.plan(0);

    // Log the planning result
    if (error_code == moveit::core::MoveItErrorCode::SUCCESS) {
      RCLCPP_INFO(logger, "Task planning completed successfully");
      RCLCPP_INFO(logger, "Found %zu solutions", t.numSolutions());

      // Use printState to log the task state
      std::ostringstream state_stream;
      t.printState(state_stream);
      RCLCPP_INFO(logger, "Task state:\n%s", state_stream.str().c_str());

    } else {
      RCLCPP_ERROR(logger, "Task planning failed with error code: %d", error_code.val);

      // Use explainFailure to log the reason for failure
      std::ostringstream failure_stream;
      t.explainFailure(failure_stream);
      RCLCPP_ERROR(logger, "Failure explanation:\n%s", failure_stream.str().c_str());
    }

    // Log a simple summary of each stage
    RCLCPP_INFO(logger, "Stage summary:");
    for (size_t i = 0; i < t.stages()->numChildren(); ++i) {
      const auto* stage = t.stages()->operator[](i);
      RCLCPP_INFO(logger, "  %s: %zu solutions, %zu failures",
                  stage->name().c_str(), stage->solutions().size(), stage->failures().size());
    }

  } catch (const InitStageException& e) {
    RCLCPP_ERROR(logger, "InitStageException caught during task planning: %s", e.what());
  }

  RCLCPP_INFO(logger, "IK Clearance Cost Demo completed");

  // Wait for the spinning thread to finish
  spinning_thread.join();

  return 0;
}

Save the file, and close it.

Add the Parameters

Now let’s create a parameter file in the same directory as our source code.

gedit ik_clearance_cost_parameters.yaml

Add this code.

ik_clearance_cost_demo:
  cumulative:
    type: bool
    default_value: false
    read_only: true
  with_world:
    type: bool
    default_value: true
    read_only: true

The “cumulative” parameter determines how the robot measures its closeness to obstacles. 

  • When set to false, the robot only considers its single closest point to any obstacle. 
  • When set to true, it considers the distance of multiple points on the robot to obstacles, adding these distances together. 

This “cumulative” approach provides a more thorough assessment of the robot’s overall proximity to obstacles, potentially leading to more cautious movements. 

The “with_world” parameter determines what the robot considers as obstacles when planning its movements. 

  • When set to true, the robot takes into account all known objects in its environment – this could include tables, chairs, walls, or any other obstacles that have been mapped or sensed. It’s like the robot is aware of its entire surroundings. 
  • When set to false, the robot might only consider avoiding collisions with itself (self-collisions) or a specific subset of objects, ignoring the broader environment. 

Save the file, and close it.

Build the Code

cd ~/ros2_ws/
colcon build
source ~/.bashrc 

Launch

Launch everything:

bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_mtc_demos.sh ik_clearance_cost

OR

mtc_demos ik_clearance_cost

Here is what you should see:

ik-clearance-moveit-task-constructor

Understanding the Motion Planning Results

RViz – “Motion Planning Tasks” Panel

The “Motion Planning Tasks” panel in RViz displays the structure and outcomes of our IK clearance cost task. The panel shows a hierarchical view with “Motion Planning Tasks” at the root, followed by “clearance IK”.

2-motion-planning-tasks

Under “clearance IK“, two stages are visible:

  1. “IK”: This represents the ComputeIK stage where inverse kinematics solutions are generated.
  2. “initial state”: This corresponds to the FixedState stage that sets the initial robot configuration.

The second column shows green checkmarks and numbers indicating the quantity of successful solutions for each task component. The image reveals that 2 solutions were found for the overall “clearance IK” task, both originating from the “IK” stage.

The “time” column displays the computational time for each component. For the “IK” stage, we see a value of 1.0055 seconds, indicating the duration of the inverse kinematics calculations.

The “cost” column is particularly noteworthy in this context. For the successful IK solutions, we observe a cost value of 66.5330. This cost is directly related to the clearance cost term we incorporated into our ComputeIK stage. 

The “comment” column provides additional context for the solutions. It displays the clearance distances between the obstacle and a specific robot part, “gripper_left1”. This information quantifies how the robot positions itself relative to the obstacle in the computed solutions.

Terminal Window – Planning Results

Analyzing the terminal output of our IK clearance cost demo:

  1. The mycobot_280 robot model was loaded successfully.
  2. A planning scene was generated, and the robot was positioned in its ‘ready’ configuration.
  3. An obstacle, represented by a sphere, was introduced to the scene at coordinates (-0.18, -0.14, 0.15) with a 0.10 radius.
  4. The FixedState and ComputeIK stages were established and incorporated into the task.
  5. Task planning concluded successfully, yielding 2 solutions.

Analyzing the terminal output of our IK clearance cost demo, we see the following task structure:

3-planning-results

This structure provides insights into the flow of solutions through our planning task:

  1. clearance IK (top level):
    • 2 solutions were propagated backward
    • 2 solutions were propagated forward
    • 2 solutions were ultimately generated
  2. IK stage:
    • 2 solutions were generated at this stage
    • 2 solutions were propagated backward
    • 2 solutions were propagated forward
  3. initial state:
    • 1 solution was generated at this stage
    • 1 solution was propagated backward
    • 1 solution was propagated forward

This output demonstrates the bidirectional nature of the planning process in the MoveIt Task Constructor. The initial state provides a starting point, which is then used by the IK stage to generate solutions. These solutions are propagated both forward and backward through the planning pipeline.

The fact that we see two solutions at the IK stage indicates that our ComputeIK stage, incorporating the clearance cost term, successfully found two distinct inverse kinematics solutions that satisfied our constraints. These solutions maintained sufficient clearance from the obstacle while reaching the target pose.

The propagation of these two solutions both forward and backward means they were feasible within the context of both the initial state and the overall task requirements. This bidirectional flow helps ensure that the generated solutions are consistent and achievable given the robot’s starting configuration and the task’s goals.

By examining these results in conjunction with the RViz visualization, you can gain a comprehensive understanding of how the robot’s configuration changes to maintain clearance from the obstacle while achieving the desired pose of the gripper.

Detailed Code Walkthrough

Now for the C++ part. Let’s go through each piece of this code, step by step.

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/
gedit ik_clearance_cost.cpp

File Header and Includes

The code begins with a comprehensive comment block outlining the file’s purpose: demonstrating motion planning with collision avoidance using the MoveIt Task Constructor. It describes the program’s functionality, which creates a scene with an obstacle and computes inverse kinematics (IK) solutions while considering clearance from obstacles. 

The file includes necessary headers for ROS 2, MoveIt, and the Task Constructor library, establishing the foundation for our IK clearance cost demo.

Main Function

All the logic for this program is contained within the main function. Let’s break it down into its key components.

ROS 2 Initialization and Node Setup

The code initializes ROS 2 and creates a node named “ik_clearance_cost_demo”. It sets up a logger for informational output. This setup ensures proper communication within the ROS 2 ecosystem.

Parameter Handling

The code sets up a parameter listener to load and manage parameters for the IK clearance cost demo. It logs whether the clearance cost should be cumulative and whether it should consider the world.

Task Setup and Robot Model Loading

A Task object is created and named “clearance IK”. The robot model (“mycobot_280”) is loaded and verified. This step is important for accurate motion planning based on the specific robot’s characteristics.

Planning Scene Setup

The code creates a planning scene, sets the robot to its default state, and then sets the arm to its “ready” position. 

Obstacle Creation

An obstacle (a sphere) is created and added to the planning scene. This obstacle will be considered during the IK calculations to ensure clearance.

FixedState Stage Setup

A FixedState stage is created to set the initial state of the robot. It uses the previously configured scene and ignores collisions at this stage.

ComputeIK Stage Setup

A ComputeIK stage is created for inverse kinematics calculations. It’s configured with the initial state, target group (“arm”), target pose, timeout, and maximum number of IK solutions to compute.

Clearance Cost Term Setup

A clearance cost term is created and added to the ComputeIK stage. This cost term will influence the IK solutions to maintain clearance from obstacles.

Task Planning and Execution

The code attempts to plan the task using the defined stages. It includes error handling for potential exceptions during planning, ensuring robustness in various scenarios.

Results Logging

The code logs the results of the planning process, including the number of solutions found, the task state, or failure explanations if the planning was unsuccessful.

Node Spinning

A separate thread is created for spinning the ROS 2 node. This allows the program to handle callbacks and events while performing its main tasks.

That’s it. Keep building!