diff --git a/hardware/.clang-format b/hardware/.clang-format new file mode 100644 index 0000000..08e9446 --- /dev/null +++ b/hardware/.clang-format @@ -0,0 +1,4 @@ + +Language: Cpp +BasedOnStyle: google +ColumnLimit: 120 \ No newline at end of file diff --git a/hardware/cyphal_system.cpp b/hardware/cyphal_system.cpp index 6c6a2be..9363a27 100644 --- a/hardware/cyphal_system.cpp +++ b/hardware/cyphal_system.cpp @@ -1,60 +1,50 @@ #define NUNAVUT_ASSERT assert #include "cyphal_hardware_interface/cyphal_system.hpp" -#include "include/cyphal_hardware_interface/cyphal_system.hpp" #include #include #include +#include #include +#include // For std::setw, std::setfill, std::hex, etc. +#include #include #include #include #include +#include // For std::ostringstream #include +#include #include -#include - +#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 "serial.hpp" - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -// #include "cyphal.hpp" -#include -#include -#include // For std::setw, std::setfill, std::hex, etc. -#include -#include // For std::ostringstream -#include - -#define NUNAVUT_ASSERT assert - extern "C" { -#include "canard.h" #include +#include #include #include #include #include #include #include + +#include "canard.h" } namespace cyphal_hardware_interface { uint64_t getMonotonicMicroseconds() { auto now = std::chrono::steady_clock::now(); - auto micros = std::chrono::duration_cast( - now.time_since_epoch()) - .count(); + auto micros = std::chrono::duration_cast(now.time_since_epoch()).count(); return static_cast(micros); } std::string cyphalUdralServo::get_non_read_register_name() const { - for (const auto &[name, reg] : registers) { + for (const auto& [name, reg] : registers) { if (uavcan_register_Value_1_0_is_empty_(®)) { return name; } @@ -62,39 +52,29 @@ std::string cyphalUdralServo::get_non_read_register_name() const { return ""; } -bool cyphalUdralServo::all_registers_read() const { - return state.register_read && get_non_read_register_name() == ""; -} +bool cyphalUdralServo::all_registers_read() const { return state.register_read && get_non_read_register_name() == ""; } cyphalSystemHardware::~cyphalSystemHardware() { if (can_socket >= 0) { - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "Closing socket of interface '%s'", can_if_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Closing socket of interface '%s'", can_if_.c_str()); // close(serial_port_handle); } } -void cyphalSystemHardware::send(const CanardMicrosecond tx_deadline_usec, - const CanardTransferMetadata *const metadata, - const size_t payload_size, - const void *const payload_data, +void cyphalSystemHardware::send(const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata* const metadata, + const size_t payload_size, const void* const payload_data, const CanardMicrosecond now_usec) { - const struct CanardPayload payload = {.size = payload_size, - .data = payload_data}; - (void)canardTxPush(&canard_tx_queues[0], &canard, tx_deadline_usec, metadata, - payload, now_usec, NULL); + const struct CanardPayload payload = {.size = payload_size, .data = payload_data}; + (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, - int register_idx) { +void cyphalSystemHardware::read_register_list_idx(unsigned char node_id, int register_idx) { uavcan_register_List_Request_1_0 msg = {0}; msg.index = register_idx; const CanardMicrosecond now_usec = getMonotonicMicroseconds(); - uint8_t serialized - [uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t serialized[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; size_t serialized_size = sizeof(serialized); - const int8_t err = uavcan_register_List_Request_1_0_serialize_( - &msg, &serialized[0], &serialized_size); + const int8_t err = uavcan_register_List_Request_1_0_serialize_(&msg, &serialized[0], &serialized_size); assert(err >= 0); if (err >= 0) { const CanardTransferMetadata transfer = { @@ -108,18 +88,14 @@ void cyphalSystemHardware::read_register_list_idx(unsigned char node_id, } } -void cyphalSystemHardware::send_readiness( - enum arming_state arming_state_target) { +void cyphalSystemHardware::send_readiness(enum arming_state arming_state_target) { reg_udral_service_common_Readiness_0_1 msg; - msg.value = arming_state_target == ARMED - ? reg_udral_service_common_Readiness_0_1_ENGAGED - : reg_udral_service_common_Readiness_0_1_STANDBY; + msg.value = arming_state_target == ARMED ? reg_udral_service_common_Readiness_0_1_ENGAGED + : reg_udral_service_common_Readiness_0_1_STANDBY; const CanardMicrosecond now_usec = getMonotonicMicroseconds(); - uint8_t serialized - [reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t serialized[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; size_t serialized_size = sizeof(serialized); - const int8_t err = reg_udral_service_common_Readiness_0_1_serialize_( - &msg, &serialized[0], &serialized_size); + const int8_t err = reg_udral_service_common_Readiness_0_1_serialize_(&msg, &serialized[0], &serialized_size); assert(err >= 0); if (err >= 0) { const CanardTransferMetadata transfer = { @@ -133,9 +109,7 @@ void cyphalSystemHardware::send_readiness( } } -void cyphalSystemHardware::write_register(cyphalUdralServo &servo, - std::string name, - uavcan_register_Value_1_0 value) { +void cyphalSystemHardware::write_register(cyphalUdralServo& servo, std::string name, uavcan_register_Value_1_0 value) { if (servo.state.requested_register_name != "" || name == "") { return; } @@ -146,15 +120,13 @@ void cyphalSystemHardware::write_register(cyphalUdralServo &servo, if (name.length() > sizeof msg.name.name.elements - 1) { name.resize(sizeof msg.name.name.elements); } - strcpy((char *)msg.name.name.elements, name.c_str()); + strcpy((char*)msg.name.name.elements, name.c_str()); msg.name.name.count = name.length(); msg.value = value; const CanardMicrosecond now_usec = getMonotonicMicroseconds(); - uint8_t serialized - [uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t serialized[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; size_t serialized_size = sizeof(serialized); - const int8_t err = uavcan_register_Access_Request_1_0_serialize_( - &msg, &serialized[0], &serialized_size); + const int8_t err = uavcan_register_Access_Request_1_0_serialize_(&msg, &serialized[0], &serialized_size); assert(err >= 0); if (err >= 0) { const CanardTransferMetadata transfer = { @@ -168,8 +140,7 @@ void cyphalSystemHardware::write_register(cyphalUdralServo &servo, } } -void cyphalSystemHardware::read_register(cyphalUdralServo &servo, - std::string name) { +void cyphalSystemHardware::read_register(cyphalUdralServo& servo, std::string name) { uavcan_register_Value_1_0 value; uavcan_register_Value_1_0_select_empty_(&value); write_register(servo, name, value); @@ -179,14 +150,13 @@ void cyphalSystemHardware::read_register(cyphalUdralServo &servo, * @brief Static method to call read_thread via std::thread * @param context pointer to this */ -void cyphalSystemHardware::read_thread_static(void *context) { - static_cast(context)->read_thread(); +void cyphalSystemHardware::read_thread_static(void* context) { + static_cast(context)->read_thread(); } hardware_interface::CallbackReturn cyphalSystemHardware::on_init( - const hardware_interface::HardwareComponentInterfaceParams ¶ms) { - if (hardware_interface::SystemInterface::on_init(params) != - hardware_interface::CallbackReturn::SUCCESS) { + const hardware_interface::HardwareComponentInterfaceParams& params) { + if (hardware_interface::SystemInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } @@ -195,49 +165,33 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init( if (info_.hardware_parameters.count("can_if") > 0) { can_if_ = info_.hardware_parameters.at("can_if"); } else { - RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"), - "No can interface specified in urdf, using default value"); + RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"), "No can interface specified in urdf, using default value"); } - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s", - can_if_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s", can_if_.c_str()); std::vector components; - components.insert(components.begin(), info_.joints.begin(), - info_.joints.end()); - components.insert(components.begin(), info_.sensors.begin(), - info_.sensors.end()); + components.insert(components.begin(), info_.joints.begin(), info_.joints.end()); + components.insert(components.begin(), info_.sensors.begin(), info_.sensors.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) { if (joint.parameters.contains("node_id")) { int node_id = -1; try { node_id = std::stoi(joint.parameters.at("node_id")); - } catch (std::exception &e) { + } catch (std::exception& e) { } if (node_id < 0 || node_id >= 0xFF) { - RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), - "No node_id '%s' invalid!", + RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "No node_id '%s' invalid!", joint.parameters.at("node_id").c_str()); return hardware_interface::CallbackReturn::ERROR; } servos[node_id].node_id = node_id; servos[node_id].name = joint.name; } else { - RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), - "No node_id configured for servo!"); + RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "No node_id configured for servo!"); 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; @@ -246,69 +200,57 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_init( // The libcanard instance requires the allocator for managing protocol states. canard = canardInit(canard_memory); - canard_tx_queues[0] = - canardTxInit(100, CANARD_MTU_CAN_CLASSIC, canard_memory); + canard_tx_queues[0] = canardTxInit(100, CANARD_MTU_CAN_CLASSIC, canard_memory); canard.node_id = 100; static struct CanardRxSubscription rx; - int8_t res = // - canardRxSubscribe(&canard, CanardTransferKindMessage, - uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, - uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx); + int8_t res = // + canardRxSubscribe(&canard, CanardTransferKindMessage, uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx); if (res < 0) { return hardware_interface::CallbackReturn::ERROR; } static struct CanardRxSubscription rx_register_list_response_; - res = // - canardRxSubscribe(&canard, CanardTransferKindResponse, - uavcan_register_List_1_0_FIXED_PORT_ID_, - uavcan_register_List_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + res = // + canardRxSubscribe(&canard, CanardTransferKindResponse, uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx_register_list_response_); if (res < 0) { return hardware_interface::CallbackReturn::ERROR; } static struct CanardRxSubscription rx_register_access_Response_; - res = // - canardRxSubscribe(&canard, CanardTransferKindResponse, - uavcan_register_Access_1_0_FIXED_PORT_ID_, - uavcan_register_Access_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + res = // + canardRxSubscribe(&canard, CanardTransferKindResponse, uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx_register_access_Response_); if (res < 0) { return hardware_interface::CallbackReturn::ERROR; } static struct CanardRxSubscription rx_reg_udral_physics_dynamics_; - res = // - canardRxSubscribe( - &canard, CanardTransferKindMessage, dynamics_state_port, - reg_udral_physics_dynamics_rotation_PlanarTs_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &rx_reg_udral_physics_dynamics_); + res = // + canardRxSubscribe(&canard, CanardTransferKindMessage, dynamics_state_port, + reg_udral_physics_dynamics_rotation_PlanarTs_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx_reg_udral_physics_dynamics_); if (res < 0) { return hardware_interface::CallbackReturn::ERROR; } return hardware_interface::CallbackReturn::SUCCESS; -} // namespace cyphal_hardware_interface +} // namespace cyphal_hardware_interface 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 */ if (can_socket < 0) { - RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), - "Could not open socket for can interface '%s'", + RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "Could not open socket for can interface '%s'", can_if_.c_str()); return hardware_interface::CallbackReturn::FAILURE; } - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "Opened socketcan socket for '%s'", can_if_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Opened socketcan socket for '%s'", can_if_.c_str()); read_thread_handle = std::thread([this]() { read_thread_static(this); }); @@ -317,11 +259,10 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure( auto start_time = std::chrono::steady_clock::now(); while (true) { bool ready = true; - for (const auto &[id, s] : servos) { + for (const auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; if (!s.state.alive || !s.all_registers_read()) { - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "Waiting for '%d'", id); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Waiting for '%d'", id); ready = false; break; } @@ -330,8 +271,7 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure( break; } if (std::chrono::steady_clock::now() > start_time + 10s) { - RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), - "Timed out while waiting for servos"); + RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "Timed out while waiting for servos"); return hardware_interface::CallbackReturn::ERROR; } std::this_thread::sleep_for(200ms); @@ -340,9 +280,8 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void cyphalSystemHardware::processReceivedTransfer( - const struct CanardRxTransfer *const transfer, - const CanardMicrosecond now_usec) { +void cyphalSystemHardware::processReceivedTransfer(const struct CanardRxTransfer* const transfer, + const CanardMicrosecond now_usec) { (void)now_usec; const int node_id = transfer->metadata.remote_node_id; if (transfer->metadata.transfer_kind == CanardTransferKindResponse) { @@ -351,49 +290,37 @@ void cyphalSystemHardware::processReceivedTransfer( } std::unique_lock lock{servos[node_id].mtx}; if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { - uavcan_register_List_Response_1_0 resp; memset(&resp, 0, sizeof resp); size_t size = transfer->payload.size; - if (uavcan_register_List_Response_1_0_deserialize_( - &resp, (uint8_t *)transfer->payload.data, &size) >= 0) { + if (uavcan_register_List_Response_1_0_deserialize_(&resp, (uint8_t*)transfer->payload.data, &size) >= 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; - read_register(servos[node_id], - servos[node_id].get_non_read_register_name()); + read_register(servos[node_id], servos[node_id].get_non_read_register_name()); } else { char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; memcpy(&name[0], resp.name.name.elements, resp.name.name.count); name[resp.name.name.count] = '\0'; - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "reg_name: %s", name); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "reg_name: %s", name); uavcan_register_Value_1_0 value; uavcan_register_Value_1_0_select_empty_(&value); servos[node_id].registers[name] = value; read_register_list_idx(node_id, servos[node_id].registers.size()); } } - } else if (transfer->metadata.port_id == - uavcan_register_Access_1_0_FIXED_PORT_ID_) { + } else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { uavcan_register_Access_Response_1_0 resp; memset(&resp, 0, sizeof resp); size_t size = transfer->payload.size; - if (uavcan_register_Access_Response_1_0_deserialize_( - &resp, (uint8_t *)transfer->payload.data, &size) >= 0) { + if (uavcan_register_Access_Response_1_0_deserialize_(&resp, (uint8_t*)transfer->payload.data, &size) >= 0) { if (servos[node_id].state.requested_register_name.length() == 0) { - RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), - "reg_value recieved but non requested!"); + RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "reg_value recieved but non requested!"); return; } - servos[node_id] - .registers[servos[node_id].state.requested_register_name] = - resp.value; - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "reg_value '%s' recieved", + servos[node_id].registers[servos[node_id].state.requested_register_name] = 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 = ""; if (servos[node_id].state.requested_register_name == "") { @@ -403,29 +330,22 @@ void cyphalSystemHardware::processReceivedTransfer( } } } else if (transfer->metadata.transfer_kind == CanardTransferKindMessage) { - - if (transfer->metadata.port_id == - uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) { - - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "heartbeat of '%d' recieved", node_id); + if (transfer->metadata.port_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)) { return; } std::unique_lock lock{servos[node_id].mtx}; uavcan_node_Heartbeat_1_0 msg; size_t size = transfer->payload.size; - if (uavcan_node_Heartbeat_1_0_deserialize_( - &msg, (uint8_t *)transfer->payload.data, &size) >= 0) { + if (uavcan_node_Heartbeat_1_0_deserialize_(&msg, (uint8_t*)transfer->payload.data, &size) >= 0) { if (!servos[node_id].state.alive) { - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "Starting to read register list of '%d'", node_id); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Starting to read register list of '%d'", node_id); read_register_list_idx(node_id, 0); } servos[node_id].state.alive = true; } } else if (transfer->metadata.port_id == dynamics_state_port) { - if (!servos.contains(node_id)) { return; } @@ -433,8 +353,8 @@ void cyphalSystemHardware::processReceivedTransfer( std::unique_lock lock{servos[node_id].mtx}; reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg; size_t size = transfer->payload.size; - if (reg_udral_physics_dynamics_rotation_PlanarTs_0_1_deserialize_( - &msg, (uint8_t *)transfer->payload.data, &size) >= 0) { + if (reg_udral_physics_dynamics_rotation_PlanarTs_0_1_deserialize_(&msg, (uint8_t*)transfer->payload.data, + &size) >= 0) { servos[node_id].state.kinematics = msg.value.kinematics; } } @@ -443,30 +363,28 @@ void cyphalSystemHardware::processReceivedTransfer( void cyphalSystemHardware::write_can() { const CanardMicrosecond now_usec = getMonotonicMicroseconds(); - struct CanardTxQueue *const que = &canard_tx_queues[0]; - struct CanardTxQueueItem *tqi = - canardTxPeek(que); // Find the highest-priority frame. + struct CanardTxQueue* const que = &canard_tx_queues[0]; + struct CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame. while (tqi != NULL) { // 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. if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) { const struct CanardFrame canard_frame = { .extended_can_id = tqi->frame.extended_can_id, - .payload = {.size = tqi->frame.payload.size, - .data = tqi->frame.payload.data}}; + .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}}; // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); // // Non-blocking write attempt. const int16_t result = socketcanPush(can_socket, &canard_frame, - 0); // Non-blocking write attempt. + 0); // Non-blocking write attempt. if (result == 0) { - break; // The queue is full, we will try again on the next iteration. + break; // The queue is full, we will try again on the next iteration. } if (result < 0) { - return; // SocketCAN interface failure (link down?) + return; // SocketCAN interface failure (link down?) } } - struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi); + struct CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi); canardTxFree(que, &canard, mut_tqi); tqi = canardTxPeek(que); @@ -475,24 +393,21 @@ void cyphalSystemHardware::write_can() { void cyphalSystemHardware::read_thread() { while (true) { - struct CanardFrame frame; CanardMicrosecond out_timestamp_usec; CanardMicrosecond timeout_usec = 0; bool loopback = false; uint8_t payload_buffer[CANARD_MTU_CAN_CLASSIC]; - int ret = socketcanPop(can_socket, &frame, &out_timestamp_usec, - sizeof(payload_buffer), payload_buffer, timeout_usec, - &loopback); + int ret = socketcanPop(can_socket, &frame, &out_timestamp_usec, sizeof(payload_buffer), payload_buffer, + timeout_usec, &loopback); if (loopback || ret == 0) { continue; } if (ret < 0) { - RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), - "socketcanPop error (ret=%d)", ret); + RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "socketcanPop error (ret=%d)", ret); return; } // RCLCPP_INFO( @@ -516,28 +431,25 @@ void cyphalSystemHardware::read_thread() { struct CanardRxTransfer transfer; memset(&transfer, 0, sizeof transfer); const int ifidx = 0; /* interface id */ - const int8_t canard_result = canardRxAccept(&canard, out_timestamp_usec, - &frame, ifidx, &transfer, NULL); + const int8_t canard_result = canardRxAccept(&canard, out_timestamp_usec, &frame, ifidx, &transfer, NULL); if (canard_result > 0) { processReceivedTransfer(&transfer, timestamp_usec); - canard.memory.deallocate(canard.memory.user_reference, - transfer.payload.allocated_size, - transfer.payload.data); + canard.memory.deallocate(canard.memory.user_reference, transfer.payload.allocated_size, transfer.payload.data); } else if (canard_result == -CANARD_ERROR_OUT_OF_MEMORY) { - (void)0; // The frame did not complete a transfer so there is nothing to - // do. + (void)0; // The frame did not complete a transfer so there is nothing to + // do. // OOM should never occur if the heap is sized correctly. You can track // OOM errors via heap API. RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "OOM!"); return; } else if (canard_result == 0) { - (void)0; // The frame did not complete a transfer so there is nothing to - // do. + (void)0; // The frame did not complete a transfer so there is nothing to + // do. using namespace std::chrono_literals; std::this_thread::sleep_for(10us); continue; } else { - assert(false); // No other error can possibly occur at runtime. + assert(false); // No other error can possibly occur at runtime. // return hardware_interface::return_type::ERROR; return; } @@ -547,16 +459,15 @@ void cyphalSystemHardware::read_thread() { } hardware_interface::CallbackReturn cyphalSystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to // your production code RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Activating"); - RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), - "Successfully activated!"); + RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Successfully activated!"); int i = 0; - for (auto &[id, s] : servos) { + for (auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; s.arm_target = ARMED; @@ -572,6 +483,12 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_activate( value.natural16.value.count = 1; 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; } @@ -579,46 +496,40 @@ hardware_interface::CallbackReturn cyphalSystemHardware::on_activate( } hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Deactivating"); - for (auto &[id, s] : servos) { + for (auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; s.arm_target = DISARMED; } return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type -cyphalSystemHardware::read(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - for (const auto &[name, descr] : joint_state_interfaces_) { +hardware_interface::return_type cyphalSystemHardware::read(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { + for (const auto& [name, descr] : joint_state_interfaces_) { if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) { - for (auto &[id, s] : servos) { + for (auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; if (s.name == descr.get_prefix_name()) { set_state(name, (double)s.state.kinematics.angular_position.radian); break; } } - } else if (descr.get_interface_name() == - hardware_interface::HW_IF_VELOCITY) { - for (auto &[id, s] : servos) { + } else if (descr.get_interface_name() == hardware_interface::HW_IF_VELOCITY) { + for (auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; if (s.name == descr.get_prefix_name()) { - set_state(name, - (double)s.state.kinematics.angular_velocity.radian_per_second); + set_state(name, (double)s.state.kinematics.angular_velocity.radian_per_second); break; } } - } else if (descr.get_interface_name() == - hardware_interface::HW_IF_ACCELERATION) { - for (auto &[id, s] : servos) { + } else if (descr.get_interface_name() == hardware_interface::HW_IF_ACCELERATION) { + for (auto& [id, s] : servos) { std::unique_lock lock{s.mtx}; if (s.name == descr.get_prefix_name()) { - set_state( - name, - (double)s.state.kinematics.angular_acceleration.radian_per_second_per_second); + set_state(name, (double)s.state.kinematics.angular_acceleration.radian_per_second_per_second); break; } } @@ -628,19 +539,65 @@ cyphalSystemHardware::read(const rclcpp::Time & /*time*/, return hardware_interface::return_type::OK; } -hardware_interface::return_type -cyphal_hardware_interface ::cyphalSystemHardware::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - - for (auto &[id, s] : servos) { +hardware_interface::return_type cyphal_hardware_interface ::cyphalSystemHardware::write( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + for (auto& [id, s] : servos) { 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); return hardware_interface::return_type::OK; } -} // namespace cyphal_hardware_interface +} // namespace cyphal_hardware_interface #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware, - hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware, hardware_interface::SystemInterface)