From 852585457977a56ba746114c7c90fcc8c4ecabe3 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 4 Feb 2025 13:41:56 +0300 Subject: [PATCH] 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. --- .../rbs_servo_hardware/rbs_servo_actuator.hpp | 12 +- .../src/rbs_servo_actuator.cpp | 137 +++++++++++------- .../src/rbs_servo_test/urdf/simple_robot.urdf | 2 + 3 files changed, 92 insertions(+), 59 deletions(-) diff --git a/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp b/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp index 8bca87b..58b3896 100644 --- a/ros2_environment/src/rbs_servo_hardware/include/rbs_servo_hardware/rbs_servo_actuator.hpp +++ b/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 +#include #include // System libs #include @@ -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_; }; diff --git a/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp index cb48ae7..fef7045 100644 --- a/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp +++ b/ros2_environment/src/rbs_servo_hardware/src/rbs_servo_actuator.cpp @@ -7,17 +7,19 @@ #include #include #include +#include #include // 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 #include #include -#include -#include #include +#include +#include +#include #include 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::quiet_NaN(); + hw_joint_target_angle_ = std::numeric_limits::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"), - "Failed to create CAN socket"); + RCLCPP_ERROR(logger_, "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"), - "Failed to get interface index"); + RCLCPP_ERROR(logger_, "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"), - "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 + RCLCPP_INFO(logger_, "Activating ...please wait..."); // set some default values for joints - if (std::isnan(hw_joint_command_)) { - hw_joint_command_ = 0; - hw_joint_state_ = 0; + if (std::isnan(hw_joint_target_angle_)) { + hw_joint_target_angle_ = 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"), - "Deactivating ...please wait..."); + RCLCPP_INFO(logger_, "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"), - "Successfully deactivated!"); + RCLCPP_INFO(logger_, "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 { - float angle; - std::memcpy(&angle, &frame.data[1], sizeof(float)); - RCLCPP_INFO(rclcpp::get_logger("RbsServoActuator"), "Received CAN frame: angle=%f rad", angle); - hw_joint_state_ = static_cast(angle); + + // Чтение кадров в цикле + while (::read(can_socket_, &frame, sizeof(frame)) == sizeof(frame)) { + // Проверка флага 'A' + if (frame.data[0] == 'A') { + // Извлекаем угол + 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(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 - - float angle = static_cast(hw_joint_command_); + frame.can_id = can_node_id_; + float angle = static_cast(hw_joint_target_angle_); 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; diff --git a/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf index 5e9c1bc..a46738a 100644 --- a/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf +++ b/ros2_environment/src/rbs_servo_test/urdf/simple_robot.urdf @@ -41,6 +41,8 @@ rbs_servo_hardware/RbsServoActuator + can0 + 1