Acquiring System State Variables from ROS Ecosystem (From other nodes and services) - DIFFBOT

[ImTheMan153]

Hello,

I am trying to implement the controller interface for my differential robot with read motor feedback and setMotorValues function that are present in my read and write functions and the last variable digstate manages another actuator I use in my robot on top of the left wheel and right wheel. I need to read the status of my robot from my ros ecosystem through other nodes via topics and depending on that I would manipulate the last extra variable (digState) I would be sending to my arduino via write function. I should update the ros ecosystem as soon as I get a feedback from the arduino through the last extra variable through read function.

Approach #1:
I tried implementing a subscriber node inside my hardware interface node but the callback function is not called and I noticed that it is kind of a limitation, correct me if I am wrong.

Approach #2:
After that, I also tried implemented a service server node (standalone node) and a a client (in hardware interface) that takes the service request and changes the internal variable digState to write to my teensy MC. Async request doesn't work but waiting till the service call is successful (spin until future) works but it affects the other syncing processes and blocks the entire main thread.

Please let me know how I can to this?

Read and Write Functions:

return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
{   

    if (!arduino_.connected()) {
        return return_type::ERROR;
    }
    arduino_.readMotorFeedback(l_wheel_.turns, l_wheel_.turnsPSec, r_wheel_.turns, r_wheel_.turnsPSec, digState);
    std::stringstream ss;
    // Update the internal states with the received values
    l_wheel_.pos = l_wheel_.calcEncAngle();
    l_wheel_.vel = l_wheel_.calcEncAnglePSec();
    r_wheel_.pos = r_wheel_.calcEncAngle();
    r_wheel_.vel = r_wheel_.calcEncAnglePSec();
    
    if(digState == 1){
        digOnStatus = 0;
        RCLCPP_INFO(logger_, "Did not Receive Request or Finished Digging...");
        std_msgs::msg::String msg;
        msg.data = "Digging completed";
        digging_status_publisher_->publish(msg);
        
        // Reset digState if you only want to publish once per event
        digState = 0;
    }
    return return_type::OK;
}

return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
{   
    if (!arduino_.connected()) {
        return return_type::ERROR;
    }
    arduino_.setMotorValues(l_wheel_.cmd / l_wheel_.rads_per_turn, r_wheel_.cmd / r_wheel_.rads_per_turn, digOnStatus);
    if (dig_on_client_->service_is_ready()) {
        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
        // No need to set fields in the request for SetBool
        auto future = dig_on_client_->async_send_request(request);

        // This waits for the service call to complete, consider handling asynchronously
        if (rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            auto response = future.get();
            digOnStatus = response->success;
                        
            if(digOnStatus == 1){
                digState = 0;
            }
        } else {
            RCLCPP_ERROR(logger_, "Failed to call service get_digOn_status");
            // Handle failure (e.g., set a default behavior for digOnStatus)
        }
    } else {
        RCLCPP_WARN(logger_, "Service 'get_digOn_status' not available.");
        
        // Consider setting a default state or behavior for digOnStatus
    }
    return return_type::OK;
}

If goal is reached digOn becomes true and passed from the server:

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_srvs/srv/set_bool.hpp" // Use SetBool for simplicity, although you might define a custom service

bool digOn = false;

class DigController : public rclcpp::Node {
public:
  DigController() : Node("dig_controller_node") {
    subscription_ = this->create_subscription<std_msgs::msg::String>(
        "robot_status", 10,
        [this](std_msgs::msg::String::SharedPtr msg) {
          if (msg->data == "Goal Reached") {
            digOn = true;
          } else {
            digOn = false;
          }
          RCLCPP_INFO(this->get_logger(), "digOn: %s", digOn ? "true" : "false");
        });
    service_ = this->create_service<std_srvs::srv::SetBool>(
        "get_digOn_status",
        [this](const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
           std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
          RCLCPP_INFO(this->get_logger(), "Handling get_digOn_status request...");
          response->success = digOn;
          response->message = digOn ? "Digging enabled" : "Digging disabled";
          RCLCPP_INFO(this->get_logger(), "Response: %s", response->message.c_str());
        });

  }

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_;
};

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DigController>());
  rclcpp::shutdown();
  return 0;
}

.hFile:

#ifndef DIFFDRIVE_ARDUINO_REAL_ROBOT_H
#define DIFFDRIVE_ARDUINO_REAL_ROBOT_H

#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include "std_msgs/msg/string.hpp"
#include "arduino_comms.h"
#include "config.h"
#include "wheel.h"

using hardware_interface::CallbackReturn;
using hardware_interface::return_type;

namespace diffdrive_arduino {

class DiffDriveArduino : public hardware_interface::SystemInterface {
public:
  DiffDriveArduino();

  CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;

  return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;

  return_type write(const rclcpp::Time &time, const rclcpp::Duration &period) override;

private:
  Config cfg_;
  ArduinoComms arduino_;
  Wheel l_wheel_;
  Wheel r_wheel_;
  int digState = 1;
  int digOnStatus = 0;
  rclcpp::Logger logger_;
  std::shared_ptr<rclcpp::Node> node_;
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr dig_on_client_;
  std::chrono::time_point<std::chrono::system_clock> time_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr digging_status_publisher_;


};

} // namespace diffdrive_arduino

#endif // DIFFDRIVE_ARDUINO_REAL_ROBOT_H

The problem I am facing is acquiring the information from other ros nodes to hardware interface.

[christophfroehlich]

The typical ros2_control approach is to write a controller to interact (from ROS via topics, services) with the hardware component. Have a look at this example of a GPIO controller.

[christophfroehlich]

duplicate of https://robotics.stackexchange.com/questions/110344/ros2-control-acquiring-system-state-variables-from-ros-ecosystem-from-other-no

[ImTheMan153]

Hello @christophfroehlich,
Thank you for your reply. I did the same. I actually used example 14 from ros_demos and wrote my own custom hardware interface. I have edited the same forum with errors. My custom plugin is not getting recognized for some reason. Although I export is perfectly, it reports the following error:
Error:

[ros2_control_node-3] [INFO] [1712445319.643191758] [controller_manager]: Loading controller 'dig_cont'
[ros2_control_node-3] [ERROR] [1712445319.643234762] [controller_manager]: Loader for controller 'dig_cont' (type 'dig_controller/DigController') not found.
[ros2_control_node-3] [INFO] [1712445319.643275038] [controller_manager]: Available classes:
[ros2_control_node-3] [INFO] [1712445319.643287872] [controller_manager]:   diff_drive_controller/DiffDriveController
[ros2_control_node-3] [INFO] [1712445319.643296303] [controller_manager]:   effort_controllers/JointGroupEffortController
[ros2_control_node-3] [INFO] [1712445319.643303796] [controller_manager]:   force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
[ros2_control_node-3] [INFO] [1712445319.643311088] [controller_manager]:   forward_command_controller/ForwardCommandController
[ros2_control_node-3] [INFO] [1712445319.643318217] [controller_manager]:   forward_command_controller/MultiInterfaceForwardCommandController
[ros2_control_node-3] [INFO] [1712445319.643325346] [controller_manager]:   imu_sensor_broadcaster/IMUSensorBroadcaster
[ros2_control_node-3] [INFO] [1712445319.643332235] [controller_manager]:   joint_state_broadcaster/JointStateBroadcaster
[ros2_control_node-3] [INFO] [1712445319.643338880] [controller_manager]:   joint_trajectory_controller/JointTrajectoryController
[ros2_control_node-3] [INFO] [1712445319.643347282] [controller_manager]:   position_controllers/JointGroupPositionController
[ros2_control_node-3] [INFO] [1712445319.643354561] [controller_manager]:   range_sensor_broadcaster/RangeSensorBroadcaster
[ros2_control_node-3] [INFO] [1712445319.643362113] [controller_manager]:   tricycle_controller/TricycleController
[ros2_control_node-3] [INFO] [1712445319.643369659] [controller_manager]:   velocity_controllers/JointGroupVelocityController
[ros2_control_node-3] [INFO] [1712445319.643378865] [controller_manager]:   ackermann_steering_controller/AckermannSteeringController
[ros2_control_node-3] [INFO] [1712445319.643386289] [controller_manager]:   admittance_controller/AdmittanceController
[ros2_control_node-3] [INFO] [1712445319.643393526] [controller_manager]:   bicycle_steering_controller/BicycleSteeringController
[ros2_control_node-3] [INFO] [1712445319.643400569] [controller_manager]:   tricycle_steering_controller/TricycleSteeringController
[spawner-5] [FATAL] [1712445319.670455522] [spawner_dig_cont]: Failed loading controller dig_cont
[ERROR] [spawner-5]: process has died [pid 202902, exit code 1, cmd '/opt/ros/iron/lib/controller_manager/spawner dig_cont --ros-args'].
[ros2_control_node-3] [INFO] [1712445319.900979719] [controller_manager]: Loading controller 'joint_broad'
[spawner-4] [INFO] [1712445319.945799144] [spawner_joint_broad]: Loaded joint_broad
[ros2_control_node-3] [INFO] [1712445319.946341843] [controller_manager]: Loading controller 'diff_cont'
[ros2_control_node-3] [INFO] [1712445319.977945678] [controller_manager]: Configuring controller 'joint_broad'
[ros2_control_node-3] [INFO] [1712445319.978038501] [joint_broad]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-6] [INFO] [1712445319.978685829] [spawner_diff_cont]: Loaded diff_cont
[ros2_control_node-3] [INFO] [1712445320.011346719] [controller_manager]: Configuring controller 'diff_cont'
[spawner-4] [INFO] [1712445320.112175671] [spawner_joint_broad]: Configured and activated joint_broad
[spawner-6] [INFO] [1712445320.178674659] [spawner_diff_cont]: Configured and activated diff_cont

Code:

#include "dig_controller/digcontroller_arduino.h"

#include <sstream>
#include "hardware_interface/lexical_casts.hpp"
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace dig_controller
{

  DigController::DigController()
    : logger_(rclcpp::get_logger("DigController"))
{}

CallbackReturn DigController::on_init(const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::ActuatorInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  RCLCPP_INFO(logger_, "Configuring Actuators...");
  time_ = std::chrono::system_clock::now();
  dcfg_.dig_joint_name = "dig_to_base_link";
  digger_.setup(dcfg_.dig_joint_name);
  cfg_.device = info_.hardware_parameters["device"];
  cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
  cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]);
  arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);  
  RCLCPP_INFO(logger_, "Finished Initializing Actuators...");
  return hardware_interface::CallbackReturn::SUCCESS;
}

CallbackReturn DigController::on_configure(const rclcpp_lifecycle::State& previous_state) {
  (void)previous_state;
  return CallbackReturn::SUCCESS;
}


std::vector<hardware_interface::StateInterface> DigController::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;

  state_interfaces.emplace_back(hardware_interface::StateInterface(dcfg_.dig_joint_name, hardware_interface::HW_IF_POSITION, &digger_.pos));

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface> DigController::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;

  command_interfaces.emplace_back(hardware_interface::CommandInterface(dcfg_.dig_joint_name, hardware_interface::HW_IF_VELOCITY, &digger_.trigger));

  return command_interfaces;
}


hardware_interface::CallbackReturn DigController::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(logger_, "Activating Dig Controller...");
  arduino_.sendEmptyMsg();

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn DigController::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(logger_, "Deactivating Dig Controller...");

  return CallbackReturn::SUCCESS;

  return hardware_interface::CallbackReturn::SUCCESS;
}

return_type DigController::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  if (!arduino_.connected()) {
        return return_type::ERROR;
    }

  arduino_.readActuatorFeedback(digger_.pos);
  /*std::stringstream ss;
  ss.str(""); // Clear the stringstream for reuse
    ss << "Digger Feedback (Received): " << digger_.pos;
    RCLCPP_INFO(logger_, ss.str().c_str());*/
  return hardware_interface::return_type::OK;
}

return_type DigController::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
 if (!arduino_.connected()) {
        return return_type::ERROR;
    }

  arduino_.setActuator(digger_.trigger);
  /*std::stringstream ss;
  ss.str(""); // Clear the stringstream for reuse
    ss << "Trigger Sent: " << digger_.trigger;
    RCLCPP_INFO(logger_, ss.str().c_str());*/
  return hardware_interface::return_type::OK;
}

}  // namespace dig_controller

  
#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  dig_controller::DigController,
  hardware_interface::ActuatorInterface
)