604 lines
25 KiB
C++
604 lines
25 KiB
C++
#define NUNAVUT_ASSERT assert
|
|
#include "cyphal_hardware_interface/cyphal_system.hpp"
|
|
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <hardware_interface/hardware_info.hpp>
|
|
#include <iomanip> // For std::setw, std::setfill, std::hex, etc.
|
|
#include <iostream>
|
|
#include <limits>
|
|
#include <memory>
|
|
#include <set>
|
|
#include <sstream>
|
|
#include <sstream> // For std::ostringstream
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
|
#include "include/cyphal_hardware_interface/cyphal_system.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "socketcan.h"
|
|
|
|
extern "C" {
|
|
#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 <uavcan/_register/Access_1_0.h>
|
|
#include <uavcan/_register/List_1_0.h>
|
|
#include <uavcan/_register/Value_1_0.h>
|
|
#include <uavcan/node/Heartbeat_1_0.h>
|
|
#include <uavcan/node/port/List_0_1.h>
|
|
|
|
#include "canard.h"
|
|
}
|
|
|
|
namespace cyphal_hardware_interface {
|
|
|
|
uint64_t getMonotonicMicroseconds() {
|
|
auto now = std::chrono::steady_clock::now();
|
|
auto micros = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
|
|
return static_cast<uint64_t>(micros);
|
|
}
|
|
|
|
std::string cyphalUdralServo::get_non_read_register_name() const {
|
|
for (const auto& [name, reg] : registers) {
|
|
if (uavcan_register_Value_1_0_is_empty_(®)) {
|
|
return name;
|
|
}
|
|
}
|
|
return "";
|
|
}
|
|
|
|
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());
|
|
// 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,
|
|
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);
|
|
}
|
|
|
|
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_];
|
|
size_t serialized_size = sizeof(serialized);
|
|
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 = {
|
|
.priority = CanardPriorityNominal,
|
|
.transfer_kind = CanardTransferKindRequest,
|
|
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_,
|
|
.remote_node_id = node_id,
|
|
.transfer_id = (CanardTransferID)(next_transfer_id++),
|
|
};
|
|
send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
|
|
}
|
|
}
|
|
|
|
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;
|
|
const CanardMicrosecond now_usec = getMonotonicMicroseconds();
|
|
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);
|
|
assert(err >= 0);
|
|
if (err >= 0) {
|
|
const CanardTransferMetadata transfer = {
|
|
.priority = CanardPriorityNominal,
|
|
.transfer_kind = CanardTransferKindMessage,
|
|
.port_id = readiness_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);
|
|
}
|
|
}
|
|
|
|
void cyphalSystemHardware::write_register(cyphalUdralServo& servo, std::string name, uavcan_register_Value_1_0 value) {
|
|
if (servo.state.requested_register_name != "" || name == "") {
|
|
return;
|
|
}
|
|
servo.state.requested_register_name = name;
|
|
unsigned char node_id = servo.node_id;
|
|
uavcan_register_Access_Request_1_0 msg;
|
|
memset(&msg, 0, sizeof msg);
|
|
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());
|
|
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_];
|
|
size_t serialized_size = sizeof(serialized);
|
|
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 = {
|
|
.priority = CanardPriorityNominal,
|
|
.transfer_kind = CanardTransferKindRequest,
|
|
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_,
|
|
.remote_node_id = node_id,
|
|
.transfer_id = (CanardTransferID)(next_transfer_id++),
|
|
};
|
|
send(now_usec + 1e9, &transfer, serialized_size, &serialized[0], now_usec);
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* @brief Static method to call read_thread via std::thread
|
|
* @param context pointer to this
|
|
*/
|
|
void cyphalSystemHardware::read_thread_static(void* context) {
|
|
static_cast<cyphalSystemHardware*>(context)->read_thread();
|
|
}
|
|
|
|
hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
|
|
const hardware_interface::HardwareComponentInterfaceParams& params) {
|
|
if (hardware_interface::SystemInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) {
|
|
return hardware_interface::CallbackReturn::ERROR;
|
|
}
|
|
|
|
can_if_ = default_if_;
|
|
|
|
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_INFO(rclcpp::get_logger("cyphalSystemHardware"), "If set as %s", can_if_.c_str());
|
|
|
|
std::vector<hardware_interface::ComponentInfo> 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_.gpios.begin(), info_.gpios.end());
|
|
|
|
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) {
|
|
}
|
|
if (node_id < 0 || node_id >= 0xFF) {
|
|
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!");
|
|
return hardware_interface::CallbackReturn::ERROR;
|
|
}
|
|
}
|
|
|
|
canard_memory.deallocate = canardDeallocate;
|
|
canard_memory.allocate = canardAllocate;
|
|
|
|
// 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.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);
|
|
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,
|
|
&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,
|
|
&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_);
|
|
if (res < 0) {
|
|
return hardware_interface::CallbackReturn::ERROR;
|
|
}
|
|
|
|
return hardware_interface::CallbackReturn::SUCCESS;
|
|
} // namespace cyphal_hardware_interface
|
|
|
|
hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
|
|
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'",
|
|
can_if_.c_str());
|
|
return hardware_interface::CallbackReturn::FAILURE;
|
|
}
|
|
|
|
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); });
|
|
|
|
/** wait_for_nodes **/
|
|
using namespace std::chrono_literals;
|
|
auto start_time = std::chrono::steady_clock::now();
|
|
while (true) {
|
|
bool ready = true;
|
|
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);
|
|
ready = false;
|
|
break;
|
|
}
|
|
}
|
|
if (ready) {
|
|
break;
|
|
}
|
|
if (std::chrono::steady_clock::now() > start_time + 10s) {
|
|
RCLCPP_ERROR(rclcpp::get_logger("cyphalSystemHardware"), "Timed out while waiting for servos");
|
|
return hardware_interface::CallbackReturn::ERROR;
|
|
}
|
|
std::this_thread::sleep_for(200ms);
|
|
}
|
|
|
|
return hardware_interface::CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
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) {
|
|
if (!servos.contains(node_id)) {
|
|
return;
|
|
}
|
|
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 (resp.name.name.count == 0) {
|
|
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());
|
|
} 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);
|
|
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_) {
|
|
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 (servos[node_id].state.requested_register_name.length() == 0) {
|
|
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].state.requested_register_name.c_str());
|
|
servos[node_id].state.requested_register_name = "";
|
|
if (servos[node_id].state.requested_register_name == "") {
|
|
auto non_read_register = servos[node_id].get_non_read_register_name();
|
|
read_register(servos[node_id], non_read_register);
|
|
}
|
|
}
|
|
}
|
|
} 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 (!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 (!servos[node_id].state.alive) {
|
|
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;
|
|
}
|
|
|
|
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) {
|
|
servos[node_id].state.kinematics = msg.value.kinematics;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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.
|
|
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}};
|
|
// 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.
|
|
if (result == 0) {
|
|
break; // The queue is full, we will try again on the next iteration.
|
|
}
|
|
if (result < 0) {
|
|
return; // SocketCAN interface failure (link down?)
|
|
}
|
|
}
|
|
|
|
struct CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi);
|
|
canardTxFree(que, &canard, mut_tqi);
|
|
|
|
tqi = canardTxPeek(que);
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
if (loopback || ret == 0) {
|
|
continue;
|
|
}
|
|
|
|
if (ret < 0) {
|
|
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"), "socketcanPop error (ret=%d)", ret);
|
|
return;
|
|
}
|
|
// RCLCPP_INFO(
|
|
// rclcpp::get_logger("cyphalSystemHardware"),
|
|
// "ok %d %dms %d: %02X %02X %02X %02X %02X %02X %02X %02X", ret,
|
|
// (int)out_timestamp_usec / 1000, (int)frame.payload.size,
|
|
// frame.payload.size <= 0 ? 0 : (int)((uint8_t
|
|
// *)frame.payload.data)[0], frame.payload.size <= 1 ? 0 :
|
|
// (int)((uint8_t *)frame.payload.data)[1], frame.payload.size <= 2 ? 0
|
|
// : (int)((uint8_t *)frame.payload.data)[2], frame.payload.size <= 3 ?
|
|
// 0 : (int)((uint8_t *)frame.payload.data)[3], frame.payload.size <= 4
|
|
// ? 0 : (int)((uint8_t *)frame.payload.data)[4], frame.payload.size <=
|
|
// 5 ? 0 : (int)((uint8_t *)frame.payload.data)[5], frame.payload.size
|
|
// <= 6 ? 0 : (int)((uint8_t *)frame.payload.data)[6],
|
|
// frame.payload.size <= 7 ? 0 : (int)((uint8_t
|
|
// *)frame.payload.data)[7]);
|
|
|
|
// The SocketCAN adapter uses the wall clock for timestamping, but we need
|
|
// monotonic. Wall clock can only be used for time synchronization.
|
|
const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds();
|
|
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);
|
|
if (canard_result > 0) {
|
|
processReceivedTransfer(&transfer, timestamp_usec);
|
|
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.
|
|
// 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.
|
|
using namespace std::chrono_literals;
|
|
std::this_thread::sleep_for(10us);
|
|
continue;
|
|
} else {
|
|
assert(false); // No other error can possibly occur at runtime.
|
|
// return hardware_interface::return_type::ERROR;
|
|
return;
|
|
}
|
|
|
|
write_can();
|
|
}
|
|
}
|
|
|
|
hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
|
|
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!");
|
|
|
|
int i = 0;
|
|
for (auto& [id, s] : servos) {
|
|
std::unique_lock lock{s.mtx};
|
|
s.arm_target = ARMED;
|
|
|
|
uavcan_register_Value_1_0 value;
|
|
uavcan_register_Value_1_0_select_natural16_(&value);
|
|
value.natural16.value.elements[0] = readiness_port;
|
|
value.natural16.value.count = 1;
|
|
write_register(s, "uavcan.sub.servo.readiness.id", value);
|
|
|
|
const uint16_t dynamics_port = dynamics_state_port;
|
|
uavcan_register_Value_1_0_select_natural16_(&value);
|
|
value.natural16.value.elements[0] = dynamics_port;
|
|
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;
|
|
}
|
|
|
|
return hardware_interface::CallbackReturn::SUCCESS;
|
|
}
|
|
|
|
hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
|
|
const rclcpp_lifecycle::State& /*previous_state*/) {
|
|
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Deactivating");
|
|
|
|
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_) {
|
|
if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) {
|
|
for (auto& [id, s] : servos) {
|
|
std::unique_lock lock{s.mtx};
|
|
if (s.name == descr.get_prefix_name()) {
|
|
set_state<double>(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) {
|
|
std::unique_lock lock{s.mtx};
|
|
if (s.name == descr.get_prefix_name()) {
|
|
set_state<double>(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) {
|
|
std::unique_lock lock{s.mtx};
|
|
if (s.name == descr.get_prefix_name()) {
|
|
set_state<double>(name, (double)s.state.kinematics.angular_acceleration.radian_per_second_per_second);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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) {
|
|
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
|
|
|
|
#include "pluginlib/class_list_macros.hpp"
|
|
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware, hardware_interface::SystemInterface)
|