Integrating ROS 2 with Custom C++ Libraries for Advanced Robot Functionality

The robotics landscape is evolving rapidly, and with ROS 2's modern architecture, we have unprecedented opportunities to create sophisticated robotic systems. However, the real power of ROS 2 emerges when we combine it with custom C++ libraries, enabling us to push the boundaries of what our robots can achieve. In this comprehensive guide, we'll explore how to seamlessly integrate custom C++ libraries with ROS 2, unlocking advanced functionality while maintaining system reliability and performance.

Understanding the Integration Landscape

Before diving into implementation details, it's crucial to understand why integrating custom C++ libraries with ROS 2 is such a powerful approach. ROS 2 provides a robust communication framework and ecosystem, while custom C++ libraries offer specialized algorithms and optimized performance for specific tasks.

Setting Up the Development Environment

First, let's establish a proper development environment that supports both ROS 2 and custom C++ library integration. We'll create a workspace structure that follows best practices:

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake my_robot_library
cd my_robot_library

Creating a Custom C++ Library

Let's start by implementing a custom C++ library for advanced trajectory planning. This example demonstrates how to structure your code for optimal integration with ROS 2:

// include/my_robot_library/trajectory_planner.hpp
#ifndef MY_ROBOT_LIBRARY_TRAJECTORY_PLANNER_HPP
#define MY_ROBOT_LIBRARY_TRAJECTORY_PLANNER_HPP

#include <Eigen/Dense>
#include <vector>
#include <memory>

namespace my_robot_library {

class TrajectoryPlanner {
public:
    struct TrajectoryPoint {
        Eigen::Vector3d position;
        Eigen::Vector3d velocity;
        double timestamp;
    };

    TrajectoryPlanner(double max_velocity, double max_acceleration);
    
    std::vector<TrajectoryPoint> generateTrajectory(
        const Eigen::Vector3d& start,
        const Eigen::Vector3d& goal,
        double duration
    );

private:
    double max_velocity_;
    double max_acceleration_;
    
    Eigen::Vector3d computeOptimalControl(
        const Eigen::Vector3d& current_state,
        const Eigen::Vector3d& desired_state
    );
};

} // namespace my_robot_library

#endif // MY_ROBOT_LIBRARY_TRAJECTORY_PLANNER_HPP

The implementation file showcases advanced C++ features and optimal performance considerations:

// src/trajectory_planner.cpp
#include "my_robot_library/trajectory_planner.hpp"
#include <cmath>

namespace my_robot_library {

TrajectoryPlanner::TrajectoryPlanner(
    double max_velocity,
    double max_acceleration
) : max_velocity_(max_velocity),
    max_acceleration_(max_acceleration) {}

std::vector<TrajectoryPlanner::TrajectoryPoint> 
TrajectoryPlanner::generateTrajectory(
    const Eigen::Vector3d& start,
    const Eigen::Vector3d& goal,
    double duration
) {
    std::vector<TrajectoryPoint> trajectory;
    const int steps = static_cast<int>(duration * 100); // 100 Hz sampling
    
    for (int i = 0; i < steps; ++i) {
        double t= static_cast<double>(i) / steps * duration;
        
        // Minimum jerk trajectory generation
        double tau = t / duration;
        double h = 10 * std::pow(tau, 3) - 15 * std::pow(tau, 4) + 6 * std::pow(tau, 5);
        
        TrajectoryPoint point;
        point.position = start + (goal - start) * h;
        point.velocity = (goal - start) * (30 * std::pow(tau, 2) - 60 * std::pow(tau, 3) + 30 * std::pow(tau, 4)) / duration;
        point.timestamp = t;
        
        trajectory.push_back(point);
    }
    
    return trajectory;
}

Eigen::Vector3d TrajectoryPlanner::computeOptimalControl(
    const Eigen::Vector3d& current_state,
    const Eigen::Vector3d& desired_state
) {
    // Implementation of optimal control law
    return (desired_state - current_state) * max_acceleration_;
}

} // namespace my_robot_library

Integrating with ROS 2

Now that we have our custom library, let's create a ROS 2 node that utilizes it. This integration showcases how to bridge the gap between custom C++ code and the ROS 2 ecosystem:

// include/my_robot_library/trajectory_node.hpp
#include "rclcpp/rclcpp.hpp"
#include "my_robot_library/trajectory_planner.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"

class TrajectoryNode : public rclcpp::Node {
public:
    TrajectoryNode();

private:
    std::unique_ptr<my_robot_library::TrajectoryPlanner> planner_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr traj_pub_;
    
    void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
};

The implementation demonstrates proper error handling and ROS 2 best practices:

// src/trajectory_node.cpp
#include "my_robot_library/trajectory_node.hpp"

TrajectoryNode::TrajectoryNode()
: Node("trajectory_node")
{
    // Parameter declaration
    declare_parameter("max_velocity", 1.0);
    declare_parameter("max_acceleration", 0.5);
    
    // Initialize planner with parameters
    planner_ = std::make_unique<my_robot_library::TrajectoryPlanner>(
        get_parameter("max_velocity").as_double(),
        get_parameter("max_acceleration").as_double()
    );
    
    // Setup ROS 2 communications
    goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
        "goal_pose", 10,
        std::bind(&TrajectoryNode::goalCallback, this, std::placeholders::_1)
    );
    
    traj_pub_ = create_publisher<trajectory_msgs::msg::JointTrajectory>(
        "trajectory", 10
    );
}

void TrajectoryNode::goalCallback(
    const geometry_msgs::msg::PoseStamped::SharedPtr msg
) {
    try {
        Eigen::Vector3d start(0.0, 0.0, 0.0); // Current position
        Eigen::Vector3d goal(
            msg->pose.position.x,
            msg->pose.position.y,
            msg->pose.position.z
        );
        
        auto trajectory = planner_->generateTrajectory(start, goal, 5.0);
        
        // Convert to ROS message
        trajectory_msgs::msg::JointTrajectory traj_msg;
        traj_msg.header.stamp = now();
        traj_msg.header.frame_id = "base_link";
        
        // Populate trajectory message
        for (const auto& point : trajectory) {
            trajectory_msgs::msg::JointTrajectoryPoint point_msg;
            point_msg.positions = {
                point.position.x(),
                point.position.y(),
                point.position.z()
            };
            point_msg.velocities = {
                point.velocity.x(),
                point.velocity.y(),
                point.velocity.z()
            };
            point_msg.time_from_start = rclcpp::Duration::from_seconds(
                point.timestamp
            );
            traj_msg.points.push_back(point_msg);
        }
        
        traj_pub_->publish(traj_msg);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(
            get_logger(),
            "Error generating trajectory: %s", e.what()
        );
    }
}

Building and Testing the Integration

To ensure our integration works correctly, we need to set up the proper build configuration. Here's the CMakeLists.txt file that ties everything together:

cmake_minimum_required(VERSION 3.8)
project(my_robot_library)

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(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(Eigen3 REQUIRED)

# Create the library
add_library(${PROJECT_NAME}
  src/trajectory_planner.cpp
)

target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  ${EIGEN3_INCLUDE_DIR}
)

# Create the node executable
add_executable(trajectory_node src/trajectory_node.cpp)
target_link_libraries(trajectory_node
  ${PROJECT_NAME}
)

ament_target_dependencies(trajectory_node
  rclcpp
  geometry_msgs
  trajectory_msgs
)

# Install
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

install(TARGETS trajectory_node
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY include/
  DESTINATION include
)

ament_package()

Performance Considerations

When integrating custom C++ libraries with ROS 2, performance is a critical consideration. Here are some key aspects to keep in mind:

Testing the Integration

Let's launch our node and test it with some sample goals:

# Terminal 1
ros2 run my_robot_library trajectory_node

# Terminal 2
ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "{
  header: {frame_id: 'base_link'},
  pose: {
    position: {x: 1.0, y: 1.0, z: 0.5},
    orientation: {w: 1.0}
  }
}"

Advanced Features and Future Improvements

As your robot's capabilities grow, you might want to extend this integration. Here are some advanced features you could consider adding:

The beauty of this integration approach is that it separates concerns effectively: the custom C++ library handles the computational heavy lifting, while ROS 2 manages communication and system integration. This separation makes it easier to test, maintain, and extend your robot's functionality over time.

Conclusion

Integrating custom C++ libraries with ROS 2 opens up a world of possibilities for advanced robot functionality. By following the patterns and practices outlined in this guide, you can create robust, high-performance robotic systems that leverage the best of both worlds: the efficiency and control of custom C++ code and the extensive ecosystem and tools of ROS 2.

Remember that successful integration requires careful attention to system architecture, build configuration, and performance considerations. As you continue to develop your robotic system, these foundations will prove invaluable in maintaining code quality and system reliability.