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:
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.
