Creating a new sas_robot_driver package

Creating a new sas_robot_driver package#

Fortunately, sas already has a good number of drivers for popular robotic manipulators. Nonetheless, it is common to need the integration with new robotic systems. This tutorial will assist you in creating a suitable package.

You might be wondering why go through the trouble of doing this. Simply put, creating a suitable sas package that has a subclass of sas::RobotDriver will allow you to

  1. Expose joint states and robot control inputs in ROS2 without programming a single subscriber, publisher, or service.

  2. Access a C++ driver via Python without any particular Python code for the new robot.

  3. Integrate with all other packages of sas, such as the teleoperation packages.

It’s not too late to turn back now. Once you taste the forbidden sas fruit you will not want to go back to writing ROS2 publishers and subscribers by yourself.

Creating the package#

The first step is to create the package with the correct dependencies. In this example we depend on sas_core, sas_common, and sas_robot_driver. Using ros2 pkg create we do

cd ~/sas_tutorial_workspace/src
ros2 pkg create sas_robot_driver_myrobot \
--build-type ament_cmake \
--dependencies rclcpp sas_core sas_common sas_robot_driver

which outputs

ros2 pkg create output
going to create a new package
package name: sas_robot_driver_myrobot
destination directory: /home/murilo/Downloads/pycharm-community-2024.3.5/bin
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['murilo <murilo.marinho@manchester.ac.uk>']
licenses: ['TODO: License declaration']
build type: ament_cmake
dependencies: ['rclcpp', 'sas_core', 'sas_common', 'sas_robot_driver']
creating folder ./sas_robot_driver_myrobot
creating ./sas_robot_driver_myrobot/package.xml
creating source and include folder
creating folder ./sas_robot_driver_myrobot/src
creating folder ./sas_robot_driver_myrobot/include/sas_robot_driver_myrobot
creating ./sas_robot_driver_myrobot/CMakeLists.txt

[WARNING]: Unknown license 'TODO: License declaration'.  This has been set in the package.xml, but no LICENSE file has been created.
It is recommended to use one of the ament license identifiers:
Apache-2.0
BSL-1.0
BSD-2.0
BSD-2-Clause
BSD-3-Clause
GPL-3.0-only
LGPL-3.0-only
MIT
MIT-0

TL;DR#

(Murilo’s) sas_robot_driver best practices

For each new robot called myrobot we have the three steps below as a must

  1. sas_robot_driver_myrobot.hpp with the driver’s class definition that inherits from sas_robot_driver. This file must not include any internal driver or library files because it will be exported.

  2. sas_robot_driver_myrobot.cpp with the driver’s class implementation. Any internal libraries or drivers must be included here so that they are not externally visible.

  3. sas_robot_driver_myrobot_node.cpp that configures the driver and calls the ROS2 loop.

The creation of the following two is trivial

  1. real_robot_launch.py a suitable launch file to properly configure sas_robot_driver_myrobot_node.cpp.

  2. joint_interface_example.py a Python script to control the C++ node (if needed).

Creating the ROS2 package#

Let’s create all the files used in the remainder of this tutorial.

cd ~/sas_tutorial_workspace/src/sas_robot_driver_myrobot
mkdir -p src
touch src/sas_robot_driver_myrobot.cpp
touch src/sas_robot_driver_myrobot_node.cpp
mkdir -p include/sas_robot_driver_myrobot
touch include/sas_robot_driver_myrobot/sas_robot_driver_myrobot.hpp
mkdir -p launch
touch launch/real_robot_launch.py
mkdir -p scripts
touch scripts/joint_interface_example.py

The subclass of sas::RobotDriver#

In this step, we’ll work on these.

└── sas_robot_driver_myrobot
    ├── CMakeLists.txt
    ├── include
    │   └── sas_robot_driver_myrobot
    │       └── sas_robot_driver_myrobot.hpp
    ├── launch
    │   └── real_robot_launch.py
    ├── package.xml
    ├── scripts
    │   └── joint_interface_example.py
    └── src
        ├── sas_robot_driver_myrobot.cpp
        └── sas_robot_driver_myrobot_node.cpp

The files in question are as follows.

sas_robot_driver_myrobot.hpp

 1#include <atomic>
 2#include <memory>
 3#include <sas_core/sas_robot_driver.hpp>
 4
 5using namespace Eigen;
 6
 7namespace sas
 8{
 9
10struct RobotDriverMyrobotConfiguration
11{
12    std::string ip;
13    std::tuple<VectorXd,VectorXd> joint_limits;
14};
15
16class RobotDriverMyrobot: public RobotDriver
17{
18private:
19    RobotDriverMyrobotConfiguration configuration_;
20
21    //Use the Impl idiom to "hide" internal driver sources
22    class Impl;
23    std::unique_ptr<Impl> impl_;
24public:
25
26    // Prevent copies as usually drivers have threads
27    RobotDriverMyrobot(const RobotDriverMyrobot&)=delete;
28    RobotDriverMyrobot()=delete;
29    ~RobotDriverMyrobot();
30
31    // This boilderplate constructor usually does the job well and prevent big changes when
32    // parameters change
33    RobotDriverMyrobot(const RobotDriverMyrobotConfiguration &configuration, std::atomic_bool* break_loops);
34
35    /// Everything below this line is an override
36    /// the concrete implementations are needed
37    VectorXd get_joint_positions() override;
38    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
39
40    void connect() override;
41    void disconnect() override;
42
43    void initialize() override;
44    void deinitialize() override;
45
46};
47}

sas_robot_driver_myrobot.cpp

  1#include "sas_robot_driver_myrobot/sas_robot_driver_myrobot.hpp"
  2#include <iostream>
  3#include <memory>
  4#include <sas_core/eigen3_std_conversions.hpp>
  5
  6namespace sas
  7{
  8
  9
 10class RobotDriverMyrobot::Impl
 11{
 12
 13public:
 14    bool connected{false};
 15    bool motor_on{false};
 16
 17    VectorXd joint_positions_;
 18
 19
 20    Impl()
 21        {
 22
 23        }
 24
 25};
 26
 27RobotDriverMyrobot::RobotDriverMyrobot(const RobotDriverMyrobotConfiguration& configuration, std::atomic_bool* break_loops):
 28    RobotDriver(break_loops),
 29    configuration_(configuration)
 30{
 31    joint_limits_ = configuration.joint_limits; //for the superclass
 32    impl_ = std::make_unique<RobotDriverMyrobot::Impl>();
 33}
 34
 35RobotDriverMyrobot::~RobotDriverMyrobot()
 36{
 37
 38}
 39
 40/**
 41 * @brief RobotDriverMyrobot::get_joint_positions
 42 * This method should always throw an exception if the user
 43 * tries to obtain the joint positions in an invalid state.
 44 *
 45 * One useful way of defining that is with a VectorXd(), which
 46 * has by default size zero until it is initialized.
 47 *
 48 * @return a VectorXd representing the configuration space in radians.
 49 */
 50VectorXd RobotDriverMyrobot::get_joint_positions()
 51{
 52    if(impl_->joint_positions_.size()==0)
 53        throw std::runtime_error("Tried to obtain invalid joint positions");
 54
 55    return impl_->joint_positions_;
 56}
 57
 58/**
 59 * @brief RobotDriverMyrobot::set_target_joint_positions
 60 * This method expects the desired joint positions in radians. The most basic
 61 * check is for the correct
 62 *
 63 * @param desired_joint_positions_rad
 64 */
 65void RobotDriverMyrobot::set_target_joint_positions(const VectorXd &desired_joint_positions_rad)
 66{
 67    if(desired_joint_positions_rad.size() != 6)
 68        throw std::runtime_error("Incorrect vector size in RobotDriverMyrobot::set_target_joint_positions");
 69
 70    impl_->joint_positions_ = desired_joint_positions_rad;
 71}
 72
 73/**
 74 * @brief RobotDriverMyrobot::connect
 75 *
 76 * Usually this method will connect to a given ip address. It is also common
 77 * for this part of the code to stop running programs or turn the robot off.
 78 * This function is expected to throw an exception of something goes wrong.
 79 * For instance, if the connection is not established an exception MUST
 80 * be thrown.
 81 */
 82void RobotDriverMyrobot::connect()
 83{
 84    //An example of exception to throw.
 85    if(impl_->connected)
 86        throw std::runtime_error("Already connected.");
 87
 88    impl_->connected = true;
 89
 90    impl_->motor_on = false;
 91
 92    //Usually after the connection is established we can read joint positions
 93    //but not all drivers work like this
 94    impl_->joint_positions_ = (VectorXd(6) << 0, 0, 0, 0, 0, 0).finished();
 95}
 96
 97/**
 98 * @brief RobotDriverMyrobot::initialize
 99 *
100 * This method is expected to turn the robot on and initialize the internal joint control loop.
101 * If there are any issues, this MUST throw an exception so that the program will finish
102 * cleanly.
103 *
104 * After this method finishes target joint states can be received.
105 */
106void RobotDriverMyrobot::initialize()
107{
108    impl_->motor_on = true;
109
110
111}
112
113/**
114 * @brief RobotDriverMyrobot::deinitialize.
115 * For safety reasons, this MUST NOT throw exceptions.
116 */
117void RobotDriverMyrobot::deinitialize()
118{
119    impl_->motor_on = false;
120
121}
122
123/**
124 * @brief RobotDriverMyrobot::disconnect
125 * For safety reasons, this MUST NOT throw exceptions.
126 */
127void RobotDriverMyrobot::disconnect()
128{
129    impl_->connected = false;
130    impl_->joint_positions_ = VectorXd();
131}
132
133}

The example class file has three important design choices to note.

First, although self evident, we rely on subclass polymorphism to integrate new classes using the same code. To that end, our class inherits from sas::RobotDriver. You will notice that sas::RobotDriver is defined in the package sas_core. This is because the package sas_core holds every code that does not depend on ROS2. Eventually the idea is to make a standalone package for this part of sas.

#include <sas_core/sas_robot_driver.hpp>

using namespace Eigen;

namespace sas
{

struct RobotDriverMyrobotConfiguration
{
    std::string ip;
    std::tuple<VectorXd,VectorXd> joint_limits;
};

class RobotDriverMyrobot: public RobotDriver

Second, we rely on the struct RobotDriverMyrobotConfiguration to simplify interaction with the constructor. This reduces the amount of code that needs to be changed if a parameter is added or removed.

struct RobotDriverMyrobotConfiguration
{
    std::string ip;
    std::tuple<VectorXd,VectorXd> joint_limits;
};

Third, we rely on the PIMPL idiom. This idiom is important to prevent driver internals to pollute the exported header. This is a very important step to guarantee that your users don’t have to worry about source files specific to the robot and that your package is correctly self-contained. This is an important design aspect and should not be confused simply with aesthetics or my constant need to sound smart.

    //Use the Impl idiom to "hide" internal driver sources
    class Impl;
    std::unique_ptr<Impl> impl_;

When using the PIMPL idiom it is important not to forget that the definition of the implementation class is made in the source. In this example, it is simply a dummy, but in practice this will depend heavily on the robot drivers.

class RobotDriverMyrobot::Impl
{

public:
    bool connected{false};
    bool motor_on{false};

    VectorXd joint_positions_;


    Impl()
        {

        }

};

Writing the ROS2 Node#

In this step, we’ll work on this.

└── sas_robot_driver_myrobot
    ├── CMakeLists.txt
    ├── include
    │   └── sas_robot_driver_myrobot
    │       └── sas_robot_driver_myrobot.hpp
    ├── launch
    │   └── real_robot_launch.py
    ├── package.xml
    ├── scripts
    │   └── joint_interface_example.py
    └── src
        ├── sas_robot_driver_myrobot.cpp
        └── sas_robot_driver_myrobot_node.cpp

sas_robot_driver_myrobot_node.cpp

 1#include <rclcpp/rclcpp.hpp>
 2#include <sas_common/sas_common.hpp>
 3#include <sas_core/eigen3_std_conversions.hpp>
 4#include <sas_robot_driver/sas_robot_driver_ros.hpp>
 5#include <sas_robot_driver_myrobot/sas_robot_driver_myrobot.hpp>
 6#include <dqrobotics/utils/DQ_Math.h>
 7
 8/*********************************************
 9 * SIGNAL HANDLER
10 * *******************************************/
11#include<signal.h>
12static std::atomic_bool kill_this_process(false);
13void sig_int_handler(int)
14{
15    kill_this_process = true;
16}
17
18int main(int argc, char** argv)
19{
20    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
21    {
22        throw std::runtime_error("::Error setting the signal int handler.");
23    }
24
25    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
26
27    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_myrobot");
28
29    try
30    {
31        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Loading parameters from parameter server.");
32
33        sas::RobotDriverMyrobotConfiguration configuration;
34
35        sas::get_ros_parameter(node,"ip",configuration.ip);
36        std::vector<double> joint_limits_min;
37        std::vector<double> joint_limits_max;
38        sas::get_ros_parameter(node,"joint_limits_min",joint_limits_min);
39        sas::get_ros_parameter(node,"joint_limits_max",joint_limits_max);
40        configuration.joint_limits = {deg2rad(sas::std_vector_double_to_vectorxd(joint_limits_min)),
41                                      deg2rad(sas::std_vector_double_to_vectorxd(joint_limits_max))};
42
43        sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
44        sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
45        robot_driver_ros_configuration.robot_driver_provider_prefix = node->get_name();
46
47        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Parameters OK.");
48
49        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Instantiating RobotDriverMyRobot.");
50        auto robot_driver_myrobot = std::make_shared<sas::RobotDriverMyrobot>(configuration,
51                                                                    &kill_this_process);
52
53        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Instantiating RobotDriverROS.");
54        sas::RobotDriverROS robot_driver_ros(node,
55                                             robot_driver_myrobot,
56                                             robot_driver_ros_configuration,
57                                             &kill_this_process);
58        robot_driver_ros.control_loop();
59
60    }
61    catch (const std::exception& e)
62    {
63        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(), std::string("::Exception::") + e.what());
64    }
65
66    return 0;
67}

There are two notable steps for this integration.

First, we configure our newly created RobotDriverMyrobotConfiguration and the existing sas::RobotDriverROSConfiguration by obtaining parameters from ROS2. Using sas::get_ros_parameter to do that reduces the amount of code to write and allows Exception generation and handling.

        sas::RobotDriverMyrobotConfiguration configuration;

        sas::get_ros_parameter(node,"ip",configuration.ip);
        std::vector<double> joint_limits_min;
        std::vector<double> joint_limits_max;
        sas::get_ros_parameter(node,"joint_limits_min",joint_limits_min);
        sas::get_ros_parameter(node,"joint_limits_max",joint_limits_max);
        configuration.joint_limits = {deg2rad(sas::std_vector_double_to_vectorxd(joint_limits_min)),
                                      deg2rad(sas::std_vector_double_to_vectorxd(joint_limits_max))};

        sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
        sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
        robot_driver_ros_configuration.robot_driver_provider_prefix = node->get_name();

        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Parameters OK.");

Second, we create an instance of sas::RobotDriverROS. This class will manage the creation of all ROS2 elements, such as publishers and subscribers, and loop through our sas::RobotDriver subclass. Notice that it has a smart pointer parameter of sas::RobotDriver, so we just need to add as argument any suitable pointer to a subclass of it.

        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Instantiating RobotDriverMyRobot.");
        auto robot_driver_myrobot = std::make_shared<sas::RobotDriverMyrobot>(configuration,
                                                                    &kill_this_process);

        RCLCPP_INFO_STREAM_ONCE(node->get_logger(), "::Instantiating RobotDriverROS.");
        sas::RobotDriverROS robot_driver_ros(node,
                                             robot_driver_myrobot,
                                             robot_driver_ros_configuration,
                                             &kill_this_process);
        robot_driver_ros.control_loop();

Contents of the launch file#

In this step, we’ll work on this.

└── sas_robot_driver_myrobot
    ├── CMakeLists.txt
    ├── include
    │   └── sas_robot_driver_myrobot
    │       └── sas_robot_driver_myrobot.hpp
    ├── launch
    │   └── real_robot_launch.py
    ├── package.xml
    ├── scripts
    │   └── joint_interface_example.py
    └── src
        ├── sas_robot_driver_myrobot.cpp
        └── sas_robot_driver_myrobot_node.cpp

The launch file will be like so.

real_robot_launch.py

 1from launch import LaunchDescription
 2from launch_ros.actions import Node
 3
 4
 5def generate_launch_description():
 6
 7    return LaunchDescription([
 8        Node(
 9            output='screen',
10            emulate_tty=True,
11            package='sas_robot_driver_myrobot',
12            executable='sas_robot_driver_myrobot_node',
13            name='myrobot_1',
14            parameters=[{
15                "ip": "127.0.0.1",
16                "joint_limits_min": [-360.0, -360.0, -360.0, -360.0, -360.0, -720.0],
17                "joint_limits_max": [360.0, 360.0, 360.0, 360.0, 360.0, 720.0],
18                "thread_sampling_time_sec": 0.002 # Robot thread is at 500 Hz
19            }]
20        ),
21
22    ])

The parameters should be no surprise defined as follows. The only one that was not defined by our struct RobotDriverMyrobotConfiguration, namely thread_sampling_time_sec, is a parameter of sas::RobotDriverROSConfiguration that defines the sampling time of the ROS2 loop.

            parameters=[{
                "ip": "127.0.0.1",
                "joint_limits_min": [-360.0, -360.0, -360.0, -360.0, -360.0, -720.0],
                "joint_limits_max": [360.0, 360.0, 360.0, 360.0, 360.0, 720.0],
                "thread_sampling_time_sec": 0.002 # Robot thread is at 500 Hz
            }]

The most memorable aspect with respect to sas is that the name will define the topic prefixes. This is important to match other elements of sas.

            name='myrobot_1',

Running the launch file#

Before running the launch file, remember to build and source

cd ~/sas_tutorial_workspace
colcon build
source install/setup.bash

ros2 launch sas_robot_driver_myrobot real_robot_launch.py

In another terminal

ros2 topic list

will show all the available topics that were created for you, freely. Notice that in none of the source files we created so far had any mention to topics or subscribers. All are created by sas.

/myrobot_1/get/home_states
/myrobot_1/get/joint_positions_max
/myrobot_1/get/joint_positions_min
/myrobot_1/get/joint_states
/myrobot_1/set/clear_positions
/myrobot_1/set/homing_signal
/myrobot_1/set/target_joint_forces
/myrobot_1/set/target_joint_positions
/myrobot_1/set/target_joint_velocities
/parameter_events
/rosout

Accessing through Python#

In this step, we’ll work on this.

└── sas_robot_driver_myrobot
    ├── CMakeLists.txt
    ├── include
    │   └── sas_robot_driver_myrobot
    │       └── sas_robot_driver_myrobot.hpp
    ├── launch
    │   └── real_robot_launch.py
    ├── package.xml
    ├── scripts
    │   └── joint_interface_example.py
    └── src
        ├── sas_robot_driver_myrobot.cpp
        └── sas_robot_driver_myrobot_node.cpp

joint_interface_example.py

 1#!/usr/bin/python3
 2import time
 3
 4from math import sin, pi
 5
 6import numpy
 7from dqrobotics import *  # Despite what PyCharm might say, this is very much necessary or DQs will not be recognized
 8from dqrobotics.utils.DQ_Math import deg2rad
 9
10from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
11from sas_robot_driver import RobotDriverClient
12
13from sas_core import Clock, Statistics
14
15
16def main(args=None):
17    try:
18        rclcpp_init()
19        node = rclcpp_Node("sas_robot_driver_myrobot_joint_space_example_node_cpp")
20
21        # 10 ms clock
22        clock = Clock(0.01)
23        clock.init()
24
25        # Initialize the RobotDriverClient
26        rdi = RobotDriverClient(node, 'myrobot_1')
27
28        # Wait for RobotDriverClient to be enabled
29        while not rdi.is_enabled():
30            rclcpp_spin_some(node)
31            time.sleep(0.1)
32
33        # Get topic information
34        print(f"topic prefix = {rdi.get_topic_prefix()}")
35
36        # Read the values sent by the RobotDriverServer
37        joint_positions = rdi.get_joint_positions()
38        print(f"joint positions = {joint_positions}")
39
40        # For some iterations. Note that this can be stopped with CTRL+C.
41        for i in range(0, 5000):
42            clock.update_and_sleep()
43
44            # Move the joints
45            target_joint_positions = joint_positions + deg2rad([10.0 * sin(i / (50.0 * pi))] * 6)
46            # print(target_joint_positions)
47            rdi.send_target_joint_positions(target_joint_positions)
48
49            rclcpp_spin_some(node)
50
51        # Statistics
52        print("Statistics for the entire loop")
53        print("  Mean computation time: {}".format(clock.get_statistics(
54            Statistics.Mean, Clock.TimeType.Computational)
55        ))
56        print("  Mean idle time: {}".format(clock.get_statistics(
57            Statistics.Mean, Clock.TimeType.Idle)
58        ))
59        print("  Mean effective thread sampling time: {}".format(clock.get_statistics(
60            Statistics.Mean, Clock.TimeType.EffectiveSampling)
61        ))
62
63        rclcpp_shutdown()
64
65    except KeyboardInterrupt:
66        print("Interrupted by user")
67    except Exception as e:
68        print("Unhandled excepts", e)
69
70
71if __name__ == '__main__':
72    main()

With the launch file running in one terminal, open another one and run

ros2 topic echo /myrobot_1/get/joint_states

Then, in yet another terminal, run the sample Python script. Before running the script, remember to build and source

cd ~/sas_tutorial_workspace
colcon build
source install/setup.bash

ros2 run sas_robot_driver_myrobot joint_interface_example.py

The output of joint_interface_example.py will be only descriptive.

joint_interface_example.py output
**************************************************************************
sas::Clock (c) Murilo M. Marinho (murilomarinho.info) 2016-2023 LGPLv3
**************************************************************************
**************************************************************************************
sas::RobotDriverClient (c) Murilo M. Marinho (murilomarinho.info) 2016-2023 LGPLv3
**************************************************************************************
[INFO] [1743579389.703532605] [sas_robot_driver_myrobot_joint_space_example_node_cpp]: ::Initializing RobotDriverClient with prefix myrobot_1
topic prefix = myrobot_1
joint positions = [0. 0. 0. 0. 0. 0.]

While the magic happens in ROS2, therefore in the terminal in which we executed ros2 topic echo

ros2 topic echo output
header:
  stamp:
    sec: 1743579392
    nanosec: 471990465
  frame_id: ''
name: []
position:
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
velocity: []
effort: []
---
header:
  stamp:
    sec: 1743579392
    nanosec: 474015460
  frame_id: ''
name: []
position:
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
velocity: []
effort: []
---
header:
  stamp:
    sec: 1743579392
    nanosec: 475985122
  frame_id: ''
name: []
position:
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
- 0.17141407117840998
velocity: []
effort: []