ROS1 to ROS2 porting¶
Introduction¶
Our goal for this exercise is to have you fully port a small ROS1 application into ROS2. We’ll be using the basic training material from sessions 1 and 2 as the initial ROS1 application. This will provide some exposure to the differences in the basic tools of ROS development including publishers, subscribers, services, and parameters. We will be using exercise 2.3 as the starting point.
Workspace setup¶
Clone the ROS2 version of
fake_ar_publisher
into your ROS2 workspacecd ~/ros2_ws/src/ git clone -b ros2 https://github.com/ros-industrial/fake_ar_publisher.git
Create empty ROS2 packages.
cd ~/ros2_ws/src/ ros2 pkg create myworkcell_core ros2 pkg create myworkcell_support
Verify the workspace builds with
colcon build
Set up a new project in Qt Creator with ‘Build System’ set to ‘Colcon’ and ‘Distribution’ set to ‘/opt/ros/eloquent’. This is optional but highly recommended since you will get code autocompletion when editing C++ in the Qt Creator IDE. Verify that the workspace builds successfully from inside the IDE.
Copy the ROS1 code from the packages at
~/industrial_training/exercises/2.3/src/
into the empty ROS2 packages. We’ll want everything copied over exceptCMakeLists.txt
andpackage.xml
, where we will be modifying the empty ROS2 versions instead. For more complex porting efforts, it will likely be more efficient to directly modify the ROS1 versions of these files.
Porting¶
We will more or less follow the same sequence that the ROS1 basic training takes in switching to ROS2.
Topic subscription¶
Open and start editing the
CMakeLists.txt
file inmyworkcell_core
Package dependencies: Ament packages you depend on are now found directly using the CMake
find_package
command. Almost always the first invocation will befind_package(ament_cmake)
which is required to make this package an ament package.Add additional dependencies for this application below this first
find_package
command:find_package(rclcpp REQUIRED) find_package(fake_ar_publisher_msgs REQUIRED)
Build the vision node: An executable is added in the usual CMake way. Rather than use
include_directories
andtarget_link_libraries
to depend on other packages, a convenience macro is provided to depend on other ament packages.add_executable(vision_node src/vision_node.cpp) ament_target_dependencies(vision_node rclcpp fake_ar_publisher)
Install the vision node: Everything in ROS2 must have an install rule in order to be used at runtime.
install(TARGETS vision_node ARCHIVE DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION lib/${PROJECT_NAME} )
Above the final call to
ament_package()
add a line which indicates which ament packages this package depends on at runtime:ament_export_dependencies(rclcpp fake_ar_publisher)
You may try to build the workspace again now and you should get a compile error about
ros/ros.h
not being found. We’re now ready to port the C++ code. Openvision_node.cpp
for editing.Change the headers: ROS2 requires headers to end in .hpp instead of .h and includes for msgs and srv require the file name to file/msg/ and file/srv/ respectively in their names.
- #include <ros/ros.h> + #include <rclcpp/rclcpp.hpp> - #include <fake_ar_publisher/ARMarker.h> + #include <fake_ar_publisher/msg/ar_marker.hpp>
Comment out the callbacks, the member variables, and the contents of the constructor. Change the
Localizer
class to inherit fromrclcpp::Node
.class Localizer : public rclcpp::Node { public: Localizer() : Node("vision_node") { //... } //... };
The main function can now be replaced with ROS2 equivalents of creating a node and spinning to wait for callbacks to be invoked:
int main(int argc, char* argv[]) { // This must be called before anything else ROS-related rclcpp::init(argc, argv); // Create the node auto node = std::make_shared<Localizer>(); // Don't exit the program. rclcpp::spin(node); }
Now we’ll port the subscriber Replace the
ar_sub_
andlast_msg_
member variables with:rclcpp::Subscription<fake_ar_publisher::msg::ARMarker>::SharedPtr ar_sub_; fake_ar_publisher::msg::ARMarker::SharedPtr last_msg_;
Add the subscription creation to the constructor:
using namespace std::placeholders; ar_sub_ = this->create_subscription<fake_ar_publisher::msg::ARMarker>("ar_pose_marker", rclcpp::QoS(1), std::bind(&Localizer::visionCallback, this, _1));
and the callback that stores the message:
void visionCallback(fake_ar_publisher::msg::ARMarker::SharedPtr msg) { last_msg_ = msg; RCLCPP_INFO(this->get_logger(), "Received ARMarker message"); }
Verify the workspace builds now and test the topic subscription. Run the publisher node and vision node in separate terminals:
ros2 run fake_ar_publisher fake_ar_publisher ros2 run myworkcell_core vision_node
Interface generation¶
We’ll now build the LocalizePart
service type to use in the myworkcell_core
package.
The
.srv
file syntax is basically unchanged, but we do have to modify the definition. Since ROS2 does not allow you to return a boolean from a service callback, we must track success and failure ourselves. Add asuccess
member to the response inLocalizePart.srv
:#request string base_frame --- #response bool success geometry_msgs/Pose pose
Modify CMake and
package.xml
to build the service. InCMakeLists.txt
add additional dependencies and a call to generate custom interfaces:find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/LocalizePart.srv" DEPENDENCIES geometry_msgs )
To use generated interfaces in the same package, another dependency must be added on built targets:
rosidl_target_interfaces(vision_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
In
package.xml
make sure theformat
version is 3 and add the following tags:<member_of_group>rosidl_interface_packages</member_of_group> <build_depend>rosidl_default_generators</build_depend> <exec_depend>rosidl_default_runtime</exec_depend>
Make sure the workspace builds and you can now see the
myworkcell_core/srv/LocalizePart
service when runningros2 srv list
.
Services¶
At the top of
vision_node.cpp
add/uncomment:#include <myworkcell_core/srv/localize_part.hpp>
Then add the service server to the vision node, replacing the
ServiceServer
member variable with:rclcpp::Service<myworkcell_core::srv::LocalizePart>::SharedPtr server_;
Add the service creation to the constructor:
server_ = this->create_service<myworkcell_core::srv::LocalizePart>("localize_part", std::bind(&Localizer::localizePart, this, _1, _2));
and the service callback:
void localizePart(myworkcell_core::srv::LocalizePart::Request::SharedPtr req, myworkcell_core::srv::LocalizePart::Response::SharedPtr res) { // Read last message fake_ar_publisher::msg::ARMarker::SharedPtr p = last_msg_; if (!p){ RCLCPP_ERROR(this->get_logger(), "no data"); res->success = false; return; } res->success = true; res->pose = p->pose.pose; }
Run both the
fake_ar_publisher
node and thevision_node
and try manually calling the service.Now we’ll set up the application node to call the service. Add a new target to
CMakeLists.txt
:add_executable(myworkcell_node src/myworkcell_node.cpp) ament_target_dependencies(myworkcell_node rclcpp fake_ar_publisher_msgs) rosidl_target_interfaces(myworkcell_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
and add
myworkcell_node
to the existinginstall
function call, aftervision_node
.Open
myworkcell_node.cpp
to start porting it over. As done with the vision node, convert theScanNPlan
class to inherit fromrclcpp::Node
and rewrite themain
function in the same way. For now, comment out the code that loads the ‘base_frame’ parameter and call thestart
function with a hardcoded"world"
.class ScanNPlan : public rclcpp::Node { public: ScanNPlan() : Node("scan_n_plan") { //... } //... } int main(int argc, char* argv[]) { //... }
Replace the
vision_client
member variable with:rclcpp::Client<myworkcell_core::srv::LocalizePart>::SharedPtr vision_client_;
and create it in the constuctor with:
vision_client_ = this->create_client<myworkcell_core::srv::LocalizePart>("localize_part");
The
start
function will be fully replaced since it is quite a bit more complex. Since ROS2 services are now asynchronous, the client returns afuture
object when called and the developer is responsible for spinning until a response returns from the vision node. Read through the newstart
function here before replacing to make sure you understand everything that is happening.void start(const std::string& base_frame) { using namespace std::chrono_literals; // Need to wait until vision node has data rclcpp::sleep_for(1s); RCLCPP_INFO(this->get_logger(), "Attempting to localize part in frame: %s", base_frame.c_str()); // The vision client needs to wait until the service appears while (!vision_client_->wait_for_service(500ms)) { if (!rclcpp::ok()) { RCLCPP_ERROR(this->get_logger(), "client interrupted while waiting for service to appear."); return; } RCLCPP_INFO(this->get_logger(), "waiting for service to appear..."); } // Create a request for the LocalizePart service call auto request = std::make_shared<myworkcell_core::srv::LocalizePart::Request>(); // The base_frame that is passed in is used to fill the request request->base_frame = base_frame; auto future = vision_client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(), "Failed to receive LocalizePart service response"); return; } auto result = future.get(); if (! result->success) { RCLCPP_ERROR(this->get_logger(), "LocalizePart service failed"); return; } RCLCPP_INFO(this->get_logger(), "Part Localized: w: %f, x: %f, y: %f, z: %f", result->pose.orientation.w, result->pose.position.x, result->pose.position.y, result->pose.position.z); }
Verify everything builds and works
Parameters¶
Parameters work slightly differently than in ROS1. The most significant change is the parameter declaration mechanism, which, when given a default value, ensures that a parameter will exist under the given name which matches the type of the default. Modify the
main
function to declare and get thebase_frame
parameter:auto node = std::make_shared<ScanNPlan>(); // String to store the base_frame parameter after getting it from the Node's parameter client std::string base_frame; node->declare_parameter("base_frame", ""); node->get_parameter("base_frame", base_frame); if (base_frame.empty()) { RCLCPP_ERROR(node->get_logger(), "No 'base_frame' parameter provided"); return -1; } // Call the vision client's LocalizePart service using base_frame as a parameter node->start(base_frame);
Unfortunately, parameters can not currently be set on the command line when using
ros2 run
. To test that the parameter is set, you must create and save a YAML file:/**: ros__parameters: base_frame: world
Then load this file when running the node:
ros2 run myworkcell_core myworkcell_node __params:=my_params_file.yaml
Launch file¶
Currently, the
myworkcell_support
package only contains a launch file for starting the three required nodes. We will port it to a ROS2 python launch script.Start by creating a new file
workcell.launch.py
under thelaunch/
directory.Modify the contents to add the required python imports and the required function which will return the launch configuration description:
from launch import LaunchDescription import launch_ros.actions def generate_launch_description(): print('Workcell is launching...')
Each node in the XML launch file will correspond to a
Node
launch ‘action’ in the new launch description. The named arguments used in the action constructor should have clear meaning. You’ll need one for thefake_ar_publisher
:fake_ar_pub_node = launch_ros.actions.Node( node_name='fake_ar_publisher_node', package='fake_ar_publisher', node_executable='fake_ar_publisher_node', output='screen', parameters=[{'x': -0.6}, {'y': 0.4}, {'z': 0.1}, {'camera_frame': 'camera_frame'}], )
one for the
vision_node
:vision_node = launch_ros.actions.Node( node_name='vision_node', package='myworkcell_core', node_executable='vision_node', output='screen', )
and one for
myworkcell_node
:myworkcell_node = launch_ros.actions.Node( node_name='myworkcell_node', package='myworkcell_core', node_executable='myworkcell_node', output='screen', parameters=[{'base_frame': 'world'}], )
Notice the
parameters
argument must be set with a list of dictionaries. Another common argument not used here isarguments
, set with a list of command line arguments that the executable will start with.Now bundle the three nodes in a
LaunchDescription
and return it from the function:return LaunchDescription([fake_ar_pub_node, vision_node, myworkcell_node])
Add an
install
call toCMakesList.txt
to install the launch file:install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ )
Modify
package.xml
to show the runtime dependency on the packages<exec_depend>fake_ar_publisher</exec_depend> <exec_depend>myworkcell_core</exec_depend>
After building the workspace test the launch file with:
ros2 launch myworkcell_support workcell.launch.py
Note that the file loaded by
ros2 launch
resides in theinstall/
directory and will not update if you edit the version in thesrc/
space. To avoid having to rebuild the workspace to see changes in the launch script, you can runcolcon build
with the--symlink-install
option so the file in the install space is just a link back to the source space. This build option is also useful when developing ROS python code in general.