Subscriber in Hardware Interface (ros2_control)

Question

Is there any example for adding subscriber to hardware interface ? Publisher works fine. My code is below, I dont know where its going wrong but unable to get values after publishing using CLI.

.hpp file '''

namespace diffdrive_ctre { class HardwareCommandPub : public rclcpp::Node { public:

HardwareCommandPub();
void publishData();
void pincmdcheck();

private: rclcpp::Publisher<amr_v4_msgs_srvs::msg::Motor>::SharedPtr pub_; rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr estop_pub_; rclcpp::Subscription<amr_v4_msgs_srvs::msg::Pin>::SharedPtr pin_sub_; rclcpp::TimerBase::SharedPtr timer_; amr_v4_msgs_srvs::msg::Pin latest_pin_cmd; };

class DiffDriveCTREHardware : public hardware_interface::SystemInterface {

''' .cpp file

HardwareCommandPub::HardwareCommandPub() : Node("Motor_Status") {

pub_ = this->create_publisher<amr_v4_msgs_srvs::msg::Motor>("talonFX_right/status", 10); estop_pub_ = this->create_publisher<std_msgs::msg::Bool>("/e_stop", 10); pin_sub_ = this->create_subscription<amr_v4_msgs_srvs::msg::Pin>("/amr/pin_cmd", 10, [this](const amr_v4_msgs_srvs::msg::Pin::SharedPtr pin_cmd) { latest_pin_cmd = *pin_cmd; });

}

void HardwareCommandPub::publishData() { // //motor1 auto message = amr_v4_msgs_srvs::msg::Motor(); message.temperature_right = motor_right.GetDeviceTemp().ToString(); message.bus_voltage_right = motor_right.GetSupplyVoltage().ToString();
message.output_current_right = motor_right.GetStatorCurrent().ToString(); message.error_right = motor_right.GetStickyFault_Hardware().ToString();

// //motor2 message.temperature_left = motor_left.GetDeviceTemp().ToString(); message.bus_voltage_left = motor_left.GetSupplyVoltage().ToString();
message.output_current_left = motor_left.GetStatorCurrent().ToString(); message.error_left = motor_left.GetStickyFault_Hardware().ToString();

// //motor pin   message.temperature_pin = pin_motor.GetDeviceTemp().ToString();   message.bus_voltage_pin =

pin_motor.GetSupplyVoltage().ToString(); message.output_current_pin = pin_motor.GetStatorCurrent().ToString(); message.error_pin = pin_motor.GetStickyFault_Hardware().ToString();

//estop auto message_estop = std_msgs::msg::Bool();
message_estop.data = estop_temp; if (rclcpp::ok()) { estop_pub_->publish(message_estop); pub_->publish(message); pincmdcheck(); } }

void HardwareCommandPub::pincmdcheck() { pin_temp = latest_pin_cmd.pin_command; RCLCPP_INFO( rclcpp::get_logger("Pin test"), "Got pin cmd '%i'and '%i' and '%s'!",pin_temp,latest_pin_cmd.pin_command); }

Answer 1

Have a look at this implementation of topic_based_ros2_control here.

Note that in general, it is not recommended to use a node inside the hardware interface if you have realtime requirements. Write your custom controller with realtime-safe implementations instead.