How-To: Add New Hardware to ROCKO

This article describes how to add new hardware to ROCKO. This could be a sensor, a controller, or any other device that you would like to integrate in ROCKO’s system.

The following document details two forms of hardware integration: hardware with C/C++ libraries and hardware with Python libraries. Follow the instructions applicable to your device: check what kinds of libraries are provided by default through the manufacturer.

Linux Commands Used In This Article

• mkdir

  • Create a new folder/directory

• cp

  • Copy a file from one place to another

• ls

  • List the files and folders in the given folder

• nano

  • File text editor

Configuring Hardware for Use in ROS2

This section applies to both Python and C/C++ hardware.

Creating the Hardware Configuration

First, we will create the appropriate folders to house the hardware configuration. In this example, we will use the icm20948 IMU.

1. Create the folder for system libraries.
   mkdir ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948

   1. If you are adding a motor component instead, create a folder in the ‘motors’ subfolder rather than ‘sensors’:
      mkdir ~/rocko_env/rocko_env/rocko_env/hardware/motors/MotorName
      
      Remember to create all of the following files in the ‘motors' subfolder in this case. That means replacing all instances of ‘sensors’ to ‘motors’.

2. Create the file to store the hardware’s library logic.
   If you are using a C++ library to interface with your hardware:
   We do this by copying the hardware CPP example file to our new folder and renaming it.
   cp ~/rocko_env/examples/example_hardware/hardware_template.cpp ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/ICM20948.cpp
   
   If you are using a Python library to interface with your hardware:
   We do this by copying the hardware CPP example file to our new folder and renaming it.
   cp ~/rocko_env/examples/example_hardware/py_hardware_template.cpp ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/ICM20948.cpp

3. Create the folder to store headers for the hardware’s library logic.
   mkdir ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/include/rocko_env

4. Create the file to store headers for the hardware’s library logic.
   We do this by copying the hardware HPP example file to our new folder and renaming it.
   cp ~/rocko_env/examples/example_hardware/hardware_template.hpp ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/include/rocko_env/ICM20948.hpp

We create a special “include/rocko_env” folder for each hardware implementation so we can tell ROS2 later where they are located. This is necessary because ROS2 will need to build the code to make it accessible while running.

Next, we will update the contents of each file to reflect the installed hardware.

If you are implementing a motor:

1. Open the hpp file to edit.
   nano ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/include/rocko_env/ICM20948.hpp

2. Follow the instructions inside the file. This means un-commenting the specified lines if you are implementing motor hardware.

   1. “Un-commenting” in this case means removing the double slashes (//) prefixing a given line of code.

If you are implementing a motor or a sensor:

1. Open the cpp file to edit.
   nano ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/ICM20948.cpp

2. We will follow the instructions listed in the TODO from here onward.

   1. Include the .hpp file
      #include “rocko_env/ICM20948.hpp”

   2. Replace <CLASS_NAME> with your class name
      We replace all instances of <CLASS_NAME> with ICM20948.

   3. If implementing a motor, we un-comment the lines underneath the “TODO: Uncomment if implementing a motor” comment.

   4. If implementing a motor, we change SensorInterface to ActuatorInterface at the end of the file.

Updating the ROS2 Configuration

We must update ROS2 configuration files to indicate to the system that new hardware is present, and tell it where the hardware files are stored.

1. Open the ROS2_control xacro file for editing.
   nano ~/rocko_env/rocko_env/rocko_env/description/ros2_control/rocko.ros2_control.xacro

   1. We will add a new <ros2_control> block to account for the new hardware. This means fulfilling the following requirements for your particular hardware implementation:

      1. The <plugin> block should contain the name of the hardware class.

      2. There should be <state_interface> tags for each state interface you intend to export. In the case of a motor controller, this might be <state_interface name="position"/> and <state_interface name="velocity"/>. For our IMU, we export yaw, pitch, and roll angles, so we add 3 state_interface tags.

         1. state interfaces are data points that we “expose” to ROS2, meaning when we define a variable as a state interface, that data point can be used by other hardware and logic.

      3. There should be <command_interface> tags for each command interface you intend to export. In the case of a motor controller, this might be <command_interface name="velocity"/>. For our IMU, we have no command interfaces, so we don’t add any.

         1. command interfaces are data points that we “expose” to ROS2 that can be monitored as well as modified. That means our “command interfaces” are data points that other hardware or controllers might influence.

      4. An example motor configuration:

         <ros2_control name="LeftDriveMotor" type="actuator"> <hardware> <plugin>rocko_env/Motor12VoltQuadEncoder</plugin> <param name="pinNumberSpeed">14</param> <param name="pinNumberDirection">10</param> <param name="invert">true</param> </hardware> <joint name="left_wheel_joint"> <command_interface name="velocity"/> <state_interface name="position"/> <state_interface name="velocity"/> </joint> </ros2_control>

2. Open the hardware XML file for editing.
   nano ~/rocko_env/rocko_env/rocko_env_hardware.xml

   1. We now add a <class> section to define our new hardware.

      <class name="rocko_env/ICM20948" type="rocko_env::ICM20948" base_class_type="hardware_interface::SensorInterface"> <description> A 9-axis IMU for robot orientation </description> </class>

      1. If you are adding motor hardware, remember to replace SensorInterface with ActuatorInterface.

3. Open the CMakeLists file for editing.
   nano ~/rocko_env/rocko_env/CMakeLists.txt

   1. Add the path to the newly created .cpp file to the add_library() section.

      1. In this case, we add our new IMU .cpp underneath the “Hardware .cpp files” section.
         rocko_env/hardware/sensors/icm20948/ICM20948.cpp

   2. Add the following line underneath “paths to hardware include folders”, with the hardware name replaced with your own.
      $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/rocko_env/hardware/sensors/icm20948/include>

   3. Add the ‘include’ directory to the first install() statement.

      1. After adding our IMU include directory, the block now looks like this:

         install( DIRECTORY rocko_env/hardware/motors/include/ rocko_env/hardware/sensors/icm20948/include/ rocko_env/hardware/sensors/quadEncoder/include/ DESTINATION include )

Congrats, ROS2 has now been configured to support your new hardware!

Exporting State and Command Interfaces

This section applies to both Python and C/C++ hardware.

Earlier you determined the state and command interfaces for your system into rocko.ros2_control.xacro. Now, we need to put them into actual code.

1. Open the .hpp file you created earlier.
   In here we will be adding keys and variables for each command and state interface you declared.

2. In the private section of the .hpp file, create a std::string variable and storage variable for each command and state interface you have.
   These keys will be used to help ros2_control access the interfaces when ROCKO is running.
   The storage variables are where we keep the data that comes in through the command interfaces or gets sent out through the state interfaces.
   The name of the std::string variable should follow this convention: <InterfaceName>_KEY, and the value assigned should be the exact text you put in the name="" section of the tag in rocko.ros2_control.xacro.
   The name of the storage variable should follow this convention: _<InterfaceName>.
   The type of these storage variables depends on what kind of data you’re sending in/out. Below is a table with some common data types. Note that there are many other kinds of data that can be sent not listed in this table.

3. Open the .cpp file you created earlier.
   Now that we’ve made keys to access our command and state interfaces and variables to store their values in, we need to use them!

4. If you have state interfaces, navigate to export_state_interfaces(). In here we will expose the storage variables we just made to ros2_control. This will allow ros2_control to get data from our hardware and use it in other places. Each state interface requires one of these lines to expose it to ros2_control:

   state_interfaces.emplace_back(hardware_interface::StateInterface(     <JointName>, <KeyVariable>, &<StorageVariable>));

5. If you have command interfaces, navigate to export_command_interfaces(). In here we will expose the storage variables we just made to ros2_control. This will allow ros2_control to send data to our hardware. Each command interface requires one of these lines to expose it to ros2_control:

   command_interfaces.emplace_back(hardware_interface::CommandInterface(     <JointName>, <KeyVariable>, &<StorageVariable>));

Do I use Python or C++?

The rest of this tutorial is a choose-your-own-adventure, depending on if you’re going to implement your hardware in Python or C++. It can be hard to decide which to use, so here are some things to consider.

Use Python if:

• The library provided for your sensor (if there is one) is written in Python

• You’re newer to writing code

• Your robot’s algorithms can deal with lag (Python runs much slower than C++)

Use C++ if:

• The library provided for your sensor (if there is one) is written in C++

• You have experience with writing code

• Your robot’s algorithms need fast cycle times

If Python fits better with your hardware and system setup, go to the Python section.
If C++ is more your speed, go to the C++ section.

Using Python Hardware Libraries

If you are using a Python library to interface with your new hardware, there are some extra steps to get everything running.

Creating a New Service Message

1. Navigate to the rocko_interface package’s services folder by calling
   cd ~/ROCKO-env/rocko_interfaces/srv/

2. Create a new service message file with the name of your new hardware.
   In our example with icm20948, we’d make that file with
   nano Icm20948Data.srv.

3. Add request and response data to the service message file
   The ros2_control library will send a request to your Python node, and your Python node will send a response back to ros2_control.
   You’ll want to add data to the request section of the message if you want ros2_control to command parts of your hardware. This is a must for motor implementations but can also be useful for sensors that may need to be reset while ROCKO is running.
   You’ll want to add data to the response section if you want to send info back to ros2_control. This is a must for sensor implementations.
   Below is our example with the icm20948:

   --- float64 yaw float64 pitch float64 roll

   Above the --- is where you put the request data and below is where the response data goes. For this implementation, we only send back data; ros2_control doesn’t command the icm20948 at all. You must also give data types for each piece of data in the request and response. You can utilize the standard message package or use others as outlined in this tutorial.

Creating a Python Node

1. Navigate to the folder where you put the CPP file earlier in the tutorial.
   In our example with icm20948, that’s~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948.

2. Create the file to store the python node.
   We do this by copying the hardware Python example file to our new folder and renaming it.
   cp ~/rocko_env/examples/example_hardware/hardware_template.py ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/ICM20948.py

3. Open the Python file to edit.
   nano ~/rocko_env/rocko_env/rocko_env/hardware/sensors/icm20948/ICM20948.py

4. Complete the TODOs in the Python file.

Implementing a Python Node

The Python node is split into two sections: the initialization section and the callback section.

The initialization section, found under def __init__(self):, runs once when ROCKO starts up. Here the hardware is set up and the service that communicates with ros2_control is started.

The callback section, found under def callback(self, request, response):, is what gets run whenever ros2_control sends your hardware a request. In here you’ll either command your hardware to do something or ask it for data that you’ll then send back to ros2_control as a response.

Below is our implementation for the icm20948 sensor

Add Service Message to .hpp file

1. Navigate to and open the .hpp file you made for this hardware.

2. Add this line to the top of the file, where the other #include statements are.
   #include "rocko_interfaces/srv/<ServiceType>.hpp"

3. Replace <ServiceType> with the name of the service message you created in rocko_interfaces

Create Service Client

1. Navigate to the onInit() function in your .cpp file
   In here is where we set up, among other things, the client that will send requests and receive data from the Python node.

2. Complete the TODOs in this function.

3. If your new service message has data in its request, then navigate to the write() function. You may need to uncomment it. In here lives code that will send a request to your Python node and receive a response back from it

   1. Complete the first TODO.

   2. Find the second TODO. Here is where we will put data into the request we will send to your Python node. Oftentimes, this data will come from the command interfaces you exported using export_command_interfaces().

   3. Find the third TODO. You only need to do this step if your service message has data in its response.
      The res variable stores the response data from your Python node; this data can be used for internal calculations for this piece of hardware, but more commonly we’ll want to give it to ros2_control to be used by other hardware or controllers. In that case, you’ll want to store the values coming from the response in the state interface storage variables we made earlier in this tutorial.

   4. As an example, let’s consider a Python node for a motor, which takes in a speed in radians/second. Our exported command interface is of type double to allow us to have decimal values in our speed, and we store that reference in the variable _cmd. Our exported state interface is also of type double, and we store that reference in the variable _speed. Our Python node takes in a request with the desired speed in radians/second. Our Python node will send back its current speed in radians/second in the response. Below is what our service message looks like:

      float64 desiredSpeed --- float64 currentSpeed

      Below is what our write() will look like:

      auto request = std::make_shared<ServiceType::Request>(); // TODO: Fill in data for your request request->desiredSpeed = _cmd; auto result = _client->async_send_request(request); // Wait for the result. if ((rclcpp::spin_until_future_complete(_node, result) == rclcpp::FutureReturnCode::SUCCESS) && result.valid()) { auto res = result.get(); // TODO: Grab the response data and put into variables for you to use _speed = res->currentSpeed } else { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service <ClassName>"); }

Using C/C++ Hardware Libraries

1. Open the .cpp file you created earlier.

2. Determine which functions you need to implement.
   C++ hardware classes have several functions that you can choose to implement depending on what you need. All of the functions are included in the .cpp file you’ve been working in, and the optional ones are commented out. Feel free to delete the ones you don’t need. Below is a table outlining each function and its purpose:

For sample code using these functions, see our motor implementation.