Revision history [back]

I specified a new node to handle communication to and from the hardware interface. Since the an executor is necessary to spin the node, I defined one in the hardware interface component and executed it in a detached thread.

joint_hardware_interface.hpp


#ifndef JOINT_HARDWARE_INTERFACE
#define JOINT_HARDWARE_INTERFACE

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/actuator_interface.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "joint_limits/joint_limits.hpp"

#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "custom_interfaces/msg/motor_command.hpp"
#include <custom_interfaces msg="" motor_state.hpp="">

namespace join_hardware_interface
{
  class JointComms : public rclcpp::Node
  {
    public:
      /**
       * @brief Constructor a new Grab Joint Comms object
       */
      JointComms();

      /**
       * @brief Create the msg and publish to MCU
       */
      void sendCommand(double position, double velocity, double acceleration);

      /**
       * @brief Position and velocity getters
       */
      double getPosition();
      double getVelocity();
    private:
      double position_;
      double velocity_;
      rclcpp::Subscription<custom_interfaces::msg::motorstate>::SharedPtr stateSub_;
      rclcpp::Publisher<custom_interfaces::msg::motorcommand>::SharedPtr commandPub_;
  };

  class JointHardware : public hardware_interface::ActuatorInterface
  {
    public:
      JointHardware();    
      CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_init(const hardware_interface::HardwareInfo &info);

      std::vector<hardware_interface::stateinterface> export_state_interfaces() override;
      std::vector<hardware_interface::commandinterface> export_command_interfaces() override;
      hardware_interface::return_type read(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;
      hardware_interface::return_type write(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

      std::shared_ptr<grabjointcomms> comms;

    private:
      rclcpp::executors::SingleThreadedExecutor executor_;  //Executor needed to subscriber
      double hw_position_command_;
      double hw_velocity_command_;
      double hw_acceleration_command_;
      double hw_position_state_;
      double hw_velocity_state_;
      int setSpeed_;
      int setAccelaration_;
      bool isActive_;

      joint_limits::JointLimits limits;
  };
}

and in the .cpp file, within the on_init() function, I created the thread.


    comms = std::make_shared<jointcomms>();
    executor_.add_node(comms);
    std::thread([this]() { executor_.spin(); }).detach();

joint_hardware_interface.hpp


#ifndef JOINT_HARDWARE_INTERFACE
#define JOINT_HARDWARE_INTERFACE

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/actuator_interface.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "joint_limits/joint_limits.hpp"

#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "custom_interfaces/msg/motor_command.hpp"
#include <custom_interfaces msg="" motor_state.hpp="">

namespace join_hardware_interface
{
  class JointComms : public rclcpp::Node
  {
    public:
      /**
       * @brief Constructor a new Grab Joint Comms object
       */
      JointComms();

      /**
       * @brief Create the msg and publish to MCU
       */
      void sendCommand(double position, double velocity, double acceleration);

      /**
       * @brief Position and velocity getters
       */
      double getPosition();
      double getVelocity();
    private:
      double position_;
      double velocity_;
      rclcpp::Subscription<custom_interfaces::msg::motorstate>::SharedPtr stateSub_;
      rclcpp::Publisher<custom_interfaces::msg::motorcommand>::SharedPtr commandPub_;
  };

  class JointHardware : public hardware_interface::ActuatorInterface
  {
    public:
      JointHardware();    
      CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_init(const hardware_interface::HardwareInfo &info);

      std::vector<hardware_interface::stateinterface> export_state_interfaces() override;
      std::vector<hardware_interface::commandinterface> export_command_interfaces() override;
      hardware_interface::return_type read(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;
      hardware_interface::return_type write(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

      std::shared_ptr<grabjointcomms> comms;

    private:
      rclcpp::executors::SingleThreadedExecutor executor_;  //Executor needed to subscriber
      double hw_position_command_;
      double hw_velocity_command_;
      double hw_acceleration_command_;
      double hw_position_state_;
      double hw_velocity_state_;
      int setSpeed_;
      int setAccelaration_;
      bool isActive_;

      joint_limits::JointLimits limits;
  };
}

and in the .cpp file, within the on_init() function, I created the thread.


    comms = std::make_shared<jointcomms>();
std::make_shared<JointComms>();
    executor_.add_node(comms);
    std::thread([this]() { executor_.spin(); }).detach();

joint_hardware_interface.hpp


#ifndef JOINT_HARDWARE_INTERFACE
#define JOINT_HARDWARE_INTERFACE

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/actuator_interface.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "joint_limits/joint_limits.hpp"

#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "custom_interfaces/msg/motor_command.hpp"
#include <custom_interfaces msg="" motor_state.hpp="">

namespace join_hardware_interface
{
  class JointComms : public rclcpp::Node
  {
    public:
      /**
       * @brief Constructor a new Grab Joint Comms object
       */
      JointComms();

      /**
       * @brief Create the msg and publish to MCU
       */
      void sendCommand(double position, double velocity, double acceleration);

      /**
       * @brief Position and velocity getters
       */
      double getPosition();
      double getVelocity();
    private:
      double position_;
      double velocity_;
      rclcpp::Subscription<custom_interfaces::msg::motorstate>::SharedPtr stateSub_;
      rclcpp::Publisher<custom_interfaces::msg::motorcommand>::SharedPtr commandPub_;
  };

  class JointHardware : public hardware_interface::ActuatorInterface
  {
    public:
      JointHardware();    
      CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state);
      CallbackReturn on_init(const hardware_interface::HardwareInfo &info);

      std::vector<hardware_interface::stateinterface> export_state_interfaces() override;
      std::vector<hardware_interface::commandinterface> export_command_interfaces() override;
      hardware_interface::return_type read(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;
      hardware_interface::return_type write(
        const rclcpp::Time & time, const rclcpp::Duration & period) override;

      std::shared_ptr<grabjointcomms> std::shared_ptr<JointComms> comms;

    private:
      rclcpp::executors::SingleThreadedExecutor executor_;  //Executor needed to subscriber
      double hw_position_command_;
      double hw_velocity_command_;
      double hw_acceleration_command_;
      double hw_position_state_;
      double hw_velocity_state_;
      int setSpeed_;
      int setAccelaration_;
      bool isActive_;

      joint_limits::JointLimits limits;
  };
}

and in the .cpp file, within the on_init() function, I created the thread.


    comms = std::make_shared<JointComms>();
    executor_.add_node(comms);
    std::thread([this]() { executor_.spin(); }).detach();