How to Add a Python ROS2 Node to a C++ ROS 2 Package

cover_python_cpp

In this tutorial, I will show you how to add a Python ROS 2 node to a C++ package. We will go through the entire process, step-by-step.

Let’s say you have a ROS 2 package that is a standard C++ ROS 2 package. What this means is that it is of the ament_cmake (CMake) build type. You can verify this by going to the package.xml file inside your package and checking the <buildtool_depend> tag.


For example, the name of my workspace is dev_ws, and I have a package named two_wheeled_robot inside that package. I go to my terminal window, and type:

cd ~/dev_ws/src/two_wheeled_robot
gedit package.xml

Here is what I see:

1-build-tool

I have a Python node that I created called lift_controller.py, and I want to add it to my package. I am using this Python node to lift the platform on top of a mobile robot that I want to visualize in RViz.

Create the Python Node

The first thing I am going to do is to write my Python node. I want to create this code inside a folder named scripts.

cd ~/dev_ws/src/two_wheeled_robot 
mkdir scripts
cd scripts
gedit lift_controller.py

Write your Python code inside that file. Here is my code (don’t worry about trying to understand the code…this is just a demo):

#!/usr/bin/env python3 

# Test code for controlling the lift mechanism on a warehouse robot
# Author: Addison Sears-Collins
# Website: https://automaticaddison.com

# ROS Client Library for Python
import rclpy
 
# Handles the creation of nodes
from rclpy.node import Node
 
# Enables usage of the standard message
import std_msgs.msg

# Enables use of sensor messages
import sensor_msgs.msg 

 
class LiftController(Node):
  """
  Create a LiftController class, which is a subclass of the Node class.
  """
  def __init__(self):
    """
    Class constructor to set up the node
    """
    # Initiate the Node class's constructor and give it a name
    super().__init__('lift_controller')
     
    # Create the publisher. This publisher will publish a JointState message
    # to a topic. The queue size is 10 messages.
    self.publisher_ = self.create_publisher(sensor_msgs.msg.JointState, 'joint_states', 10)

    # Create the subscriber. This subscriber will subscribe to a JointState message
    self.subscription = self.create_subscription(sensor_msgs.msg.JointState, 'joint_states', self.listener_callback, 10)
         
  def listener_callback(self, msg):
    """
    Callback function.
    """
    # Create a JointStates message
    new_msg = sensor_msgs.msg.JointState()
 
    # Set the message's data
    new_msg.header.stamp = self.get_clock().now().to_msg()
    new_msg.name = msg.name
    new_msg.position = msg.position
    new_msg.position[2] = 0.30 
     
    # Publish the message to the topic
    self.publisher_.publish(new_msg)
     
def main(args=None):
 
  # Initialize the rclpy library
  rclpy.init(args=args)
 
  # Create the node
  lift_controller = LiftController()
 
  # Spin the node so the callback function is called.
  rclpy.spin(lift_controller)
 
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  lift_controller.destroy_node()
 
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
 
if __name__ == '__main__':
  main()

Save the file and close it.

Make sure this shebang line is on the first line of your Python node.

#!/usr/bin/env python3

Make the file executable using the following command.

chmod +x lift_controller.py

Set Up the Package

Now we need to set up the package so that it can build Python executables.

First, we need to create a folder for any Python libraries or modules that we want to import into our Python node. This folder needs to have an empty __init__.py file inside it. It will also have the same name as the package.

cd ~/dev_ws/src/two_wheeled_robot 
mkdir two_wheeled_robot
touch two_wheeled_robot/__init__.py

If you have a module that you want to import into your Python node, you can place it inside the folder as follows.

touch two_wheeled_robot/module_to_import.py

If you want to import that module into your Python node, you would write this at the top of your Python node:

from two_wheeled_robot.module_to_import import ...

Here is an example folder tree of what my two_weeled_robot package looks like.

two_wheeled_robot/
# --> package information, configuration, and compilation
├── CMakeLists.txt
├── package.xml
# --> Python contents
├── two_wheeled_robot
│   ├── __init__.py
│   └── module_to_import.py
├── scripts
│   └──  lift_controller.py
# --> C++ contents
├── include
│   └── two_wheeled_robot
│       └── cpp_node.hpp
└── src
    └── cpp_node.cpp

Update the Package.xml File

Here is my package.xml file. You will see that I have added the ament_cmake_python build tool. I have also added dependencies, including the ROS2 Python library (rclpy).

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>two_wheeled_robot</name>
  <version>1.0.0</version>
  <description>A two-wheeled mobile robot with an IMU sensor and LIDAR</description>
  <maintainer email="automaticaddison@todo.todo">AutomaticAddison</maintainer>
  <license>MIT License</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>rviz</exec_depend>
  <exec_depend>xacro</exec_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Update the CMakeLists.txt File

Here is my CMakeLists.txt File.

cmake_minimum_required(VERSION 3.5)
project(two_wheeled_robot)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

# Include cpp "include" directory
include_directories(include)

install(
  DIRECTORY config include launch maps media meshes models params rviz scripts src two_wheeled_robot urdf worlds
  DESTINATION share/${PROJECT_NAME}
)

# Install Python modules
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(PROGRAMS
  scripts/lift_controller.py
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

I have added dependencies for ament_cmake_python, rclpy, etc.

I added the path to the include directories in case I want to create C++ header files later on. 

I include the extra folders in the directory.

I install Python modules.

I then provide the path to the Python executables.

If you have C++ nodes, these executables would be located under the include_directories(include) line.

# Create Cpp executable
add_executable(cpp_executable src/cpp_node.cpp)
ament_target_dependencies(cpp_executable rclcpp)

# Install Cpp executables
install(TARGETS
  cpp_executable
  DESTINATION lib/${PROJECT_NAME}
)

Build and Run the Node

To build the node, go back to the root of your workspace.

cd ~/dev_ws/

Build the package.

colcon build

Now, open a new terminal window, and run the node.

ros2 run two_wheeled_robot lift_controller.py

To see active ROS 2 topics, you can open a new terminal window and type:

ros2 topic list

References

This blog post over at RoboticsBackend was very helpful.