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.
This commit is contained in:
parent
74c5b1d12c
commit
8525854579
3 changed files with 92 additions and 59 deletions
|
|
@ -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_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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"),
|
||||
"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<double>(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<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
|
||||
|
||||
float angle = static_cast<double>(hw_joint_command_);
|
||||
frame.can_id = can_node_id_;
|
||||
float angle = static_cast<float>(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;
|
||||
|
|
|
|||
|
|
@ -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" />
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue