diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 36e7e5706..d275c6db1 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip - URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0rc0/librmcs-sdk-src-3.2.0-rc.0.zip + URL_HASH SHA256=accefeb3a46d7da32a215765d0907de36c25353dd7951e342aaddfe98d2971e5 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp deleted file mode 100644 index 3b656dd6c..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ /dev/null @@ -1,769 +0,0 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryOmni - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryOmni() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_(create_partner_component(get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - - bottom_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); - top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); - imu_board_ = - std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - } - - ~DeformableInfantryOmni() override = default; - - void update() override { - bottom_board_->update(); - top_board_->update(); - imu_board_->update(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - bottom_board_->command_update(even); - top_board_->command_update(); - } - -private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - - class Command : public rmcs_executor::Component { - public: - explicit Command(DeformableInfantryOmni& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryOmni& deformableInfantry; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit BottomBoard( - DeformableInfantryOmni& status, rmcs_executor::Component& command, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , status_{status} - , command_{command} { - - status.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(status.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.0) - .enable_multi_turn_angle() - .set_reversed()); - - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - status.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - status.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - status.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - kNaN); - status.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, kNaN); - status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); - status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kNaN); - - status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); - status.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = start_transmit(); - if (even) { - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - } - } - - private: - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - - DeformableInfantryOmni& status_; - Component& command_; - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{status_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{status_, command_, "/chassis/right_front_wheel"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{status_, command_, "/chassis/left_front_joint"}, - device::LkMotor{status_, command_, "/chassis/left_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_back_joint"}, - device::LkMotor{status_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{status_, command_}; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{status_, command_, "/gimbal/bullet_feeder"}; - - OutputInterface& tf_{status_.tf_}; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; - *velocity_output = kNaN; - return; - } - - const auto to_physical_angle = [](double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - }; - const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; - *encoder_alpha_dot_ = kNaN; - *radius_ = kNaN; - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - status_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - status_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - status_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : kNaN, - supercap_rx ? supercap_.chassis_voltage() : kNaN, - supercap_rx ? supercap_.chassis_power() : kNaN, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - }; - - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit ImuBoard(DeformableInfantryOmni& status, const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{status.tf_} - , bmi088_{1000, 0.2, 0.0} { - - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::Bmi088 bmi088_; - }; - - class TopBoard final : private librmcs::agent::CBoard { - friend class DeformableInfantryOmni; - - public: - explicit TopBoard( - DeformableInfantryOmni& status, Command& command, const std::string& serial_filter = {}) - : librmcs::agent::CBoard( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , tf_(status.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(status, command, "/gimbal/pitch") - , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") - , scope_motor_(status, command, "/gimbal/scope") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast(status.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - } - - ~TopBoard() override = default; - - void update() { - bmi088_.update_status(); - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = start_transmit(); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_imu_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"left front", "left back", "right back", "right front"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - InputInterface timestamp_; - - std::unique_ptr imu_board_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - std::shared_ptr command_; - uint32_t cmd_tick_ = 0; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmni, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp deleted file mode 100644 index 6514c58b9..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ /dev/null @@ -1,876 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryV2 - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryV2() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , deformable_infantry_command_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - joints_calibrate_subscription_ = create_subscription( - "/joints/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - joints_calibrate_subscription_callback(std::move(msg)); - }); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - rmcs_board_lite = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_rmcs_board").as_string()); - top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string()); - } - - ~DeformableInfantryV2() override = default; - - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - - void update() override { - rmcs_board_lite->update(); - top_board_->update(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); - top_board_->command_update(); - } - -private: - class DeformableInfantryV2Command; - class BottomBoard; - class TopBoard; - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %d", - rmcs_board_lite->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %d", - rmcs_board_lite->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %d", - rmcs_board_lite->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %d", - rmcs_board_lite->chassis_steer_motors_[3].calibrate_zero_point()); - } - - void joints_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[3].calibrate_zero_point()); - } - - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite || !top_board_) - return; - - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - rmcs_board_lite->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - class DeformableInfantryV2Command : public rmcs_executor::Component { - public: - explicit DeformableInfantryV2Command(DeformableInfantryV2& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryV2& deformableInfantry; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - explicit BottomBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , deformable_infantry_(deformableInfantry) - , tf_(deformableInfantry.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_yaw_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/yaw") - , dr16_(deformableInfantry) - , chassis_wheel_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_wheel"},} - , chassis_steer_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_steering"}} - , chassis_joint_motors_{device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_joint"}} - , next_chassis_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , next_supercap_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , supercap_(deformableInfantry, deformableInfantry_command) - , gimbal_bullet_feeder_( - deformableInfantry, deformableInfantry_command, "/gimbal/bullet_feeder") { - - deformableInfantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(10.0) - .enable_multi_turn_angle() - .set_reversed()); - - // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); - deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); - deformableInfantry.register_output( - "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, nan_); - - deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - deformableInfantry.get_parameter_or( - "debug_log_wheel_motor", debug_log_wheel_motor_, false); - deformableInfantry.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = start_transmit(); - if (even) { - // Steer motors: same as V1 - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } else { - // V2: Wheel DJI frames (wheel only, no joint packed in) - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - // V2: Joint LK motors - individual CAN frames - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), - }); - } - } - - private: - DeformableInfantryV2& deformable_infantry_; - - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - - static double to_physical_angle_(double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - } - - static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; - *velocity_output = nan_; - return; - } - - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = nan_; - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : nan_, - supercap_rx ? supercap_.chassis_voltage() : nan_, - supercap_rx ? supercap_.chassis_power() : nan_, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (data.can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::LkMotor gimbal_yaw_motor_; - device::Dr16 dr16_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::LkMotor chassis_joint_motors_[4]; - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_; - Clock::time_point next_supercap_feedback_log_time_; - device::Supercap supercap_; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - explicit TopBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); - }); - } - - ~TopBoard() override = default; - - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request path. - } - - void update() { - bmi088_.update_status(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - std::atomic& hard_sync_pending_; - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - }; - - OutputInterface tf_; - InputInterface timestamp_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; - - std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr top_board_; - - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; - rclcpp::Subscription::SharedPtr joints_calibrate_subscription_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - uint32_t cmd_tick_ = 0; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b179..17711aa7f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,16 +1,14 @@ #pragma once +#include +#include +#include #include #include #include - -#include -#include +#include #include -#include -#include -#include #include #include #include @@ -19,230 +17,178 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - explicit Dr16(rmcs_executor::Component& component) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - - component.register_output("/remote/mouse", mouse_output_); - std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); - component.register_output("/remote/keyboard", keyboard_output_); - std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - - component.register_output("/remote/rotary_knob", rotary_knob_output_); - - // Simulate the rotary knob as a switch, with anti-shake algorithm. - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - } + Dr16() = default; - void store_status(const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != 6 + 8 + 4) + void store_status(std::span uart_data) { + if (uart_data.size() != kStatusSize) return; - // Avoid using reinterpret_cast here because it does not account for pointer alignment. - // Dr16DataPart structures are aligned, and using reinterpret_cast on potentially unaligned - // uart_data can cause undefined behavior on architectures that enforce strict alignment - // requirements (e.g., ARM). - // Directly accessing unaligned memory through a casted pointer can lead to crashes, - // inefficiencies, or incorrect data reads. Instead, std::memcpy safely copies the data from - // unaligned memory to properly aligned structures without violating alignment or strict - // aliasing rules. + last_receive_time_.store(Clock::now(), std::memory_order_relaxed); + + auto* cursor = uart_data.data(); uint64_t part1{}; - std::memcpy(&part1, uart_data, 6); - uart_data += 6; + std::memcpy(&part1, cursor, kPart1Size); + cursor += kPart1Size; data_part1_.store(part1, std::memory_order::relaxed); uint64_t part2{}; - std::memcpy(&part2, uart_data, 8); - uart_data += 8; + std::memcpy(&part2, cursor, kPart2Size); + cursor += kPart2Size; data_part2_.store(part2, std::memory_order::relaxed); uint32_t part3{}; - std::memcpy(&part3, uart_data, 4); - uart_data += 4; + std::memcpy(&part3, cursor, kPart3Size); data_part3_.store(part3, std::memory_order::relaxed); } void update_status() { - auto part1 alignas(uint64_t) = - std::bit_cast(data_part1_.load(std::memory_order::relaxed)); - - auto channel_to_double = [](int32_t value) { - value -= 1024; - if (-660 <= value && value <= 660) - return value / 660.0; - return 0.0; - }; - joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); - joystick_right_.x = channel_to_double(static_cast(part1.joystick_channel1)); - joystick_left_.y = -channel_to_double(static_cast(part1.joystick_channel2)); - joystick_left_.x = channel_to_double(static_cast(part1.joystick_channel3)); + const auto raw_part1 = data_part1_.load(std::memory_order::relaxed); + const auto part1 = std::bit_cast(raw_part1); - switch_right_ = static_cast(part1.switch_right); - switch_left_ = static_cast(part1.switch_left); - - auto part2 alignas(uint64_t) = - std::bit_cast(data_part2_.load(std::memory_order::relaxed)); + joystick_right_ = { + channel_to_double(static_cast(part1.joystick_channel1)), + -channel_to_double(static_cast(part1.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(part1.joystick_channel3)), + -channel_to_double(static_cast(part1.joystick_channel2)), + }; - mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; - mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; + const auto raw_part2 = data_part2_.load(std::memory_order::relaxed); + const auto part2 = std::bit_cast(raw_part2); - mouse_.left = part2.mouse_left; - mouse_.right = part2.mouse_right; + mouse_velocity_ = { + -static_cast(part2.mouse_velocity_y) / 32768.0, + -static_cast(part2.mouse_velocity_x) / 32768.0, + }; + mouse_wheel_ = -static_cast(part2.mouse_velocity_z) / 32768.0; + mouse_ = { + .left = part2.mouse_left, + .right = part2.mouse_right, + }; - auto part3 alignas(uint32_t) = - std::bit_cast(data_part3_.load(std::memory_order::relaxed)); + const auto raw_part3 = data_part3_.load(std::memory_order::relaxed); + const auto part3 = std::bit_cast(raw_part3); - keyboard_ = part3.keyboard; + keyboard_ = std::bit_cast(part3.keyboard); rotary_knob_ = channel_to_double(part3.rotary_knob); - - *joystick_right_output_ = joystick_right(); - *joystick_left_output_ = joystick_left(); - - *switch_right_output_ = switch_right(); - *switch_left_output_ = switch_left(); - - *mouse_velocity_output_ = mouse_velocity(); - *mouse_wheel_output_ = mouse_wheel(); - - *mouse_output_ = mouse(); - *keyboard_output_ = keyboard(); - - *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } - struct Vector { - constexpr static Vector zero() { return {.x = 0, .y = 0}; } - double x, y; - }; - - enum class Switch : uint8_t { kUnknown = 0, kUp = 1, kDown = 2, kMiddle = 3 }; + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } - struct [[gnu::packed]] Mouse { - constexpr static Mouse zero() { - constexpr uint8_t zero = 0; - return std::bit_cast(zero); - } + [[nodiscard]] bool valid() const noexcept { + const auto last_receive_time = last_receive_time_.load(std::memory_order_relaxed); + return last_receive_time != TimePoint::min() + && Clock::now() - last_receive_time <= kFreshTimeout; + } - bool left : 1; - bool right : 1; - }; - static_assert(sizeof(Mouse) == 1); + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } - struct [[gnu::packed]] Keyboard { - constexpr static Keyboard zero() { - constexpr uint16_t zero = 0; - return std::bit_cast(zero); - } + [[nodiscard]] rmcs_msgs::Switch switch_right() const noexcept { return switch_right_; } - bool w : 1; - bool s : 1; - bool a : 1; - bool d : 1; - bool shift : 1; - bool ctrl : 1; - bool q : 1; - bool e : 1; - bool r : 1; - bool f : 1; - bool g : 1; - bool z : 1; - bool x : 1; - bool c : 1; - bool v : 1; - bool b : 1; - }; - static_assert(sizeof(Keyboard) == 2); + [[nodiscard]] rmcs_msgs::Switch switch_left() const noexcept { return switch_left_; } - Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } - Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } - rmcs_msgs::Switch switch_right() const { - return std::bit_cast(switch_right_); - } - rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } - Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } - rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } - rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + [[nodiscard]] double rotary_knob() const noexcept { return rotary_knob_; } - double rotary_knob() const { return rotary_knob_; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } - double mouse_wheel() const { return mouse_wheel_; } + [[nodiscard]] rmcs_msgs::Switch rotary_knob_switch() const noexcept { + return rotary_knob_switch_; + } private: - static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; - void update_rotary_knob_switch() { - constexpr double divider = 0.7, anti_shake_shift = 0.05; - double upper_divider = divider, lower_divider = -divider; - - auto& switch_value = *rotary_knob_switch_output_; - if (switch_value == rmcs_msgs::Switch::UP) - upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::MIDDLE) - upper_divider += anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::DOWN) - upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - - const auto knob_value = -*rotary_knob_output_; - if (knob_value > upper_divider) { - switch_value = rmcs_msgs::Switch::UP; - } else if (knob_value < lower_divider) { - switch_value = rmcs_msgs::Switch::DOWN; - } else { - switch_value = rmcs_msgs::Switch::MIDDLE; - } - } + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + + static constexpr std::size_t kPart1Size = 6; + static constexpr std::size_t kPart2Size = 8; + static constexpr std::size_t kPart3Size = 4; + static constexpr std::size_t kStatusSize = kPart1Size + kPart2Size + kPart3Size; struct [[gnu::packed]] Dr16DataPart1 { uint64_t joystick_channel0 : 11; uint64_t joystick_channel1 : 11; uint64_t joystick_channel2 : 11; uint64_t joystick_channel3 : 11; - uint64_t switch_right : 2; - uint64_t switch_left : 2; - + uint64_t switch_left : 2; uint64_t padding : 16; }; static_assert(sizeof(Dr16DataPart1) == 8); - std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ - .joystick_channel0 = 1024, - .joystick_channel1 = 1024, - .joystick_channel2 = 1024, - .joystick_channel3 = 1024, - .switch_right = static_cast(Switch::kUnknown), - .switch_left = static_cast(Switch::kUnknown), - .padding = 0, - })}; - static_assert(decltype(data_part1_)::is_always_lock_free); struct [[gnu::packed]] Dr16DataPart2 { int16_t mouse_velocity_x; int16_t mouse_velocity_y; int16_t mouse_velocity_z; - bool mouse_left; bool mouse_right; }; static_assert(sizeof(Dr16DataPart2) == 8); + + struct [[gnu::packed]] Dr16DataPart3 { + uint16_t keyboard; + uint16_t rotary_knob; + }; + static_assert(sizeof(Dr16DataPart3) == 4); + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + void update_rotary_knob_switch() { + constexpr double divider = 0.7; + constexpr double anti_shake_shift = 0.05; + + double upper_divider = divider; + double lower_divider = -divider; + if (rotary_knob_switch_ == rmcs_msgs::Switch::UP) { + upper_divider -= anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::MIDDLE) { + upper_divider += anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::DOWN) { + upper_divider += anti_shake_shift; + lower_divider += anti_shake_shift; + } + + const auto knob_value = -rotary_knob_; + if (knob_value > upper_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::UP; + } else if (knob_value < lower_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::DOWN; + } else { + rotary_knob_switch_ = rmcs_msgs::Switch::MIDDLE; + } + } + + std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ + .joystick_channel0 = 1024, + .joystick_channel1 = 1024, + .joystick_channel2 = 1024, + .joystick_channel3 = 1024, + .switch_right = static_cast(rmcs_msgs::Switch::UNKNOWN), + .switch_left = static_cast(rmcs_msgs::Switch::UNKNOWN), + .padding = 0, + })}; + static_assert(decltype(data_part1_)::is_always_lock_free); + std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ .mouse_velocity_x = 0, .mouse_velocity_y = 0, @@ -252,45 +198,27 @@ class Dr16 { })}; static_assert(decltype(data_part2_)::is_always_lock_free); - struct [[gnu::packed]] Dr16DataPart3 { - Keyboard keyboard; - uint16_t rotary_knob; - }; - static_assert(sizeof(Dr16DataPart3) == 4); - std::atomic data_part3_ = {std::bit_cast(Dr16DataPart3{ - .keyboard = Keyboard::zero(), + std::atomic data_part3_{std::bit_cast(Dr16DataPart3{ + .keyboard = 0, .rotary_knob = 0, })}; static_assert(decltype(data_part3_)::is_always_lock_free); - Vector joystick_right_ = Vector::zero(); - Vector joystick_left_ = Vector::zero(); + std::atomic last_receive_time_{TimePoint::min()}; - Switch switch_right_ = Switch::kUnknown; - Switch switch_left_ = Switch::kUnknown; + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); - Vector mouse_velocity_ = Vector::zero(); + rmcs_msgs::Switch switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; - Mouse mouse_ = Mouse::zero(); - Keyboard keyboard_ = Keyboard::zero(); + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); double rotary_knob_ = 0.0; double mouse_wheel_ = 0.0; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp new file mode 100644 index 000000000..de30634df --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "hardware/device/dr16.hpp" +#include "hardware/device/vt13.hpp" + +namespace rmcs_core::hardware::device { + +class RemoteControl { +public: + RemoteControl(rmcs_executor::Component& component, Dr16& dr16, Vt13& vt13) + : dr16_(dr16) + , vt13_(vt13) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); + + component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); + component.register_output( + "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); + } + + void update() { + if (dr16_.valid() || !vt13_.valid() || vt13_.mode_switch() == Vt13::ModeSwitch::kNormal) { + *switch_right_output_ = dr16_.switch_right(); + *switch_left_output_ = dr16_.switch_left(); + + *joystick_right_output_ = dr16_.joystick_right(); + *joystick_left_output_ = dr16_.joystick_left(); + + *mouse_velocity_output_ = dr16_.mouse_velocity(); + *mouse_wheel_output_ = dr16_.mouse_wheel(); + + *mouse_output_ = dr16_.mouse(); + *keyboard_output_ = dr16_.keyboard(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { + *switch_right_output_ = rmcs_msgs::Switch::DOWN; + *switch_left_output_ = rmcs_msgs::Switch::DOWN; + + *joystick_right_output_ = Eigen::Vector2d::Zero(); + *joystick_left_output_ = Eigen::Vector2d::Zero(); + + *mouse_velocity_output_ = Eigen::Vector2d::Zero(); + *mouse_wheel_output_ = 0; + + *mouse_output_ = rmcs_msgs::Mouse::zero(); + *keyboard_output_ = rmcs_msgs::Keyboard::zero(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; + *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; + + *joystick_right_output_ = vt13_.joystick_right(); + *joystick_left_output_ = vt13_.joystick_left(); + + *mouse_velocity_output_ = vt13_.mouse_velocity(); + *mouse_wheel_output_ = vt13_.mouse_wheel(); + + *mouse_output_ = vt13_.mouse(); + *keyboard_output_ = vt13_.keyboard(); + } + + *rotary_knob_output_ = dr16_.rotary_knob(); + *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); + } + +private: + Dr16& dr16_; + Vt13& vt13_; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp new file mode 100644 index 000000000..4a4500215 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -0,0 +1,385 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class Vt13 { +public: + enum class ModeSwitch : uint8_t { + kUnknown = 0, + kCine = 1, + kNormal = 2, + kSport = 3, + }; + + Vt13() = default; + + void store_status(std::span uart_data) { + store_calls_.fetch_add(1, std::memory_order_relaxed); + received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); + + const auto written = data_buffer_.emplace_back_n( + [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { + *storage = *iter++; + }, + uart_data.size()); + if (written != uart_data.size()) { + const auto dropped = uart_data.size() - written; + overflow_count_.fetch_add(1, std::memory_order_relaxed); + overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); + if (should_log_overflow()) { + RCLCPP_WARN( + logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, + uart_data.size()); + } + } + } + + void update_status() { + const auto now = Clock::now(); + auto readable = data_buffer_.readable(); + peak_readable_ = std::max(peak_readable_, readable); + + while (readable) { + ReadResult result = VerificationFailed{}; + + const std::byte front = *data_buffer_.peek_front(); + if (front == std::byte{0xa9}) + result = read_remote_control_data(readable, now); + else if (front == std::byte(0xa5)) + result = read_referee_style_data(readable, now); + else { + unknown_prefix_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + std::to_integer(front), readable); + } + } + + if (std::holds_alternative(result)) { + break; + } + if (std::holds_alternative(result)) { + verification_failures_++; + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; + } + if (std::holds_alternative(result)) { + readable -= std::get(result).read; + continue; + } + } + + refresh_validity(now); + maybe_log_statistics(now); + } + + [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } + [[nodiscard]] bool valid() const noexcept { return valid_; } + + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } + + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + +private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); + static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); + static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); + static constexpr std::size_t kRefereeFrameMaxSize = 256; + + struct Incomplete {}; + struct VerificationFailed {}; + struct Success { + std::size_t read; + }; + using ReadResult = std::variant; + + struct [[gnu::packed]] RemoteControlData { + static constexpr uint16_t kHeaderMagic = 0x53a9; + + uint16_t header; + + uint16_t joystick_channel0 : 11; + uint16_t joystick_channel1 : 11; + uint16_t joystick_channel2 : 11; + uint16_t joystick_channel3 : 11; + + uint8_t mode_switch : 2; + uint8_t pause_button : 1; + uint8_t left_custom_button : 1; + uint8_t right_custom_button : 1; + uint16_t dial : 11; + uint8_t trigger : 1; + uint8_t padding1 : 3; + + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + uint8_t mouse_left : 2; + uint8_t mouse_right : 2; + uint8_t mouse_middle : 2; + uint8_t padding2 : 2; + + uint16_t keyboard; + + uint16_t crc16; + }; + + struct [[gnu::packed]] RefereeFrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; + }; + + ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RemoteControlData)) + return Incomplete{}; + + RemoteControlData data; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RemoteControlData)); + + if (data.header != RemoteControlData::kHeaderMagic) { + remote_bad_header_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", + data.header, readable); + } + return VerificationFailed{}; + } + if (!rmcs_utility::dji_crc::verify_crc16(data)) { + remote_bad_crc_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); + + update_remote_control_data(data); + valid_ = true; + last_remote_control_received_at_ = now; + remote_success_count_++; + return Success{sizeof(RemoteControlData)}; + } + + void update_remote_control_data(const RemoteControlData& data) { + mode_switch_ = static_cast(data.mode_switch + 1); + + joystick_right_ = { + channel_to_double(static_cast(data.joystick_channel1)), + -channel_to_double(static_cast(data.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(data.joystick_channel2)), + -channel_to_double(static_cast(data.joystick_channel3)), + }; + + mouse_velocity_ = { + -data.mouse_velocity_y / 32768.0, + -data.mouse_velocity_x / 32768.0, + }; + mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; + + mouse_ = { + .left = static_cast(data.mouse_left), + .right = static_cast(data.mouse_right), + }; + keyboard_ = std::bit_cast(data.keyboard); + } + + ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { + if (readable < sizeof(RefereeFrameHeader)) + return Incomplete{}; + + RefereeFrameHeader header; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RefereeFrameHeader)); + + if (!rmcs_utility::dji_crc::verify_crc8(header)) { + referee_bad_crc8_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); + return VerificationFailed{}; + } + + const std::size_t total_frame_size = + sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (total_frame_size > kRefereeFrameMaxSize) { + referee_oversize_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", + header.data_length, total_frame_size, readable); + } + return VerificationFailed{}; + } + if (readable < total_frame_size) + return Incomplete{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + referee_discarded_count_++; + return Success{total_frame_size}; + } + + bool should_log_verification_failure(const TimePoint now) { + if (last_verification_log_time_ != TimePoint::min() + && now - last_verification_log_time_ < kVerificationLogInterval) + return false; + last_verification_log_time_ = now; + return true; + } + + bool should_log_overflow() { + const auto now = Clock::now(); + if (last_overflow_log_time_ != TimePoint::min() + && now - last_overflow_log_time_ < kOverflowLogInterval) + return false; + + last_overflow_log_time_ = now; + return true; + } + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void maybe_log_statistics(const TimePoint now) { + if (last_statistics_log_time_ == TimePoint::min()) { + last_statistics_log_time_ = now; + return; + } + + const auto elapsed = now - last_statistics_log_time_; + if (elapsed < kStatisticsLogInterval) + return; + + const auto readable = data_buffer_.readable(); + const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); + const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); + const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); + const auto overflow_dropped_bytes = + overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + + RCLCPP_INFO( + logger_, + "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " + "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " + "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", + static_cast(store_calls) / elapsed_seconds, + static_cast(received_bytes) / elapsed_seconds, remote_success_count_, + verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, + referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, + unknown_prefix_count_, static_cast(overflow_count), + static_cast(overflow_dropped_bytes), readable, peak_readable_, + valid_ ? "true" : "false"); + + remote_success_count_ = 0; + verification_failures_ = 0; + remote_bad_header_count_ = 0; + remote_bad_crc_count_ = 0; + referee_discarded_count_ = 0; + referee_bad_crc8_count_ = 0; + referee_oversize_count_ = 0; + unknown_prefix_count_ = 0; + peak_readable_ = readable; + last_statistics_log_time_ = now; + } + + void reset_remote_control_state() { + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_ = Eigen::Vector2d::Zero(); + joystick_right_ = Eigen::Vector2d::Zero(); + mouse_velocity_ = Eigen::Vector2d::Zero(); + mouse_wheel_ = 0; + mouse_ = rmcs_msgs::Mouse::zero(); + keyboard_ = rmcs_msgs::Keyboard::zero(); + } + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); + rmcs_utility::RingBuffer data_buffer_{1024}; + + std::atomic store_calls_{0}; + std::atomic received_bytes_{0}; + std::atomic overflow_count_{0}; + std::atomic overflow_dropped_bytes_{0}; + + TimePoint last_remote_control_received_at_ = TimePoint::min(); + TimePoint last_verification_log_time_ = TimePoint::min(); + TimePoint last_overflow_log_time_ = TimePoint::min(); + TimePoint last_statistics_log_time_ = TimePoint::min(); + + bool valid_ = false; + std::size_t peak_readable_ = 0; + std::size_t remote_success_count_ = 0; + std::size_t verification_failures_ = 0; + std::size_t remote_bad_header_count_ = 0; + std::size_t remote_bad_crc_count_ = 0; + std::size_t referee_discarded_count_ = 0; + std::size_t referee_bad_crc8_count_ = 0; + std::size_t referee_oversize_count_ = 0; + std::size_t unknown_prefix_count_ = 0; + + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + double mouse_wheel_ = 0; + + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 234d5aa70..3c01ac5a7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include @@ -25,20 +25,22 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { class OmniInfantry : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::CBoard { + , private librmcs::agent::RmcsBoardLite { public: OmniInfantry() : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} + , librmcs::agent::RmcsBoardLite(get_parameter("board_serial").as_string(), {true}) , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -53,7 +55,7 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} + , remote_control_(*this, dr16_, vt13_) , bmi088_(1000, 0.2, 0.0) { for (auto& motor : chassis_wheel_motors_) @@ -126,9 +128,8 @@ class OmniInfantry [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); return size; }; } @@ -144,6 +145,8 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); + vt13_.update_status(); + remote_control_.update(); supercap_.update_status(); } @@ -154,11 +157,11 @@ class OmniInfantry .can_id = 0x1FE, .can_data = device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } .as_bytes(), }); @@ -171,11 +174,11 @@ class OmniInfantry .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } .as_bytes(), }); @@ -188,11 +191,11 @@ class OmniInfantry .can_id = 0x200, .can_data = device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } .as_bytes(), }); } @@ -277,6 +280,10 @@ class OmniInfantry } } + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( @@ -285,7 +292,7 @@ class OmniInfantry } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { @@ -324,6 +331,8 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; + device::Vt13 vt13_; + device::RemoteControl remote_control_; device::Bmi088 bmi088_; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp deleted file mode 100644 index 9d917a2b1..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ /dev/null @@ -1,500 +0,0 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class Sentry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Sentry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - gimbal_board_ = - std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.08, 0.0, 0.0}); - tf_->set_transform( - Eigen::Translation3d{0.07128, 0.0, 0.0481}); - } - - auto update() -> void override { - top_board_->update(); - bottom_board_->update(); - gimbal_board_->update(); - tf_->set_transform( - gimbal_board_->imu_pose().conjugate()); - } - -private: - class GimbalBoard final : private librmcs::agent::CBoard { - public: - explicit GimbalBoard(std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) { - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - } - - GimbalBoard(const GimbalBoard&) = delete; - GimbalBoard& operator=(const GimbalBoard&) = delete; - GimbalBoard(GimbalBoard&&) = delete; - GimbalBoard& operator=(GimbalBoard&&) = delete; - - ~GimbalBoard() override = default; - - auto update() -> void { - bmi088_.update_status(); - imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - } - - auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } - - private: - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 bmi088_{1000, 0.2, 0.0}; - Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class Sentry; - - public: - explicit TopBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}, librmcs::agent::AdvancedOptions options = {}) - : librmcs::agent::RmcsBoardLite(board_serial, options) - , tf_(sentry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") - , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") - , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") - , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") - , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); - } - - auto update() -> void { - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - - const auto pitch_angle = - std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); - - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_bullet_feeder_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_top_yaw_motor_.angle()); - tf_->set_state(pitch_angle); - } - - auto command_update() -> void { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - } - - private: - auto can0_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x202) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_right_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - auto can3_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_top_yaw_motor_.store_status(data.can_data); - } - - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::LkMotor gimbal_top_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - friend class Sentry; - - public: - explicit BottomBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(sentry.tf_) - , dr16_(sentry) - , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") - , chassis_wheel_motors_( - {sentry, sentry_command, "/chassis/left_front_wheel"}, - {sentry, sentry_command, "/chassis/left_back_wheel"}, - {sentry, sentry_command, "/chassis/right_back_wheel"}, - {sentry, sentry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {sentry, sentry_command, "/chassis/left_front_steering"}, - {sentry, sentry_command, "/chassis/left_back_steering"}, - {sentry, sentry_command, "/chassis/right_back_steering"}, - {sentry, sentry_command, "/chassis/right_front_steering"}) - , supercap_(sentry, sentry_command) { - sentry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point))); - - for (auto& motor : chassis_wheel_motors_) { - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - } - - constexpr auto kSteerNames = std::array{ - "right_back_zero_point", - "right_front_zero_point", - "left_front_zero_point", - "left_back_zero_point", - }; - for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { - const auto zero_point = sentry.get_parameter(name).as_int(); - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point)) - .enable_multi_turn_angle()); - } - - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - auto update() -> void { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - auto command_update() -> void { - using namespace device; - - auto builder = start_transmit(); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - - auto cache = CanPacket8{}; - auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { - auto command = [&](T arg) { - if constexpr (std::same_as) { - return arg; - } else { - const auto valid = arg >= 0 && arg < 4; - return valid ? motors[arg].generate_command() - : CanPacket8::PaddingQuarter{}; - } - }; - cache = CanPacket8{command(args)...}; - return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; - }; - - if (can_transmission_mode_) { - builder.can1_transmit(generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) - .can2_transmit(generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); - } else { - builder.can1_transmit(generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) - .can2_transmit(generate( - 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - auto dbus_receive_callback(const librmcs::data::UartDataView& data) -> void override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - auto can1_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x141) - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x202) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[3].store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); - } - - auto uart1_receive_callback(const librmcs::data::UartDataView& data) -> void override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - - device::Dr16 dr16_; - device::LkMotor gimbal_bottom_yaw_motor_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - }; - - struct CommandTransmitter : public rmcs_executor::Component { - std::function fn; - - template - explicit CommandTransmitter(Fn&& fn) - : fn{std::forward(fn)} {} - - auto update() -> void override { fn(); } - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); - text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"right back", "right front", "left front", "left back"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - auto command_update() -> void { - top_board_->command_update(); - bottom_board_->command_update(); - } - std::shared_ptr command_component_{ - create_partner_component( - get_component_name() + "_command", [this] { command_update(); })}; - - InputInterface timestamp_; - OutputInterface tf_; - - std::unique_ptr gimbal_board_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp deleted file mode 100644 index d30c1ce06..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ /dev/null @@ -1,591 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/benewake.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringHero - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHero() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - - register_output("/tf", tf_); - tf_->set_transform( - Eigen::Translation3d{0.16, 0.0, 0.15}); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - temperature_logging_timer_.reset(1000); - } - - SteeringHero(const SteeringHero&) = delete; - SteeringHero& operator=(const SteeringHero&) = delete; - SteeringHero(SteeringHero&&) = delete; - SteeringHero& operator=(SteeringHero&&) = delete; - - ~SteeringHero() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroCommand(SteeringHero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - private: - SteeringHero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(hero.tf_) - , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) - , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) { - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-x, -y, z); - }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - imu_.update_status(); - const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - benewake_.update_status(); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(data.can_data); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::Benewake benewake_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_friction_wheels_[4]; - device::LkMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) - , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { - - hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); - return size; - }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - - void update() { - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[2].generate_command(), - chassis_steering_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 imu_; - OutputInterface& tf_; - - OutputInterface chassis_yaw_velocity_imu_; - - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::LkMotor gimbal_bottom_yaw_motor_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp deleted file mode 100644 index 2b30039e1..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ /dev/null @@ -1,537 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringInfantry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringInfantry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringInfantry(const SteeringInfantry&) = delete; - SteeringInfantry& operator=(const SteeringInfantry&) = delete; - SteeringInfantry(SteeringInfantry&&) = delete; - SteeringInfantry& operator=(SteeringInfantry&&) = delete; - - ~SteeringInfantry() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - } - - void command_update() { - - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); - } - class SteeringInfantryCommand : public rmcs_executor::Component { - public: - explicit SteeringInfantryCommand(SteeringInfantry& steering_infantry) - : steering_infantry(steering_infantry) {} - - void update() override { steering_infantry.command_update(); } - - SteeringInfantry& steering_infantry; - }; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - explicit TopBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(steering_infantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(steering_infantry, steering_infantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - steering_infantry, steering_infantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - steering_infantry, steering_infantry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - steering_infantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - steering_infantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - // Eigen::AngleAxisd pitch_link_to_bmi088_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() override = default; - - void update() { - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_pitch_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_velocity_command(gimbal_pitch_motor_.control_velocity()) - .as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(data.can_data); - } - } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x142) - gimbal_pitch_motor_.store_status(data.can_data); - } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - - explicit BottomBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(steering_infantry.tf_) - , dr16_(steering_infantry) - , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") - , gimbal_bullet_feeder_( - steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") - , chassis_wheel_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) - , supercap_(steering_infantry, steering_infantry_command) { - - steering_infantry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); - return size; - }; - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - steering_infantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gy(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - tf_->set_state( - gimbal_yaw_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - if (can_transmission_mode_) { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - auto steer_builder = start_transmit(); - steer_builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - chassis_steer_motors_[1].generate_command(), - chassis_steer_motors_[2].generate_command(), - chassis_steer_motors_[3].generate_command(), - } - .as_bytes(), - }); - } else { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_ - .generate_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy()) - .as_bytes(), - }); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x203) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - gimbal_bullet_feeder_.store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x208) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - device::LkMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - }; - - OutputInterface tf_; - - std::shared_ptr command_component_; - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp index 2c829c64a..82a93fbac 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp @@ -90,6 +90,43 @@ class RingBuffer { return std::launder(reinterpret_cast(storage_[out & mask_].data)); } + /*! + * @brief Visit elements from the head without consuming them + * @tparam F Functor with signature `void(T&)` + * @param callback_functor Invoked for each readable element in FIFO order + * @param count Maximum number of elements to inspect (defaults to all available) + * @return Number of elements actually visited + * @note Consumer-only. Does not advance `out_` or destroy elements. + * Mutations performed through the callback are applied in place to the + * buffered elements. + */ + template + requires requires(F& f, T& t) { + { f(t) } noexcept; + } size_t peek_front_n(F callback_functor, size_t count = std::numeric_limits::max()) { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + + const auto readable = in - out; + count = std::min(count, readable); + if (!count) + return 0; + + const auto offset = out & mask_; + const auto slice = std::min(count, max_size() - offset); + + auto process = [&callback_functor](std::byte* storage) { + auto& element = *std::launder(reinterpret_cast(storage)); + callback_functor(element); + }; + for (size_t i = 0; i < slice; i++) + process(storage_[offset + i].data); + for (size_t i = 0; i < count - slice; i++) + process(storage_[i].data); + + return count; + } + /*! * @brief Peek the last produced element (consumer side) * @return Pointer to the last element, or nullptr if empty