[ROS2] Subscriber in component

edit

Configuration

• ROS Distro: Humble
• OS Version: Ubuntu 22.04

Description

Hi, I am trying to create a hardware_interface plugin for ros2_control. I want to create a publisher to write() and a subscriber to read(). I am running into issues setting up the subscriber. Then code is given below.

Code

#include <cstdlib>
#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "custom_interfaces/msg/motor_command.hpp"

#include "test_hardware_interface/test_hardware_interface.hpp"
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"

using namespace hardware_interface;

static const auto LOGGER = rclcpp::get_logger("Grab-Hardware");

namespace test_hardware_interface
{
  //HardwareComms Methods
  HardwareComms::HardwareComms() : Node("joint_comms")
  {
    //Lamda callback for stateSub_
    auto stateCB =
      [this](const std_msgs::msg::Float64::SharedPtr state) -> void
      {
        RCLCPP_INFO(this->get_logger(), "Callback");
        state_ = state->data;
      };
    commandPub_ = this->create_publisher<custom_interfaces::msg::motorcommand>(
      "/test_pub",
      0
    );

    stateSub_ = this->create_subscription<std_msgs::msg::float64>(
      "/test_sub",
      0,
      stateCB
    );
  }

  void HardwareComms::sendCommand(double position, double velocity, double acceleration)
  {
    auto command = custom_interfaces::msg::MotorCommand();
    command.position = position;
    command.velocity = velocity;
    command.acceleration = acceleration;
    commandPub_->publish(command);
  }

  double HardwareComms::getState()
  {
    return state_;
  }

  //HardwareInterface Methods
  CallbackReturn HardwareInterface::on_init(const hardware_interface::HardwareInfo &info)
  {
    RCLCPP_INFO(LOGGER, "Initialising...");
    if(
      hardware_interface::ActuatorInterface::on_init(info) !=
        hardware_interface::CallbackReturn::SUCCESS)
    {
      return hardware_interface::CallbackReturn::ERROR;
    }

    comms = std::make_shared<hardwarecomms>();

    hw_joint_state_           = std::numeric_limits<double>::quiet_NaN();
    hw_position_command_      = std::numeric_limits<double>::quiet_NaN();
    hw_velocity_command_      = std::numeric_limits<double>::quiet_NaN();
    hw_acceleration_command_  = std::numeric_limits<double>::quiet_NaN();

    const ComponentInfo &joint = info_.joints[0];
    if(joint.command_interfaces.size() != 3) {
      RCLCPP_FATAL(
        LOGGER,
        "Joint '%s' has %zu command interfaces found. 3 expected.", joint.name.c_str(),
        joint.command_interfaces.size()
      );
      return hardware_interface::CallbackReturn::ERROR;
    }

    RCLCPP_INFO(LOGGER, "Checking Position IF");
    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
      RCLCPP_FATAL(
        LOGGER,
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION
      );
      return hardware_interface::CallbackReturn::ERROR;
    }
    RCLCPP_INFO(LOGGER, "Clear!");

    RCLCPP_INFO(LOGGER, "Checking Velocity IF");
    if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
      RCLCPP_FATAL(
        LOGGER,
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY
      );
      return hardware_interface::CallbackReturn::ERROR;
    }
    RCLCPP_INFO(LOGGER, "Clear!");

    RCLCPP_INFO(LOGGER, "Checking Acceleration IF");
    if (joint.command_interfaces[2].name != hardware_interface::HW_IF_ACCELERATION) {
      RCLCPP_FATAL(
        LOGGER,
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[2].name.c_str(), hardware_interface::HW_IF_ACCELERATION
      );
      return hardware_interface::CallbackReturn::ERROR;
    }
    RCLCPP_INFO(LOGGER, "Clear!");

    if (joint.state_interfaces.size() != 1) {
      RCLCPP_FATAL(
        LOGGER, "Joint '%s' has %zu state interface. 1 expected.",
        joint.name.c_str(), joint.state_interfaces.size());
      return hardware_interface::CallbackReturn::ERROR;
    }

    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
      RCLCPP_FATAL(
        LOGGER, "Joint '%s' have %s state interface. '%s' expected.",
        joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
        hardware_interface::HW_IF_POSITION);
      return hardware_interface::CallbackReturn::ERROR;
    }

    RCLCPP_INFO(LOGGER, "Done!");
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  CallbackReturn HardwareInterface::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
  {
    RCLCPP_INFO(LOGGER, "Configuring...");
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  std::vector<stateinterface> HardwareInterface::export_state_interfaces()
  {
    std::vector<hardware_interface::stateinterface> state_interfaces;

    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_));

    return state_interfaces;
  }

  std::vector<commandinterface> HardwareInterface::export_command_interfaces()
  {
    std::vector<hardware_interface::commandinterface> command_interfaces;

    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_position_command_));
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_command_));
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[0].name ...