Refactor ROS2 actuator with improved CAN handling and logging

- Introduced `rclcpp::Logger` for consistent logging throughout `RbsServoActuator`.
- Renamed joint state and command variables to `hw_joint_angle`, `hw_joint_velocity`, `hw_joint_target_angle`, and `hw_joint_target_velocity` for clarity.
- Added parameters `can_interface` and `can_node_id`, allowing configurable CAN interface and node ID.
- Enhanced CAN initialization and error handling in `on_init` to properly retrieve interface index and bind the socket.
- Implemented structured CAN message handling:
  - Sends activation command when the actuator is enabled.
  - Reads and logs incoming CAN frames for joint state updates.
  - Ensures error handling for failed read/write operations.
- Updated URDF (`simple_robot.urdf`) to include `can_interface` and `can_node_id` parameters for hardware configuration.

ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp

@@ -3,6 +3,7 @@
 // Libs for fake connection 
 // WARN: Delete it in the future
 #include <netinet/in.h>
+#include <rclcpp/logger.hpp>
 #include <sys/socket.h>
 // System libs
 #include <memory>
@@ -58,11 +59,15 @@ private:
   // Parameters for the rbs_servo_hardware simulation
   // double hw_start_sec_;
   // double hw_stop_sec_;
+  
+  rclcpp::Logger logger_ = rclcpp::get_logger("rbs_servo_hardware");
 
   // Store the command for the robot
-  double hw_joint_command_;
+  double hw_joint_target_angle_;
+  double hw_joint_target_velocity_;
   // Store the states for the robot
-  double hw_joint_state_;
+  double hw_joint_angle_;
+  double hw_joint_velocity_;
 
   // Fake "mechanical connection" between actuator and sensor using sockets
   // struct sockaddr_in address_;
@@ -70,7 +75,8 @@ private:
   // int sockoptval_ = 1;
   // int sock_;
 
-  std::string can_interface_ = "can0";
+  std::string can_interface_;
+  int can_node_id_;
   int can_socket_;
 };
 

ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp

@@ -7,17 +7,19 @@
 #include <cmath>
 #include <limits>
 #include <memory>
+#include <rclcpp_lifecycle/state.hpp>
 #include <vector>
 // ROS specific libs
 #include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include <rclcpp/logger.hpp>
 #include <linux/can.h>
 #include <linux/can/raw.h>
-#include <sys/socket.h>
-#include <sys/ioctl.h>
 #include <net/if.h>
+#include <rclcpp/logger.hpp>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
 #include <unistd.h>
 
 namespace rbs_servo_hardware {
@@ -28,26 +30,29 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  hw_joint_command_ = std::numeric_limits<double>::quiet_NaN();
+  hw_joint_target_angle_ = std::numeric_limits<double>::quiet_NaN();
+
+  can_interface_ = info_.hardware_parameters["can_interface"];
+  can_node_id_ = stoi(info_.hardware_parameters["can_node_id"]);
 
   const hardware_interface::ComponentInfo &joint = info_.joints[0];
   // RbsServoActuator has exactly one command interface and one joint
   if (joint.command_interfaces.size() != 1) {
-    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+    RCLCPP_FATAL(logger_,
                  "Joint '%s' has %zu command interfaces found. 1 expected.",
                  joint.name.c_str(), joint.command_interfaces.size());
     return hardware_interface::CallbackReturn::ERROR;
   }
 
   if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
-    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+    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;
   } else if (joint.state_interfaces[0].name !=
              hardware_interface::HW_IF_POSITION) {
-    RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+    RCLCPP_FATAL(logger_,
                  "Joint '%s' have %s state interface found. '%s' expected.",
                  joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
                  hardware_interface::HW_IF_POSITION);
@@ -55,8 +60,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
   }
 
   if ((can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
-    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"),
+    RCLCPP_ERROR(logger_, "Failed to create CAN socket");
-                 "Failed to create CAN socket");
     return hardware_interface::CallbackReturn::ERROR;
   }
 
@@ -64,8 +68,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
   strncpy(ifr.ifr_name, can_interface_.c_str(), IFNAMSIZ - 1);
   ifr.ifr_name[IFNAMSIZ - 1] = '\0';
   if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) {
-    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"),
+    RCLCPP_ERROR(logger_, "Failed to get interface index");
-                 "Failed to get interface index");
     return hardware_interface::CallbackReturn::ERROR;
   }
 
@@ -73,7 +76,7 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
   addr.can_family = AF_CAN;
   addr.can_ifindex = ifr.ifr_ifindex;
   if (bind(can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
-    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to bind CAN socket");
+    RCLCPP_ERROR(logger_, "Failed to bind CAN socket");
     return hardware_interface::CallbackReturn::ERROR;
   }
 
@@ -85,14 +88,20 @@ RbsServoActuator::on_init(const hardware_interface::HardwareInfo &info) {
   //       server->h_length);
   // address_.sin_port = htons(socket_port_);
 
-  // RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+  // RCLCPP_INFO(logger_,
   //             "Trying to connect to port %d.", socket_port_);
   // if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) {
-  //   RCLCPP_FATAL(rclcpp::get_logger("RbsServoActuator"),
+  //   RCLCPP_FATAL(logger_,
   //                "Connection over socket failed.");
   //   return hardware_interface::CallbackReturn::ERROR;
   // }
-  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Connected to socket");
+  RCLCPP_INFO(logger_, "Connected to socket");
+
+  return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn
+on_configure(const rclcpp_lifecycle::State & /*previous_state*/) {
 
   return hardware_interface::CallbackReturn::SUCCESS;
 }
@@ -110,7 +119,7 @@ RbsServoActuator::export_state_interfaces() {
 
   state_interfaces.emplace_back(hardware_interface::StateInterface(
       info_.joints[0].name, hardware_interface::HW_IF_POSITION,
-      &hw_joint_state_));
+      &hw_joint_angle_));
 
   return state_interfaces;
 }
@@ -121,7 +130,7 @@ RbsServoActuator::export_command_interfaces() {
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
       info_.joints[0].name, hardware_interface::HW_IF_POSITION,
-      &hw_joint_command_));
+      &hw_joint_target_angle_));
 
   return command_interfaces;
 }
@@ -130,24 +139,31 @@ hardware_interface::CallbackReturn RbsServoActuator::on_activate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
   // START: This part here is for exemplary purposes - Please do not copy to
   // your production code
-  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+  RCLCPP_INFO(logger_, "Activating ...please wait...");
-              "Activating ...please wait...");
-
-  // for (int i = 0; i < hw_start_sec_; i++) {
-  //   rclcpp::sleep_for(std::chrono::seconds(1));
-  //   RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds
-  //   left...",
-  //               hw_start_sec_ - i);
-  // }
-  // END: This part here is for exemplary purposes - Please do not copy to your
-  // production code
 
   // set some default values for joints
-  if (std::isnan(hw_joint_command_)) {
+  if (std::isnan(hw_joint_target_angle_)) {
-    hw_joint_command_ = 0;
+    hw_joint_target_angle_ = 0;
-    hw_joint_state_ = 0;
+    hw_joint_angle_ = 0;
   }
 
+  // Создание CAN-кадра для включения двигателя
+  struct can_frame frame;
+  frame.can_id = can_node_id_;  // Идентификатор сообщения (замените на ваш ID)
+  frame.can_dlc = 2;    // Длина данных (1 байт для флага + 1 байт для значения)
+  frame.data[0] = 'E';  // Флаг для команды управления двигателем
+  frame.data[1] = true; // Значение для включения двигателя (true -> 0x01)
+
+  // Отправка сообщения
+  if (::write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) {
+    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"),
+                 "Failed to send CAN frame for motor activation");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+              "Motor activation command sent successfully");
+
   RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
               "Successfully activated!");
 
@@ -158,18 +174,16 @@ hardware_interface::CallbackReturn RbsServoActuator::on_deactivate(
     const rclcpp_lifecycle::State & /*previous_state*/) {
   // START: This part here is for exemplary purposes - Please do not copy to
   // your production code
-  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+  RCLCPP_INFO(logger_, "Deactivating ...please wait...");
-              "Deactivating ...please wait...");
 
   // for (int i = 0; i < hw_stop_sec_; i++) {
   //   rclcpp::sleep_for(std::chrono::seconds(1));
-  //   RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "%.1f seconds
+  //   RCLCPP_INFO(logger_, "%.1f seconds
   //   left...",
   //               hw_stop_sec_ - i);
   // }
 
-  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"),
+  RCLCPP_INFO(logger_, "Successfully deactivated!");
-              "Successfully deactivated!");
   // END: This part here is for exemplary purposes - Please do not copy to your
   // production code
   close(can_socket_);
@@ -179,40 +193,51 @@ hardware_interface::CallbackReturn RbsServoActuator::on_deactivate(
 hardware_interface::return_type
 RbsServoActuator::read(const rclcpp::Time & /*time*/,
                        const rclcpp::Duration & /*period*/) {
-
   struct can_frame frame;
-  if (::read(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) {
+
-    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to read CAN frame");
+  // Чтение кадров в цикле
-  } else {
+  while (::read(can_socket_, &frame, sizeof(frame)) == sizeof(frame)) {
-    float angle;
+    // Проверка флага 'A'
-    std::memcpy(&angle, &frame.data[1], sizeof(float));
+    if (frame.data[0] == 'A') {
-    RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Received CAN frame: angle=%f rad", angle);
+      // Извлекаем угол
-    hw_joint_state_ = static_cast<double>(angle);
+      float angle;
+      std::memcpy(&angle, &frame.data[1], sizeof(float));
+
+      // Логируем полученный угол
+      RCLCPP_DEBUG(logger_, "Received CAN frame with angle: %f rad", angle);
+
+      // Обновляем состояние сустава
+      hw_joint_angle_ = static_cast<double>(angle);
+      return hardware_interface::return_type::OK;
+    } else {
+      // Игнорирование кадров с другими флагами
+      RCLCPP_DEBUG(logger_, "Ignoring CAN frame with unknown flag: %c",
+                   frame.data[0]);
+    }
   }
+
+  if (errno != EAGAIN) {
+    RCLCPP_ERROR(logger_, "Failed to read CAN frame: %s", strerror(errno));
+    return hardware_interface::return_type::ERROR;
+  }
+
   return hardware_interface::return_type::OK;
 }
 
 hardware_interface::return_type rbs_servo_hardware::RbsServoActuator::write(
     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
-  // START: This part here is for exemplary purposes - Please do not copy to
-  // your production code
-  RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Writing command: %f",
-              hw_joint_command_);
 
   struct can_frame frame;
-  frame.can_id = 0x2; // Replace with your desired CAN ID
+  frame.can_id = can_node_id_;
-  
+  float angle = static_cast<float>(hw_joint_target_angle_);
-  float angle = static_cast<double>(hw_joint_command_);
   frame.data[0] = 'A';
-  std::memcpy(&frame.data[1], &angle, sizeof(float)); 
+  std::memcpy(&frame.data[1], &angle, sizeof(float));
   frame.can_dlc = 5;
-  // frame.data[speed_string.size() + 1] = '\0'
-  ; // You'll need to serialize speed to fit CAN frame
 
   if (::write(can_socket_, &frame, sizeof(frame)) != sizeof(frame)) {
-    RCLCPP_ERROR(rclcpp::get_logger("RbsServoActuator"), "Failed to send CAN frame");
+    RCLCPP_ERROR(logger_, "Failed to send CAN frame");
   } else {
-    RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Sent CAN frame: angle=%f rad", angle);
+    RCLCPP_DEBUG(logger_, "Sent CAN frame: angle=%f rad", angle);
   }
 
   return hardware_interface::return_type::OK;

ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf

@@ -41,6 +41,8 @@
   <ros2_control name="simble_robot_hardware_interface" type="actuator">
     <hardware>
       <plugin>rbs_servo_hardware/RbsServoActuator</plugin>
+      <param name="can_interface">can0</param>
+      <param name="can_node_id">1</param>
     </hardware>
     <joint name="joint1">
       <command_interface name="position" />