Robot & Chisel

stuff by michael ferguson. robots, woodworking, etc.

© 2024. All rights reserved.

UBR-1 on ROS2

08 Jun 2020 ubr1  robots  ros2 

The latest ROS2 release, Foxy Fitzroy, was released last Friday. In addition to targeting an Ubuntu LTS (20.04), this release will get 3 years of support (more than the previous 2 years, but still quite a bit less than a ROS1 LTS release which get 5).

Since I already have the UBR-1 running 20.04, I decided to try getting it running on ROS2. I’ve made a decent amount of progress, but there is still a long ways to go, so this is the first of what will probably be several blog posts on porting the UBR-1 to ROS2.

I’ve had several false starts at switching over to ROS2 over the past few years. Each time I would start going through some tutorials, hit something that wasn’t documented or wasn’t working and just go back to using ROS1. I’d love to say the documentation is way better now, but…

Starting with the Messages

The challenge with such a big port is trying to just get started on the mountain of code in front of you. Logically, you need to start on the lowest level dependencies first. For this project, it was a series of message packages that had to be ported.

The robot_calibration package includes an action definition that I use for the gripper on the UBR-1, so the drivers depend on it. ROS2 message packages are pretty straight forward to port from ROS1, although there are a few caveats to be aware of. I didn’t have to change any of my actual message definitions, so the entire change consisted of package.xml and CMakeLists.txt changes. You can find the commit here. A similar commit exists for porting the robot_controllers_msgs package.

So, what are some of those caveats to be aware of?

The first one relates to terrible error messages. I was originally missing this from the package.xml:

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

Which causes this build error:

CMake Error at /usr/share/cmake-3.16/Modules/FindPackageHandleStandardArgs.cmake:146 (message):
  Could NOT find FastRTPS (missing: FastRTPS_INCLUDE_DIR FastRTPS_LIBRARIES)

What??? It took a while to figure that one - especially since my debugging went like this:

The second caveat to watch out for: make sure ALL your dependencies are specified for messages. I finished porting all of these message packages and then ran into issues when I finally got to my first node using them. The nodes exited immediately due to missing symbols. The initial port of ubr_msgs compiled just fine. The node depending on it also compiled just fine, but would not run.

The fix was simply to add a std_msgs dependency. A few packages you probably should depend on:

Porting robot_controllers_interface

I had recently updated the UBR-1 to ROS Noetic and to use the robot_controllers package that I wrote at Fetch Robotics. So my next step was to port the robot_controllers_interface to ROS2. For those following along, here’s a few commits:

The package.xml and CMakeLists.txt are largely uneventful:

The actual API changes for were a bit more involved. Many things now require access to the rclcpp::Node (which is similar, but not exactly like the ros::NodeHandle). Most examples simply show developing a ROS2 component that derives from rclcpp::Node. Which is nice for simple demos, but in a larger system with multiple controllers, leads to a lot of overhead (each node has a bunch of services for parameters, etc).

I initially started passing lots of extra strings around until I found this undocumented feature: sub_namespaces - which gives you functionality similar to NodeHandles. This seemed like a great way to get rid of the string name I was passing around. Unfortunately, it’s not only undocumented, it’s mostly broken for parameters. So I went back to passing names manually and concatenating them in the code.

This also leads to some interesting issues that didn’t exist in ROS1: nested parameter names use a . for a separator, while topic names still use a /.

As I started to move into porting actual controllers, which are loaded as plugins, it became apparent there isn’t much (any?) docs on using pluginlib in ROS2. I did find an issue that suggested looking at the RVIZ plugins, which at least pointed out that the declaration of a plugin library has moved from the exports in the package.xml to a CMake directive. I’ll dig into that more in the next post when I talk about porting robot_controllers in detail.

One part of this port really stood out to me for how clean it made the code. While there are some quirky aspects to parameters (why a .?), parsing large blocks of parameters in ROS always got messy, consider this piece of code:

  // Find and load default controllers
  XmlRpc::XmlRpcValue controller_params;
  if (nh.getParam("default_controllers", controller_params))
  {
    if (controller_params.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR_NAMED("ControllerManager", "Parameter 'default_controllers' should be a list.");
      return -1;
    }
    else
    {
      // Load each controller
      for (int c = 0; c < controller_params.size(); c++)
      {
        // Make sure name is valid
        XmlRpc::XmlRpcValue &controller_name = controller_params[c];
        if (controller_name.getType() != XmlRpc::XmlRpcValue::TypeString)
        {
          ROS_WARN_NAMED("ControllerManager", "Controller name is not a string?");
          continue;
        }

        // Create controller (in a loader)
        load(static_cast<std::string>(controller_name));
      }
    }

With the new ROS2 API, it becomes this:

   // Find default controllers
   std::vector<std::string> controller_names =
     node_->declare_parameter<std::vector<std::string>>("default_controllers", std::vector<std::string>());
   if (controller_names.empty())
   {
     RCLCPP_WARN(node_->get_logger(), "No controllers loaded.");
     return -1;
   }

   // Load each controller
   for (auto controller_name : controller_names)
   {
     RCLCPP_INFO(node->get_logger(), "Loading %s", controller_name.c_str());
     load(controller_name);
   }

Launch Files

This was perhaps the most frustrating part of this exercise thus far. Documentation is lacking, and examples vary so widely it is like the Wild West out there. There just aren’t many real robots running ROS2 yet.

I finally managed to hack together a launch file which starts:

You can find the launch file on GitHub

Progress in RVIZ

At this point I was publishing the joint positions, IMU, and laser data. It was time to fire up rviz2:

Next Steps

I’m continuing to port robot_controllers - I’m sure I’ll have more posts about that.

Recent Posts