nils/cyphal_hardware_interface
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fmt

Browse Source
This commit is contained in:
Nils Schulte 2025-07-20 01:18:15 +02:00
parent df45978fa4
commit ed0d1c4d07
2 changed files with 178 additions and 217 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
hardware/.clang-format Normal file
View File

@ -0,0 +1,4 @@
Language: Cpp
BasedOnStyle: google
ColumnLimit: 120

315
hardware/cyphal_system.cpp
View File

@ -1,55 +1,45 @@
#define NUNAVUT_ASSERT assert #define NUNAVUT_ASSERT assert
#include "cyphal_hardware_interface/cyphal_system.hpp" #include "cyphal_hardware_interface/cyphal_system.hpp"
#include "include/cyphal_hardware_interface/cyphal_system.hpp"
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <cstddef> #include <cstddef>
#include <cstdint>
#include <hardware_interface/hardware_info.hpp> #include <hardware_interface/hardware_info.hpp>
#include <iomanip> // For std::setw, std::setfill, std::hex, etc.
#include <iostream>
#include <limits> #include <limits>
#include <memory> #include <memory>
#include <set> #include <set>
#include <sstream> #include <sstream>
#include <sstream> // For std::ostringstream
#include <string> #include <string>
#include <thread>
#include <vector> #include <vector>
#include <thread> #include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "include/cyphal_hardware_interface/cyphal_system.hpp"
#include "rclcpp/rclcpp.hpp"
#include "socketcan.h" #include "socketcan.h"
// #include "serial.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
// #include "cyphal.hpp"
#include <chrono>
#include <cstdint>
#include <iomanip> // For std::setw, std::setfill, std::hex, etc.
#include <iostream>
#include <sstream> // For std::ostringstream
#include <thread>
#define NUNAVUT_ASSERT assert
extern "C" { extern "C" {
#include "canard.h"
#include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h> #include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h>
#include <reg/udral/physics/dynamics/rotation/Planar_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h> #include <reg/udral/service/common/Readiness_0_1.h>
#include <uavcan/_register/Access_1_0.h> #include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h> #include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h> #include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h> #include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/node/port/List_0_1.h> #include <uavcan/node/port/List_0_1.h>
#include "canard.h"
} }
namespace cyphal_hardware_interface { namespace cyphal_hardware_interface {
uint64_t getMonotonicMicroseconds() { uint64_t getMonotonicMicroseconds() {
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
auto micros = std::chrono::duration_cast<std::chrono::microseconds>( auto micros = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
now.time_since_epoch())
.count();
return static_cast<uint64_t>(micros); return static_cast<uint64_t>(micros);
} }
@ -62,39 +52,29 @@ std::string cyphalUdralServo::get_non_read_register_name() const {
return ""; return "";
} }
bool cyphalUdralServo::all_registers_read() const { bool cyphalUdralServo::all_registers_read() const { return state.register_read && get_non_read_register_name() == ""; }
return state.register_read && get_non_read_register_name() == "";
}
cyphalSystemHardware::~cyphalSystemHardware() { cyphalSystemHardware::~cyphalSystemHardware() {
if (can_socket >= 0) { if (can_socket >= 0) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Closing socket of interface '%s'", can_if_.c_str());
"Closing socket of interface '%s'", can_if_.c_str());
// close(serial_port_handle); // close(serial_port_handle);
} }
} }
void cyphalSystemHardware::send(const CanardMicrosecond tx_deadline_usec, void cyphalSystemHardware::send(const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata* const metadata,
const CanardTransferMetadata *const metadata, const size_t payload_size, const void* const payload_data,
const size_t payload_size,
const void *const payload_data,
const CanardMicrosecond now_usec) { const CanardMicrosecond now_usec) {
const struct CanardPayload payload = {.size = payload_size, const struct CanardPayload payload = {.size = payload_size, .data = payload_data};
.data = payload_data}; (void)canardTxPush(&canard_tx_queues[0], &canard, tx_deadline_usec, metadata, payload, now_usec, NULL);
(void)canardTxPush(&canard_tx_queues[0], &canard, tx_deadline_usec, metadata,
payload, now_usec, NULL);
} }
void cyphalSystemHardware::read_register_list_idx(unsigned char node_id, void cyphalSystemHardware::read_register_list_idx(unsigned char node_id, int register_idx) {
int register_idx) {
uavcan_register_List_Request_1_0 msg = {0}; uavcan_register_List_Request_1_0 msg = {0};
msg.index = register_idx; msg.index = register_idx;
const CanardMicrosecond now_usec = getMonotonicMicroseconds(); const CanardMicrosecond now_usec = getMonotonicMicroseconds();
uint8_t serialized uint8_t serialized[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = uavcan_register_List_Request_1_0_serialize_( const int8_t err = uavcan_register_List_Request_1_0_serialize_(&msg, &serialized[0], &serialized_size);
&msg, &serialized[0], &serialized_size);
assert(err >= 0); assert(err >= 0);
if (err >= 0) { if (err >= 0) {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -108,18 +88,14 @@ void cyphalSystemHardware::read_register_list_idx(unsigned char node_id,
} }
} }
void cyphalSystemHardware::send_readiness( void cyphalSystemHardware::send_readiness(enum arming_state arming_state_target) {
enum arming_state arming_state_target) {
reg_udral_service_common_Readiness_0_1 msg; reg_udral_service_common_Readiness_0_1 msg;
msg.value = arming_state_target == ARMED msg.value = arming_state_target == ARMED ? reg_udral_service_common_Readiness_0_1_ENGAGED
? reg_udral_service_common_Readiness_0_1_ENGAGED
: reg_udral_service_common_Readiness_0_1_STANDBY; : reg_udral_service_common_Readiness_0_1_STANDBY;
const CanardMicrosecond now_usec = getMonotonicMicroseconds(); const CanardMicrosecond now_usec = getMonotonicMicroseconds();
uint8_t serialized uint8_t serialized[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = reg_udral_service_common_Readiness_0_1_serialize_( const int8_t err = reg_udral_service_common_Readiness_0_1_serialize_(&msg, &serialized[0], &serialized_size);
&msg, &serialized[0], &serialized_size);
assert(err >= 0); assert(err >= 0);
if (err >= 0) { if (err >= 0) {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -133,9 +109,7 @@ void cyphalSystemHardware::send_readiness(
} }
} }
void cyphalSystemHardware::write_register(cyphalUdralServo &servo, void cyphalSystemHardware::write_register(cyphalUdralServo& servo, std::string name, uavcan_register_Value_1_0 value) {
std::string name,
uavcan_register_Value_1_0 value) {
if (servo.state.requested_register_name != "" || name == "") { if (servo.state.requested_register_name != "" || name == "") {
return; return;
} }
@ -150,11 +124,9 @@ void cyphalSystemHardware::write_register(cyphalUdralServo &servo,
msg.name.name.count = name.length(); msg.name.name.count = name.length();
msg.value = value; msg.value = value;
const CanardMicrosecond now_usec = getMonotonicMicroseconds(); const CanardMicrosecond now_usec = getMonotonicMicroseconds();
uint8_t serialized uint8_t serialized[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = uavcan_register_Access_Request_1_0_serialize_( const int8_t err = uavcan_register_Access_Request_1_0_serialize_(&msg, &serialized[0], &serialized_size);
&msg, &serialized[0], &serialized_size);
assert(err >= 0); assert(err >= 0);
if (err >= 0) { if (err >= 0) {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -168,8 +140,7 @@ void cyphalSystemHardware::write_register(cyphalUdralServo &servo,
} }
} }
void cyphalSystemHardware::read_register(cyphalUdralServo &servo, void cyphalSystemHardware::read_register(cyphalUdralServo& servo, std::string name) {
std::string name) {
uavcan_register_Value_1_0 value; uavcan_register_Value_1_0 value;
uavcan_register_Value_1_0_select_empty_(&value); uavcan_register_Value_1_0_select_empty_(&value);
write_register(servo, name, value); write_register(servo, name, value);
@ -185,8 +156,7 @@ void cyphalSystemHardware::read_thread_static(void *context) {
hardware_interface::CallbackReturn cyphalSystemHardware::on_init( hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
const hardware_interface::HardwareComponentInterfaceParams& params) { const hardware_interface::HardwareComponentInterfaceParams& params) {
if (hardware_interface::SystemInterface::on_init(params) != if (hardware_interface::SystemInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) {
hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
@ -195,17 +165,13 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
if (info_.hardware_parameters.count("can_if") > 0) { if (info_.hardware_parameters.count("can_if") > 0) {
can_if_ = info_.hardware_parameters.at("can_if"); can_if_ = info_.hardware_parameters.at("can_if");
} else { } else {
RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"), "No can interface specified in urdf, using default value");
"No can interface specified in urdf, using default value");
} }
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s", RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s", can_if_.c_str());
can_if_.c_str());
std::vector<hardware_interface::ComponentInfo> components; std::vector<hardware_interface::ComponentInfo> components;
components.insert(components.begin(), info_.joints.begin(), components.insert(components.begin(), info_.joints.begin(), info_.joints.end());
info_.joints.end()); components.insert(components.begin(), info_.sensors.begin(), info_.sensors.end());
components.insert(components.begin(), info_.sensors.begin(),
info_.sensors.end());
components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end()); components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end());
for (const hardware_interface::ComponentInfo& joint : components) { for (const hardware_interface::ComponentInfo& joint : components) {
@ -216,28 +182,16 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
} catch (std::exception& e) { } catch (std::exception& e) {
} }
if (node_id < 0 || node_id >= 0xFF) { if (node_id < 0 || node_id >= 0xFF) {
RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "No node_id '%s' invalid!",
"No node_id '%s' invalid!",
joint.parameters.at("node_id").c_str()); joint.parameters.at("node_id").c_str());
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
servos[node_id].node_id = node_id; servos[node_id].node_id = node_id;
servos[node_id].name = joint.name; servos[node_id].name = joint.name;
} else { } else {
RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "No node_id configured for servo!");
"No node_id configured for servo!");
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
// for (const auto &i : joint.command_interfaces) {
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "%s: %s: %d",
// joint.name.c_str(), i.name.c_str(),
// (int)i.parameters.size());
// }
// for (const auto &i : joint.state_interfaces) {
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "%s: %s: %d",
// joint.name.c_str(), i.name.c_str(),
// (int)i.parameters.size());
// }
} }
canard_memory.deallocate = canardDeallocate; canard_memory.deallocate = canardDeallocate;
@ -246,26 +200,21 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
// The libcanard instance requires the allocator for managing protocol states. // The libcanard instance requires the allocator for managing protocol states.
canard = canardInit(canard_memory); canard = canardInit(canard_memory);
canard_tx_queues[0] = canard_tx_queues[0] = canardTxInit(100, CANARD_MTU_CAN_CLASSIC, canard_memory);
canardTxInit(100, CANARD_MTU_CAN_CLASSIC, canard_memory);
canard.node_id = 100; canard.node_id = 100;
static struct CanardRxSubscription rx; static struct CanardRxSubscription rx;
int8_t res = // int8_t res = //
canardRxSubscribe(&canard, CanardTransferKindMessage, canardRxSubscribe(&canard, CanardTransferKindMessage, uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
if (res < 0) { if (res < 0) {
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
static struct CanardRxSubscription rx_register_list_response_; static struct CanardRxSubscription rx_register_list_response_;
res = // res = //
canardRxSubscribe(&canard, CanardTransferKindResponse, canardRxSubscribe(&canard, CanardTransferKindResponse, uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_1_0_FIXED_PORT_ID_, uavcan_register_List_Response_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&rx_register_list_response_); &rx_register_list_response_);
if (res < 0) { if (res < 0) {
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
@ -273,10 +222,8 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
static struct CanardRxSubscription rx_register_access_Response_; static struct CanardRxSubscription rx_register_access_Response_;
res = // res = //
canardRxSubscribe(&canard, CanardTransferKindResponse, canardRxSubscribe(&canard, CanardTransferKindResponse, uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_1_0_FIXED_PORT_ID_, uavcan_register_Access_Response_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&rx_register_access_Response_); &rx_register_access_Response_);
if (res < 0) { if (res < 0) {
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
@ -284,11 +231,9 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
static struct CanardRxSubscription rx_reg_udral_physics_dynamics_; static struct CanardRxSubscription rx_reg_udral_physics_dynamics_;
res = // res = //
canardRxSubscribe( canardRxSubscribe(&canard, CanardTransferKindMessage, dynamics_state_port,
&canard, CanardTransferKindMessage, dynamics_state_port,
reg_udral_physics_dynamics_rotation_PlanarTs_0_1_EXTENT_BYTES_, reg_udral_physics_dynamics_rotation_PlanarTs_0_1_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx_reg_udral_physics_dynamics_);
&rx_reg_udral_physics_dynamics_);
if (res < 0) { if (res < 0) {
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
@ -298,17 +243,14 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
hardware_interface::CallbackReturn cyphalSystemHardware::on_configure( hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
const rclcpp_lifecycle::State& /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/) {
can_socket = socketcanOpen(can_if_.c_str(), 8); /* MTU == 8 -> classic CAN */ can_socket = socketcanOpen(can_if_.c_str(), 8); /* MTU == 8 -> classic CAN */
if (can_socket < 0) { if (can_socket < 0) {
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "Could not open socket for can interface '%s'",
"Could not open socket for can interface '%s'",
can_if_.c_str()); can_if_.c_str());
return hardware_interface::CallbackReturn::FAILURE; return hardware_interface::CallbackReturn::FAILURE;
} }
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Opened socketcan socket for '%s'", can_if_.c_str());
"Opened socketcan socket for '%s'", can_if_.c_str());
read_thread_handle = std::thread([this]() { read_thread_static(this); }); read_thread_handle = std::thread([this]() { read_thread_static(this); });
@ -320,8 +262,7 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
for (const auto& [id, s] : servos) { for (const auto& [id, s] : servos) {
std::unique_lock lock{s.mtx}; std::unique_lock lock{s.mtx};
if (!s.state.alive || !s.all_registers_read()) { if (!s.state.alive || !s.all_registers_read()) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Waiting for '%d'", id);
"Waiting for '%d'", id);
ready = false; ready = false;
break; break;
} }
@ -330,8 +271,7 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
break; break;
} }
if (std::chrono::steady_clock::now() > start_time + 10s) { if (std::chrono::steady_clock::now() > start_time + 10s) {
RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "Timed out while waiting for servos");
"Timed out while waiting for servos");
return hardware_interface::CallbackReturn::ERROR; return hardware_interface::CallbackReturn::ERROR;
} }
std::this_thread::sleep_for(200ms); std::this_thread::sleep_for(200ms);
@ -340,8 +280,7 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
return hardware_interface::CallbackReturn::SUCCESS; return hardware_interface::CallbackReturn::SUCCESS;
} }
void cyphalSystemHardware::processReceivedTransfer( void cyphalSystemHardware::processReceivedTransfer(const struct CanardRxTransfer* const transfer,
const struct CanardRxTransfer *const transfer,
const CanardMicrosecond now_usec) { const CanardMicrosecond now_usec) {
(void)now_usec; (void)now_usec;
const int node_id = transfer->metadata.remote_node_id; const int node_id = transfer->metadata.remote_node_id;
@ -351,49 +290,37 @@ void cyphalSystemHardware::processReceivedTransfer(
} }
std::unique_lock lock{servos[node_id].mtx}; std::unique_lock lock{servos[node_id].mtx};
if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
uavcan_register_List_Response_1_0 resp; uavcan_register_List_Response_1_0 resp;
memset(&resp, 0, sizeof resp); memset(&resp, 0, sizeof resp);
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (uavcan_register_List_Response_1_0_deserialize_( if (uavcan_register_List_Response_1_0_deserialize_(&resp, (uint8_t*)transfer->payload.data, &size) >= 0) {
&resp, (uint8_t *)transfer->payload.data, &size) >= 0) {
if (resp.name.name.count == 0) { if (resp.name.name.count == 0) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Register list read from '%d'", node_id);
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"Register list read from '%d'", node_id);
servos[node_id].state.register_read = true; servos[node_id].state.register_read = true;
read_register(servos[node_id], read_register(servos[node_id], servos[node_id].get_non_read_register_name());
servos[node_id].get_non_read_register_name());
} else { } else {
char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0};
memcpy(&name[0], resp.name.name.elements, resp.name.name.count); memcpy(&name[0], resp.name.name.elements, resp.name.name.count);
name[resp.name.name.count] = '\0'; name[resp.name.name.count] = '\0';
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "reg_name: %s", name);
"reg_name: %s", name);
uavcan_register_Value_1_0 value; uavcan_register_Value_1_0 value;
uavcan_register_Value_1_0_select_empty_(&value); uavcan_register_Value_1_0_select_empty_(&value);
servos[node_id].registers[name] = value; servos[node_id].registers[name] = value;
read_register_list_idx(node_id, servos[node_id].registers.size()); read_register_list_idx(node_id, servos[node_id].registers.size());
} }
} }
} else if (transfer->metadata.port_id == } else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
uavcan_register_Access_1_0_FIXED_PORT_ID_) {
uavcan_register_Access_Response_1_0 resp; uavcan_register_Access_Response_1_0 resp;
memset(&resp, 0, sizeof resp); memset(&resp, 0, sizeof resp);
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (uavcan_register_Access_Response_1_0_deserialize_( if (uavcan_register_Access_Response_1_0_deserialize_(&resp, (uint8_t*)transfer->payload.data, &size) >= 0) {
&resp, (uint8_t *)transfer->payload.data, &size) >= 0) {
if (servos[node_id].state.requested_register_name.length() == 0) { if (servos[node_id].state.requested_register_name.length() == 0) {
RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "reg_value recieved but non requested!");
"reg_value recieved but non requested!");
return; return;
} }
servos[node_id] servos[node_id].registers[servos[node_id].state.requested_register_name] = resp.value;
.registers[servos[node_id].state.requested_register_name] = RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "reg_value '%s' recieved",
resp.value;
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"reg_value '%s' recieved",
servos[node_id].state.requested_register_name.c_str()); servos[node_id].state.requested_register_name.c_str());
servos[node_id].state.requested_register_name = ""; servos[node_id].state.requested_register_name = "";
if (servos[node_id].state.requested_register_name == "") { if (servos[node_id].state.requested_register_name == "") {
@ -403,29 +330,22 @@ void cyphalSystemHardware::processReceivedTransfer(
} }
} }
} else if (transfer->metadata.transfer_kind == CanardTransferKindMessage) { } else if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
if (transfer->metadata.port_id == uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) {
if (transfer->metadata.port_id == RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "heartbeat of '%d' recieved", node_id);
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"heartbeat of '%d' recieved", node_id);
if (!servos.contains(node_id)) { if (!servos.contains(node_id)) {
return; return;
} }
std::unique_lock lock{servos[node_id].mtx}; std::unique_lock lock{servos[node_id].mtx};
uavcan_node_Heartbeat_1_0 msg; uavcan_node_Heartbeat_1_0 msg;
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (uavcan_node_Heartbeat_1_0_deserialize_( if (uavcan_node_Heartbeat_1_0_deserialize_(&msg, (uint8_t*)transfer->payload.data, &size) >= 0) {
&msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
if (!servos[node_id].state.alive) { if (!servos[node_id].state.alive) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Starting to read register list of '%d'", node_id);
"Starting to read register list of '%d'", node_id);
read_register_list_idx(node_id, 0); read_register_list_idx(node_id, 0);
} }
servos[node_id].state.alive = true; servos[node_id].state.alive = true;
} }
} else if (transfer->metadata.port_id == dynamics_state_port) { } else if (transfer->metadata.port_id == dynamics_state_port) {
if (!servos.contains(node_id)) { if (!servos.contains(node_id)) {
return; return;
} }
@ -433,8 +353,8 @@ void cyphalSystemHardware::processReceivedTransfer(
std::unique_lock lock{servos[node_id].mtx}; std::unique_lock lock{servos[node_id].mtx};
reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg; reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg;
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (reg_udral_physics_dynamics_rotation_PlanarTs_0_1_deserialize_( if (reg_udral_physics_dynamics_rotation_PlanarTs_0_1_deserialize_(&msg, (uint8_t*)transfer->payload.data,
&msg, (uint8_t *)transfer->payload.data, &size) >= 0) { &size) >= 0) {
servos[node_id].state.kinematics = msg.value.kinematics; servos[node_id].state.kinematics = msg.value.kinematics;
} }
} }
@ -444,16 +364,14 @@ void cyphalSystemHardware::processReceivedTransfer(
void cyphalSystemHardware::write_can() { void cyphalSystemHardware::write_can() {
const CanardMicrosecond now_usec = getMonotonicMicroseconds(); const CanardMicrosecond now_usec = getMonotonicMicroseconds();
struct CanardTxQueue* const que = &canard_tx_queues[0]; struct CanardTxQueue* const que = &canard_tx_queues[0];
struct CanardTxQueueItem *tqi = struct CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame.
canardTxPeek(que); // Find the highest-priority frame.
while (tqi != NULL) { while (tqi != NULL) {
// Attempt transmission only if the frame is not yet timed out while waiting // Attempt transmission only if the frame is not yet timed out while waiting
// in the TX queue. Otherwise just drop it and move on to the next one. // in the TX queue. Otherwise just drop it and move on to the next one.
if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) { if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) {
const struct CanardFrame canard_frame = { const struct CanardFrame canard_frame = {
.extended_can_id = tqi->frame.extended_can_id, .extended_can_id = tqi->frame.extended_can_id,
.payload = {.size = tqi->frame.payload.size, .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
.data = tqi->frame.payload.data}};
// const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); // // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); //
// Non-blocking write attempt. // Non-blocking write attempt.
const int16_t result = socketcanPush(can_socket, &canard_frame, const int16_t result = socketcanPush(can_socket, &canard_frame,
@ -475,24 +393,21 @@ void cyphalSystemHardware::write_can() {
void cyphalSystemHardware::read_thread() { void cyphalSystemHardware::read_thread() {
while (true) { while (true) {
struct CanardFrame frame; struct CanardFrame frame;
CanardMicrosecond out_timestamp_usec; CanardMicrosecond out_timestamp_usec;
CanardMicrosecond timeout_usec = 0; CanardMicrosecond timeout_usec = 0;
bool loopback = false; bool loopback = false;
uint8_t payload_buffer[CANARD_MTU_CAN_CLASSIC]; uint8_t payload_buffer[CANARD_MTU_CAN_CLASSIC];
int ret = socketcanPop(can_socket, &frame, &out_timestamp_usec, int ret = socketcanPop(can_socket, &frame, &out_timestamp_usec, sizeof(payload_buffer), payload_buffer,
sizeof(payload_buffer), payload_buffer, timeout_usec, timeout_usec, &loopback);
&loopback);
if (loopback || ret == 0) { if (loopback || ret == 0) {
continue; continue;
} }
if (ret < 0) { if (ret < 0) {
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "socketcanPop error (ret=%d)", ret);
"socketcanPop error (ret=%d)", ret);
return; return;
} }
// RCLCPP_INFO( // RCLCPP_INFO(
@ -516,13 +431,10 @@ void cyphalSystemHardware::read_thread() {
struct CanardRxTransfer transfer; struct CanardRxTransfer transfer;
memset(&transfer, 0, sizeof transfer); memset(&transfer, 0, sizeof transfer);
const int ifidx = 0; /* interface id */ const int ifidx = 0; /* interface id */
const int8_t canard_result = canardRxAccept(&canard, out_timestamp_usec, const int8_t canard_result = canardRxAccept(&canard, out_timestamp_usec, &frame, ifidx, &transfer, NULL);
&frame, ifidx, &transfer, NULL);
if (canard_result > 0) { if (canard_result > 0) {
processReceivedTransfer(&transfer, timestamp_usec); processReceivedTransfer(&transfer, timestamp_usec);
canard.memory.deallocate(canard.memory.user_reference, canard.memory.deallocate(canard.memory.user_reference, transfer.payload.allocated_size, transfer.payload.data);
transfer.payload.allocated_size,
transfer.payload.data);
} else if (canard_result == -CANARD_ERROR_OUT_OF_MEMORY) { } else if (canard_result == -CANARD_ERROR_OUT_OF_MEMORY) {
(void)0; // The frame did not complete a transfer so there is nothing to (void)0; // The frame did not complete a transfer so there is nothing to
// do. // do.
@ -552,8 +464,7 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
// your production code // your production code
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Activating"); RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Activating");
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Successfully activated!");
"Successfully activated!");
int i = 0; int i = 0;
for (auto& [id, s] : servos) { for (auto& [id, s] : servos) {
@ -572,6 +483,12 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
value.natural16.value.count = 1; value.natural16.value.count = 1;
write_register(s, "uavcan.pub.servo.dynamics.id", value); write_register(s, "uavcan.pub.servo.dynamics.id", value);
const uint16_t setpoint_port = 50 + i;
uavcan_register_Value_1_0_select_natural16_(&value);
value.natural16.value.elements[0] = setpoint_port;
value.natural16.value.count = 1;
write_register(s, "uavcan.sub.servo.setpoint.id", value);
i += 1; i += 1;
} }
@ -588,10 +505,9 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
} }
return hardware_interface::CallbackReturn::SUCCESS; return hardware_interface::CallbackReturn::SUCCESS;
} }
hardware_interface::return_type
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/) {
hardware_interface::return_type cyphalSystemHardware::read(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/) {
for (const auto& [name, descr] : joint_state_interfaces_) { for (const auto& [name, descr] : joint_state_interfaces_) {
if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) { if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) {
for (auto& [id, s] : servos) { for (auto& [id, s] : servos) {
@ -601,24 +517,19 @@ cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
break; break;
} }
} }
} else if (descr.get_interface_name() == } else if (descr.get_interface_name() == hardware_interface::HW_IF_VELOCITY) {
hardware_interface::HW_IF_VELOCITY) {
for (auto& [id, s] : servos) { for (auto& [id, s] : servos) {
std::unique_lock lock{s.mtx}; std::unique_lock lock{s.mtx};
if (s.name == descr.get_prefix_name()) { if (s.name == descr.get_prefix_name()) {
set_state<double>(name, set_state<double>(name, (double)s.state.kinematics.angular_velocity.radian_per_second);
(double)s.state.kinematics.angular_velocity.radian_per_second);
break; break;
} }
} }
} else if (descr.get_interface_name() == } else if (descr.get_interface_name() == hardware_interface::HW_IF_ACCELERATION) {
hardware_interface::HW_IF_ACCELERATION) {
for (auto& [id, s] : servos) { for (auto& [id, s] : servos) {
std::unique_lock lock{s.mtx}; std::unique_lock lock{s.mtx};
if (s.name == descr.get_prefix_name()) { if (s.name == descr.get_prefix_name()) {
set_state<double>( set_state<double>(name, (double)s.state.kinematics.angular_acceleration.radian_per_second_per_second);
name,
(double)s.state.kinematics.angular_acceleration.radian_per_second_per_second);
break; break;
} }
} }
@ -628,13 +539,60 @@ cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
hardware_interface::return_type hardware_interface::return_type cyphal_hardware_interface ::cyphalSystemHardware::write(
cyphal_hardware_interface ::cyphalSystemHardware::write(
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
for (auto& [id, s] : servos) { for (auto& [id, s] : servos) {
std::unique_lock lock{s.mtx}; std::unique_lock lock{s.mtx};
} }
for (auto& [id, s] : servos) {
std::unique_lock lock{s.mtx};
auto cmd_pos = joint_command_interfaces_.find(s.name + "/" + hardware_interface::HW_IF_POSITION);
auto cmd_vel = joint_command_interfaces_.find(s.name + "/" + hardware_interface::HW_IF_VELOCITY);
auto cmd_acc = joint_command_interfaces_.find(s.name + "/" + hardware_interface::HW_IF_ACCELERATION);
auto cmd_torque = joint_command_interfaces_.find(s.name + "/" + hardware_interface::HW_IF_EFFORT);
reg_udral_physics_dynamics_rotation_Planar_0_1 msg;
msg.kinematics.angular_position.radian =
cmd_pos != joint_command_interfaces_.end() ? get_command(cmd_pos->first) : NAN;
msg.kinematics.angular_velocity.radian_per_second =
cmd_vel != joint_command_interfaces_.end() ? get_command(cmd_vel->first) : NAN;
msg.kinematics.angular_acceleration.radian_per_second_per_second =
cmd_acc != joint_command_interfaces_.end() ? get_command(cmd_acc->first) : NAN;
msg._torque.newton_meter = cmd_torque != joint_command_interfaces_.end() ? get_command(cmd_torque->first) : NAN;
bool any_valid = !std::isnan(msg._torque.newton_meter) ||
!std::isnan(msg.kinematics.angular_acceleration.radian_per_second_per_second) ||
!std::isnan(msg.kinematics.angular_velocity.radian_per_second) ||
!std::isnan(msg.kinematics.angular_position.radian);
if (!any_valid) {
continue;
}
const CanardMicrosecond now_usec = getMonotonicMicroseconds();
uint8_t serialized[reg_udral_physics_dynamics_rotation_Planar_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized);
const int8_t err =
reg_udral_physics_dynamics_rotation_Planar_0_1_serialize_(&msg, &serialized[0], &serialized_size);
assert(err >= 0);
if (err >= 0) {
if (s.registers.contains("uavcan.sub.servo.setpoint.id") &&
uavcan_register_Value_1_0_is_natural16_(&s.registers["uavcan.sub.servo.setpoint.id"]) &&
s.registers["uavcan.sub.servo.setpoint.id"].natural16.value.count == 1) {
const uint16_t port = s.registers["uavcan.sub.servo.setpoint.id"].natural16.value.elements[0];
const CanardTransferMetadata transfer = {
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindMessage,
.port_id = port,
.remote_node_id = CANARD_NODE_ID_UNSET,
.transfer_id = (CanardTransferID)(next_transfer_id++),
};
send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
}
}
}
send_readiness(ARMED); send_readiness(ARMED);
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
@ -642,5 +600,4 @@ cyphal_hardware_interface ::cyphalSystemHardware::write(
} // namespace cyphal_hardware_interface } // namespace cyphal_hardware_interface
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware, PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware, hardware_interface::SystemInterface)
hardware_interface::SystemInterface)