UBR-1 on ROS2 (Part 2)

In my last post I ported robot_controller_msgs to ROS2. I’ve now ported the actual controllers and made a number of changes to the Controller and ControllerManager classes. I can now move all the UBR-1 joints, which makes it almost as happy as this:

Image from Melonee Wise

Porting Gravity Compensation

Porting the gravity compensation controller was quite easy - it has very little in the way of a ROS interface. The full commit is on GitHub.

As I mentioned in my last post, the way you declare plugins has changed a bit. In ROS1, you would have an export directive in the package.xml that noted the location of the .xml file describing your plugins. In ROS2, this has moved to the CMake:

pluginlib_export_plugin_description_file(robot_controllers_interface robot_controllers.xml)

The one interesting quirk I did come across was that my driver node uses the “robot_description” parameter, and so do most controllers. You can’t declare the same parameter twice, so I ended up with this in the controllers:

if (!node->has_parameter("robot_description"))
{
  node->declare_parameter<std::string>("robot_description", "");
}

I will also note one piece of code I use quite frequently, but haven’t really noticed used in the tutorials or demos. When declaring a parameter, you can get the value right then (very similar to how parameters in ROS1 were often used:

std::string root = node->declare_parameter<std::string>("root", "torso_lift_link");

Action Servers

Many controllers make extensive use of ROS actions. While Actions exist in ROS2, there isn’t a direct replacement for the SimpleActionServer and the API has changed quite a bit.

There are several tutorial on ROS2 actions, but they are limited to Python. For C++, you have to look at ros2/examples.

In ROS1 each of the action-based controllers in robot_controllers had an execute thread for the action server and the main thread would call update() periodically. With the new APIs things are a bit different. The examples show spinning up a new thread for the execute callback, but I ended up refactoring things to use the existing update() function to do more of the work and spun up a rclcpp::Timer to publish feedback outside of update().

We’ll walk through the FollowJointTrajectoryController as an example. In the handle_goal callback I do some minimal checking of the trajectory goal, but usually accept the goal:

if (goal_handle->trajectory.joint_names.size() != joints_.size())
{
  RCLCPP_ERROR(rclcpp::get_logger(getName()),
               "Trajectory goal size does not match controlled joints size.");
  return rclcpp_action::GoalResponse::REJECT;
}

return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

The handle_accepted callback then sets us up to use the goal. Once a goal is fully accepted at the end of the callback, I save a shared_ptr to the goal handle, which is needed to eventually call succeed or abort. The first step in the callback is therefore to check if there is an active_goal_, and preempt it:

bool preempted = false;
if (active_goal_)
{
  result->error_code = -6;
  result->error_string = "preempted";
  active_goal_->abort(result);
  active_goal_.reset();
  preempted = true;
}

There is not currently a “preempted” state like in ROS1 (issue), so we just call abort. The preempted flag here gets used later on to splice the new trajectory into the old one and use the current velocity of the joints.

Then we can grab the actual goal message, and process it just like we would in a ROS1 action callback:

const auto goal = goal_handle->get_goal();

// process goal->trajectory so
// update() can use it
sampler_ = ...

// at the end of the callback
active_goal_ = goal_handle;

Then the update() function, which is called at some fixed rate, processes the goal and eventually marks it as succeeded or aborted:

// Is trajectory active?
if (active_goal_ && sampler_)
{
  // sample and command joints

  // If we've violated the tolerances
  active_goal_->abort(result);
  active_goal_.reset();

  // If we've completed the trajectory
  active_goal_->succeed(result);
  active_goal_.reset();  
}

Asynchronous Services

In ROS1, services were based on XML-RPC. Calling a service was synchronous - if the service provider died while processing your call then your client would hang. While there are work arounds (such as creating a thread for your service client), APIs should be easy to use correctly and hard to use incorrectly. For this reason, I often used actions in ROS1 where a service would have otherwise sufficed.

With ROS2, services can be called asynchronously. The ControllerManager exposes an API to start, stop, and load controllers. For ROS2, I converted this to a service. You can see the (commit) which does this conversion, as well as porting all of the Python scripts that interact with the service. This greatly simplifies the code - converting three callbacks into a single one with a significantly cleaner function signature.

ROS2 services are well documented with tutorials for both C++ and Python so I won’t go into any more detail here.

Using TF2

The TF library was one of the original hits in ROS1, making it easy to track where all your robot bits are located and transform data between different coordinate frames. ROS2 includes the TF2 library. The documentation on how to use it is currently quite sparse, at some point I’m planning to add to the tutorial, but in the meantime, here’s a quick highlight (in C++):

As with later ROS1 releases, you’ll want to depend on the tf2_ros package, and likely you want to depend on the tf2_geometry_msgs and tf2_sensor_msgs packages if you are converting those data types. Similarly, you still need a Buffer and a TransformListener:

std::shared_ptr<tf2_ros::Buffer> tf_buffer;
std::shared_ptr<tf2_ros::TransformListener> tf_listener;

The big change from ROS1 is the buffer now takes a clock in the constructor, so you need access to an rclcpp::Node instance:

tf_buffer.reset(new tf2_ros::Buffer(node.get_clock()));
tf_listener.reset(new tf2_ros::TransformListener(*tf_buffer_));

From that point, it is pretty much just like ROS1 to transform things (see a real example in robot_controllers/src/point_head.cpp):

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

geometry_msg::msgs::PointStamped in = <blah>;
geometry_msg::msgs::PointStamped out;

try
{
  tf_buffer->transform(in, out, "target_frame");
}
catch (const tf2::TransformException& ex)
{
  RCLCPP_ERROR(node.get_logger(), "Could not transform point.");
}

The broadcaster works pretty much identically to ROS1, other than the message renaming and the constructor taking an rclcpp::Node::SharedPtr:

std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster;

broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(nodePtr);

geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node->now();
transform.header.frame_id = "odom";
transform.child_frame_id = "base_link";
// Fill in transform.transform.translation
// Fill in transform.transform.rotation

broadcaster->sendTransform(transform);

You can find the full/real code in robot_controllers/src/diff_drive_base.cpp.

Next Steps

I’m currently wrapping up making the head camera work. That will be the subject of my next post in a few days.