nils/cyphal_hardware_interface
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
cyphal_hardware_interface/hardware/cyphal_system.cpp
Nils Schulte d81c099dea wip
2025-07-19 00:48:37 +02:00

474 lines
18 KiB
C++
Raw Blame History

#define NUNAVUT_ASSERT assert
#include "cyphal_hardware_interface/cyphal_system.hpp"
#include <chrono>
#include <cmath>
#include <cstddef>
#include <hardware_interface/hardware_info.hpp>
#include <limits>
#include <memory>
#include <set>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#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 <iostream>
#include <thread>
#include <cstdint>
#define NUNAVUT_ASSERT assert
extern "C" {
#include "canard.h"
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/node/port/List_0_1.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);
}
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_list(
) {
uavcan_register_List_Request_1_0 msg = {0};
msg.index = 0;
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);
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 = 125,
.transfer_id =
(CanardTransferID)(next_transfer_id++),
};
send(now_usec +
1e9, // Set transmission deadline 1 second, optimal for heartbeat.
&transfer, serialized_size, &serialized[0], now_usec);
}
}
/**
* @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::HardwareInfo &info) {
if (hardware_interface::SystemInterface::on_init(info) !=
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) {
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());
}
// std::string param_name = "command:" + i.name;
// if (joint.parameters.count(param_name) <= 0) {
// RCLCPP_FATAL(
// rclcpp::get_logger("cyphalSystemHardware"),
// "command interface '%s' of joint/gpio '%s' has no parameter "
// "'command:%s'.",
// i.name.c_str(), joint.name.c_str(), i.name.c_str());
// return hardware_interface::CallbackReturn::ERROR;
// }
// auto interface_name = joint.parameters.at(param_name);
/*if (interfaces_map.count(interface_name) > 0) {
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
"Sync Interface '%s' used twice. FATAL ERROR.",
interface_name.c_str());
return hardware_interface::CallbackReturn::ERROR;
}
interfaces_map[interface_name] = {
std::numeric_limits<double>::quiet_NaN(),
(joint.parameters.count("factor") > 0)
? std::stod(joint.parameters.at("factor"))
: 1.0,
joint.name, true, i.name};*/
}
/*
for (const auto &i : joint.state_interfaces) {
std::string param_name = "state:" + i.name;
if (joint.parameters.count(param_name) <= 0) {
RCLCPP_FATAL(
rclcpp::get_logger("cyphalSystemHardware"),
"state interface '%s' of joint/sensor/gpio '%s' has no parameter "
"'state:%s'.",
i.name.c_str(), joint.name.c_str(), i.name.c_str());
return hardware_interface::CallbackReturn::ERROR;
}
auto interface_name = joint.parameters.at(param_name);
if (interfaces_map.count(interface_name) > 0) {
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
"Sync Interface '%s' used twice. FATAL ERROR.",
interface_name.c_str());
return hardware_interface::CallbackReturn::ERROR;
}
interfaces_map[interface_name] = {
std::numeric_limits<double>::quiet_NaN(),
(joint.parameters.count("factor") > 0)
? std::stod(joint.parameters.at("factor"))
: 1.0,
joint.name, false, i.name};
}
}
// for (const auto &[key, value] : interfaces_map) {
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
// "Interface %s added (factor: %f).", key.c_str(), value.factor);
// }*/
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;
}
//res = //
// canardRxSubscribe(&canard, CanardTransferKindResponse,
// uavcan_register_List_1_0_FIXED_PORT_ID_,
// uavcan_register_List_Request_1_0_EXTENT_BYTES_,
// CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
//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_list();
read_thread_handle = std::thread([this]() { read_thread_static(this); });
return hardware_interface::CallbackReturn::SUCCESS;
}
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!");
return hardware_interface::CallbackReturn::SUCCESS;
}
static void
processReceivedTransfer(const struct CanardRxTransfer *const transfer,
const CanardMicrosecond now_usec) {
if (transfer->metadata.transfer_kind == CanardTransferKindResponse) {
if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
uavcan_register_List_Response_1_0 resp = {0};
size_t size = transfer->payload.size;
if (uavcan_register_List_Response_1_0_deserialize_(
&resp, (uint8_t *)transfer->payload.data, &size) >= 0) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"uavcan_register_List_1_0_FIXED_PORT_ID_");
}
}
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"CanardTransferKindResponse %d",
transfer->metadata.remote_node_id);
} else if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
if (transfer->metadata.port_id ==
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_) {
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) {
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Heartbeat %d",
transfer->metadata.remote_node_id);
}
}
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"transfer->metadata.port_id == %d",
(int)transfer->metadata.port_id);
// if(transfer->metadata.port_id == 103)
// // Publish the servo status -- this is a low-rate message with
// low-severity diagnostics.
// reg_udral_service_actuator_common_Status_0_1 msg = {0};
// // TODO: POPULATE THE MESSAGE: temperature, errors, etc.
// msg.motor_temperature.kelvin = state->servo.motor_temperature;
// msg.controller_temperature.kelvin =
// state->servo.controller_temperature; uint8_t
// serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
// size_t serialized_size = sizeof(serialized);
// const int8_t err =
// reg_udral_service_actuator_common_Status_0_1_serialize_(&msg,
// &serialized[0], &serialized_size);
// assert(err >= 0);
// if (err >= 0)
// {
// const CanardTransferMetadata transfer = {
// .priority = CanardPriorityNominal,
// .transfer_kind = CanardTransferKindMessage,
// .port_id = state->port_id.pub.servo_status,
// .remote_node_id = CANARD_NODE_ID_UNSET,
// .transfer_id = (CanardTransferID)servo_transfer_id,
// };
// send(state, now_usec + MEGA, &transfer, serialized_size,
// &serialized[0], now_usec);
// }
}
}
void cyphalSystemHardware::write_can() {
// Transmit pending frames from the prioritized TX queues managed by
// libcanard.
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.
// digitalWrite(38, 0); /*enable*/delay(1000);
// digitalWrite(38, 1); /*disable*/delay(1000);
// const int16_t result = 0;
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?)
//return -result; // 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;
}
}
}
hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
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"), "Deactivating");
/* for (auto i = 0; i < hw_stop_sec_; i++) {
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"%.1f seconds left...", hw_stop_sec_ - i);
}
// END: This part here is for exemplary purposes - Please do not copy to
your
// production code
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
"Successfully deactivated!");
*/
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
const rclcpp::Duration & /*period*/) {
// if (!sync)
// return hardware_interface::return_type::ERROR;
return hardware_interface::return_type::OK;
}
hardware_interface::return_type
cyphal_hardware_interface ::cyphalSystemHardware::write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
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)
Reference in New Issue View Git Blame Copy Permalink