Using ros2 control hardware interface make the communication slow

Question

I'm writing the hardware interface for controlling my robot's motor via ros2 humble in ubuntu 22.04. I receive the manual from the hardware developer with hexadecimal command set to enable_torque, write, and read position of the motor. For example, to send the command, I need to send a the list of target position for all the motors (18 motors for my robot) in each timestep. So I wrote the serial port cpp code for testing the command and features. The code is shown below.

#ifndef YONAKA_COMM_HPP
#define YONAKA_COMM_HPP

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <iostream>
#include <vector>
#include <numeric>
#include <cmath>
#include <string>
#include <chrono>
#include <thread>

class YonakaComm 
{
public:
    YonakaComm();

    // Initialize the serial port
    bool init(const char* device_name = "/dev/ttyUSB0", uint32_t baud_rate = 115200);

    // Torque on/off command
    bool torqueOnOff(bool onoff);

    // Read motor position
    float readPosition(uint8_t index);

    // Write position command for all motors
    void writePosition(const std::vector<double>& pos_command);

    void closePort();

private:
    int serial_port_;

    // Function to calculate checksum (SUM)
    std::string read_confirmation(int serial_port);

    // Function to calculate checksum (SUM)
    uint8_t calculate_checksum(const std::vector<uint8_t>& message);

    // Function to convert motor value (2 bytes) back to radians
    double motor_value_to_rad(uint16_t motor_value);

    // Function to convert radians to motor value (2 bytes)
    uint16_t rad_to_motor_value(double rad);

    // Function to send read command
    void send_read_command();

    // Map baud rate to termios flag
    speed_t baud_rate_to_flag(uint32_t baud_rate);
};

#endif // YONAKA_COMM_HPP



// Constructor
YonakaComm::YonakaComm() : serial_port_(-1) {}

// Initialize the serial port
bool YonakaComm::init(const char* device_name, uint32_t baud_rate) {
    // Open the serial port
    serial_port_ = open(device_name, O_RDWR);

    if (serial_port_ < 0) {
        std::cerr << "Error opening serial port: " << device_name << std::endl;
        return false;
    }
    
    // fcntl(serial_port_, F_SETFL);
    struct termios tty;
    tcgetattr(serial_port_, &tty);

    // Set Baud rate
    cfsetispeed(&tty, baud_rate_to_flag(baud_rate));
    cfsetospeed(&tty, baud_rate_to_flag(baud_rate));

    // Configure port
    tty.c_cflag &= ~PARENB;  // No parity
    tty.c_cflag &= ~CSTOPB;  // 1 stop bit
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;      // 8 data bits
    tty.c_cflag &= ~CRTSCTS; // No hardware flow control
    tty.c_cflag |= CREAD | CLOCAL;  // Enable receiver, ignore status lines

    tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // Disable input/output flow control
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);

    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // Disable canonical input, echo, signals

    tty.c_oflag &= ~OPOST;   // Disable output processing

    tty.c_cc[VMIN] = 1;     // Min number of characters to read
    tty.c_cc[VTIME] = 0;     // No read timeout

    tcsetattr(serial_port_, TCSANOW, &tty);
    tcflush(serial_port_, TCIFLUSH);

    std::cout << "Serial port initialized: " << device_name << " at baud rate " << baud_rate << std::endl;
    return true;
}

// Torque on/off command
bool YonakaComm::torqueOnOff(bool onoff) {
    std::vector<uint8_t> message = {0xff, 3, 0x10}; // Command to turn torque on/off
    message.push_back(onoff ? 1 : 0);  // Torque On: 1, Torque Off: 0

    // Calculate and append the checksum (exclude header)
    uint8_t checksum = calculate_checksum(std::vector<uint8_t>(message.begin() + 1, message.end()));
    message.push_back(checksum);

    // Write the message to the serial port
    tcflush(serial_port_, TCIFLUSH);
    if (write(serial_port_, message.data(), message.size()) < 0) {
        std::cerr << "Failed to write to serial port" << std::endl;
        return false;
    }

    std::cout << "Torque " << (onoff ? "On" : "Off") << " command sent" << std::endl;

    // for (auto byte : message) {
    //     std::cout << "0x" << std::hex << static_cast<int>(byte) << " ";
    // }
    // std::cout << std::dec << std::endl;

    // Read confirmation from the motor
    // std::string confirmation = read_confirmation(serial_port_);
    // std::cout << "Confirmation received: " << confirmation << std::endl;

    return true;
}


// Read motor position
float YonakaComm::readPosition(uint8_t index) {
    // Send the read command
    send_read_command();

    // Buffer to store the data
    uint8_t buffer[40];  // Increase the buffer size to ensure it captures all the data

    // Read from the serial port
    int bytes_read = read(serial_port_, buffer, sizeof(buffer));

    // Handle errors during read
    if (bytes_read < 0) {
        std::cerr << "Failed to read motor positions" << std::endl;
        return -1;
    }
    
    // Assuming that the data starts at buffer[3] and is 2 bytes per motor
    if (2 * index + 4 >= bytes_read) {
        std::cerr << "Index out of range or incomplete data received" << bytes_read << std::endl;
        return -1;
    }

    // Combine high and low byte to get the motor value
    uint16_t motor_value = (buffer[2 * index + 3] << 8) | buffer[2 * index + 4];

    // Convert motor value to radians
    double motor_position_rad = motor_value_to_rad(motor_value);

    tcflush(serial_port_, TCIFLUSH);
    
    // Return the motor position in radians
    return static_cast<float>(motor_position_rad);
}


// Write position command for all motors
void YonakaComm::writePosition(const std::vector<double>& pos_command) {
    std::vector<uint8_t> message = {0xff, 38, 0x11};  // Header and command

    for (double pos : pos_command) {
        uint16_t motor_value = rad_to_motor_value(pos);
        message.push_back((motor_value >> 8));  // High byte
        message.push_back(motor_value & 0xff);         // Low byte
        // std::cout << pos_command[17] << std::endl;
    }

    

    uint8_t checksum = calculate_checksum(std::vector<uint8_t>(message.begin() + 1, message.end())); // Exclude header
    message.push_back(checksum);

    tcflush(serial_port_, TCIFLUSH);
    write(serial_port_, message.data(), message.size());

    std::cout << pos_command[17] << std::endl;
}

// std::string read_confirmation(int serial_port) {
//     char buf[256];
//     int bytes_read = read(serial_port, buf, sizeof(buf) - 1);
//     if (bytes_read < 0) {
//         std::cerr << "Error reading from serial port" << std::endl;
//         return "";
//     }
//     buf[bytes_read] = '\0';  // Null-terminate the string
//     return std::string(buf);
// }

// Function to calculate checksum (SUM)
uint8_t YonakaComm::calculate_checksum(const std::vector<uint8_t>& message) {
    return std::accumulate(message.begin(), message.end(), 0) & 0xff;
}

// Function to convert motor value (2 bytes) back to radians
double YonakaComm::motor_value_to_rad(uint16_t motor_value) {
    return (static_cast<double>(motor_value) / 65535.0) * 191.0 - 95.5; // Scale to radians
}

// Function to convert radians to 16-bit motor value based on provided rules
uint16_t YonakaComm::rad_to_motor_value(double radians) {
    // Radians range from -95.5 to 95.5, mapped to 0x0000 to 0xFFFF
    // Applying the linear transformation
    uint16_t motor_value = static_cast<uint16_t>(((radians + 95.5) / 191.0) * 0xFFFF);
    return motor_value;
}

// Function to send read command
void YonakaComm::send_read_command() {
    std::vector<uint8_t> message = {0xff, 2, 0x21};  // Header and read command
    uint8_t checksum = calculate_checksum(std::vector<uint8_t>(message.begin() + 1, message.end())); // Exclude header
    message.push_back(checksum);

    tcflush(serial_port_, TCIFLUSH);
    write(serial_port_, message.data(), message.size());
    // std::cout << "Read command sent" << std::endl;
}

// Map baud rate to termios flag
speed_t YonakaComm::baud_rate_to_flag(uint32_t baud_rate) {
    switch (baud_rate) {
        case 9600: return B9600;
        case 19200: return B19200;
        case 38400: return B38400;
        case 57600: return B57600;
        case 115200: return B115200;
        default: return B115200;
    }
}

// Close the serial port
void YonakaComm::closePort() {
    if (serial_port_ != -1) {
        close(serial_port_);
        serial_port_ = -1;
        std::cout << "Serial port closed." << std::endl;
    }
}

I tested this, and each function works fine. However, I would like to integrate this to the ros2 control framework, the hardware interface is shown here

// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "yonaka_hardware/yonaka_hardware.hpp"

#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <vector>
#include <string>

#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

#define COMMAND_INTERFACE_NUM 1 // pos
#define STATE_INTERFACE_NUM 1 // pos
#define ARM_JOINT_NUM 3
#define LEG_JOINT_NUM 3

#define FUTURE false

namespace yonaka_hardware
{
constexpr const char * kYonakaHardware = "YonakaHardware";

CallbackReturn YonakaHardware::on_init(const hardware_interface::HardwareInfo & info)
{
  RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "Initializing hardware...");
  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
    return CallbackReturn::ERROR;
  }

  // Retrieve infomation from URDF
  joints_.resize(info_.joints.size(), Joint());
  joint_index_.resize(info_.joints.size(), 0);

  for (uint i = 0; i < info_.joints.size(); i++) {
    joint_index_[i] = std::stoi(info_.joints[i].parameters.at("index"));

    joints_[i].state.position = std::numeric_limits<double>::quiet_NaN();
    // joints_[i].state.velocity = std::numeric_limits<double>::quiet_NaN();
    // joints_[i].state.effort = std::numeric_limits<double>::quiet_NaN();

    joints_[i].command.position = std::numeric_limits<double>::quiet_NaN();
    // joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
    // joints_[i].command.effort = std::numeric_limits<double>::quiet_NaN();

    joints_[i].prev_command.position = joints_[i].command.position;
    // joints_[i].prev_command.velocity = joints_[i].command.velocity;
    // joints_[i].prev_command.effort = joints_[i].command.effort;
    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "joint_index %d: %d", i, joint_index_[i]);
  }

  // Verify the setting of "use_dummy", "usb_port", "baud_rate" parameters.
  if (
    info_.hardware_parameters.find("use_dummy") != info_.hardware_parameters.end() &&
    info_.hardware_parameters.at("use_dummy") == "true")
  {
    use_dummy_ = true;
    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "dummy mode");
    return CallbackReturn::SUCCESS;
  }

  return CallbackReturn::SUCCESS;
}

CallbackReturn YonakaHardware::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Configuring hardware...");

    auto usb_port = info_.hardware_parameters.at("usb_port");
    auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate"));

    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "usb_port: %s", usb_port.c_str());
    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "baud_rate: %d", baud_rate);

    if (!yonaka_comm_.init(usb_port.c_str(), baud_rate))
    {
        RCLCPP_ERROR(rclcpp::get_logger("YonakaHardware"), "Failed to initialize serial communication");
        return CallbackReturn::ERROR;
    }

    // Optionally set initial parameters (e.g., torque off)
    if (!yonaka_comm_.torqueOnOff(false))
    {
        RCLCPP_ERROR(rclcpp::get_logger("YonakaHardware"), "Failed to disable torque during configuration");
        return CallbackReturn::ERROR;
    }

    RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Hardware configuration successful.");
    return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> YonakaHardware::export_state_interfaces()
{
  RCLCPP_DEBUG(rclcpp::get_logger(kYonakaHardware), "export_state_interfaces");
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    state_interfaces.emplace_back(
      hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].state.position));
#if FUTURE
    state_interfaces.emplace_back(
      hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].state.velocity));
    state_interfaces.emplace_back(
      hardware_interface::StateInterface(
        info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &joints_[i].state.effort));
#endif
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> YonakaHardware::export_command_interfaces()
{
  RCLCPP_DEBUG(rclcpp::get_logger(kYonakaHardware), "export_command_interfaces");
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++) {
    command_interfaces.emplace_back(
      hardware_interface::CommandInterface(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joints_[i].command.position));
#if FUTURE
    command_interfaces.emplace_back(
      hardware_interface::CommandInterface(
        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joints_[i].command.velocity));
#endif
  }

  return command_interfaces;
}

CallbackReturn YonakaHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Activating ...please wait...");

  enable_torque(true);

  RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Successfully activated!");

  return CallbackReturn::SUCCESS;
}

CallbackReturn YonakaHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Deactivating ...please wait...");

  enable_torque(false);

  RCLCPP_DEBUG(rclcpp::get_logger(kYonakaHardware), "stop");

  RCLCPP_INFO(rclcpp::get_logger("YonakaHardware"), "Successfully deactivated!");

  return CallbackReturn::SUCCESS;
}

return_type YonakaHardware::read(
  const rclcpp::Time & /*time*/,
  const rclcpp::Duration & /*period*/)
{
  if (use_dummy_) {
    return return_type::OK;
  }

  std::vector<uint8_t> index(info_.joints.size(), 0);

  std::copy(joint_index_.begin(), joint_index_.end(), index.begin());

  for (uint i = 0; i < index.size(); i++) {
    joints_[i].state.position = yonaka_comm_.readPosition(index[i]);
    // joints_[i].state.velocity = yonaka_comm_.getVelocity(index[i]);
    // joints_[i].state.effort = yonaka_comm_.getCurrent(currents[i]);
  }

  RCLCPP_INFO_ONCE(rclcpp::get_logger("YonakaHardware"), "Reading motor positions...");

  return return_type::OK;
}

return_type YonakaHardware::write(
  const rclcpp::Time & /*time*/,
  const rclcpp::Duration & /*period*/)
{
  if (use_dummy_) {
    for (auto & joint : joints_) {
      joint.prev_command.position = joint.command.position;
      joint.state.position = joint.command.position;
    }
    return return_type::OK;
  }

  // Position control
  if (std::any_of(joints_.cbegin(), joints_.cend(), [](auto j) {
        return j.command.position != j.prev_command.position;
      }))
  {
    // set_control_mode(ControlMode::Position);
    set_joint_positions();
    return return_type::OK;
  }

  return return_type::OK;
}

return_type YonakaHardware::enable_torque(const bool enabled)
{
  if (enabled && !torque_enabled_) {
    if (!yonaka_comm_.torqueOnOff(enabled)) {
      RCLCPP_FATAL(rclcpp::get_logger(kYonakaHardware), "Failed to On torque");
      return return_type::ERROR;
    }

    reset_command();
    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "Torque enabled");

  } else if (!enabled && torque_enabled_) {
    if (!yonaka_comm_.torqueOnOff(enabled)) {
      RCLCPP_FATAL(rclcpp::get_logger(kYonakaHardware), "Failed to Off torque");
      return return_type::ERROR;
    }
    RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "Torque disabled");
  }

  torque_enabled_ = enabled;
  return return_type::OK;
}

return_type YonakaHardware::reset_command()
{
  for (uint i = 0; i < joints_.size(); i++) {
    joints_[i].command.position = joints_[i].state.position;
    // joints_[i].command.velocity = 0.0;
    // joints_[i].command.effort = 0.0;

    joints_[i].prev_command.position = joints_[i].command.position;
    // joints_[i].prev_command.velocity = joints_[i].command.velocity;
    // joints_[i].prev_command.effort = joints_[i].command.effort;
  }

  return return_type::OK;
}

CallbackReturn YonakaHardware::set_joint_positions()
{
  std::vector<double> commands(info_.joints.size(), 0.0);
  std::vector<uint8_t> index(info_.joints.size(), 0);

  // Copy joint index
  std::copy(joint_index_.begin(), joint_index_.end(), index.begin());

  for (uint i = 0; i < index.size(); i++) {
    joints_[i].prev_command.position = joints_[i].command.position;
    commands[i] = joints_[i].command.position;
    // RCLCPP_INFO(rclcpp::get_logger(kYonakaHardware), "%f", commands[i]);
  }

#if FUTURE
  // Convert the int32_t commands to a double vector
  std::vector<double> double_commands(commands.size());
  std::transform(commands.begin(), commands.end(), double_commands.begin(), [](int32_t c) {
    return static_cast<double>(c);
  });
#endif

  // Now pass the vector of doubles to writePosition
  yonaka_comm_.writePosition(commands);

  return CallbackReturn::SUCCESS;
}

#if FUTURE
CallbackReturn YonakaHardware::set_joint_params()
{
  const char * log = nullptr;
  for (uint i = 0; i < info_.joints.size(); ++i) {
    for (auto paramName : kExtraJointParameters) {
      if (info_.joints[i].parameters.find(paramName) != info_.joints[i].parameters.end()) {
        auto value = std::stoi(info_.joints[i].parameters.at(paramName));
        if (!yonaka_comm_.itemWrite(joint_index_[i], paramName, value, &log)) {
          RCLCPP_FATAL(rclcpp::get_logger(kYonakaHardware), "%s", log);
          return CallbackReturn::ERROR;
        }
        RCLCPP_INFO(
          rclcpp::get_logger(
            kYonakaHardware), "%s set to %d for joint %d", paramName, value, i);
      }
    }
  }
  return CallbackReturn::SUCCESS;
}
#endif

}  // namespace yonaka_hardware

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(yonaka_hardware::YonakaHardware, hardware_interface::SystemInterface)

Everything works fine, I could launch the controller, read the motor position and sync with the robot_state in Rviz and I can send the command via topic to move the motor. However, the action of the real hardware is super delay and not smooth, and if I keep publishing the topic until some point, the motor will freeze and cannot control any more (need to restart the ros2 controller).

I'm not sure that my code is not written or constructed well or not. When I write my custom ros2 node to send the command to move the motor directly, without ros2_control framework, I can move the motor smoothly and continuously with the same publishing rate. So if someone have the same problem and can see where I was missing, please guide me for the improvement, please.

I'm using joint trajectory controller to control the robot. Here is my config file

# Controller manager configuration
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_true: false

    ### Controllers available
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    position_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


position_trajectory_controller:
  ros__parameters:
    joints:
      - f_left_leg_joint1
      - f_left_leg_joint2
      - f_left_leg_joint3
      - f_right_leg_joint1
      - f_right_leg_joint2
      - f_right_leg_joint3
      - b_left_leg_joint1
      - b_left_leg_joint2
      - b_left_leg_joint3
      - b_right_leg_joint1
      - b_right_leg_joint2
      - b_right_leg_joint3
      - left_arm_joint1
      - left_arm_joint2
      - left_arm_joint3
      - right_arm_joint1
      - right_arm_joint2
      - right_arm_joint3

    command_interfaces:
      - position

    state_interfaces:
      - position

    state_publish_rate: 50.0 # Hz, Defaults to 50
    action_monitor_rate: 20.0 # Hz, Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: true
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

My robot is using ESP32 for micro controller, the motor is Steadywin Brushless motor. The baudrate is set at 1152000.

Thank you very much for your time and support! I'm sorry if some of the part is not described well, let's me know if you need additional information.

Answer 1

I encountered the same issue before. The root cause is that serial communication is handled directly in the hardware interface's read and write methods. This approach leads to delays and blocking behavior because serial communication is slow.

To resolve this, implement a separate thread dedicated to serial communication. This thread will handle all communication with the hardware, running at a high frequency to ensure real-time response. In addition to that, maintain a RobotStatus class to manage the robot's current state, including updated speed commands and positions. You can set an update_threshold time and make the serial calls based on the difference between the time now and the last update time.

Here’s how the solution works:

• Communication Thread: This thread continuously sends and receives data with the serial hardware, updating the RobotStatus class with the latest positions and executing the latest desired speed or position commands. This way decouples the communication process from the ros control loop.

• Write Method: Instead of directly sending commands via serial in the write method, simply update the desired positions in the RobotStatus class. Now, it is the job of the communication thread to execute those desired positions using serial commands.

• Read Method: In the read method, retrieve the positions directly from the RobotStatus class. These positions are already updated by the communication thread, which means the read method just gets the last retrieved data by the communication thread. There will be no latency due to hardware communicaiton here too.

After separating the communication logic into its own thread, I achieved 50+ Hz frequency. It could have been faster, but this was enough for my application. It may also help to profile your serial communication to understand the delays. I benchmarked communication by tracking requests and responses and counted the missed responses. This was using CAN communication, may be different for serial but there is a way fot sure. This is a very interesting task!

Answer 2

My guess is that the update rate of 100Hz is too high for your hardware_component/serial port. Try to reduce the parameter and see if it runs smoother. We don't have a mechanism implemented which tells you if any component or controller does not finish within time.