diff --git a/.gitmodules b/.gitmodules index d4d71be28..d62982951 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "rmcs_ws/src/rmcs_core/librmcs"] - path = rmcs_ws/src/rmcs_core/librmcs - url = https://github.com/Alliance-Algorithm/librmcs.git [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git diff --git a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml index bd9b8abfe..41d3572a1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml @@ -2,7 +2,7 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::Hero -> hero_hardware + - rmcs_core::hardware::MecanumHero -> hero_hardware - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::Command -> referee_command @@ -27,13 +27,10 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - hero_hardware: ros__parameters: - usb_pid_top_board: 0xeb37 - usb_pid_bottom_board: 0xc1d3 + board_serial_top_board: "(TODO)" + board_serial_bottom_board: "(TODO)" yaw_motor_zero_point: 61054 pitch_motor_zero_point: 54062 external_imu_port: /dev/ttyUSB0 @@ -162,41 +159,3 @@ right_front_wheel_velocity_pid_controller: kp: 0.185 ki: 0.00 kd: 0.00 - -auto_aim_controller: - ros__parameters: - # capture - use_video: false # If true, use video stream instead of camera. - video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" - exposure_time: 3 - invert_image: false - # identifier - armor_model_path: "/models/mlp.onnx" - # pnp - fx: 1.722231837421459e+03 - fy: 1.724876404292754e+03 - cx: 7.013056440882832e+02 - cy: 5.645821718351237e+02 - k1: -0.064232403853946 - k2: -0.087667493884102 - k3: 0.792381808294582 - # tracker - armor_predict_duration: 500 - # controller - gimbal_predict_duration: 100 - yaw_error: 0.02 - pitch_error: 0.06 - shoot_velocity: 28.0 - predict_sec: 0.095 - # etc - buff_predict_duration: 200 - buff_model_path: "/models/buff_nocolor_v6.onnx" - omni_exposure: 1000.0 - record_fps: 120 - debug: false # Setup in actual using.Debug mode is used when referee is not ready - debug_color: 0 # 0 For blue while 1 for red. mine - debug_robot_id: 4 - debug_buff_mode: false - record: false - raw_img_pub: false # Set false in actual use - image_viewer_type: 0 diff --git a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml new file mode 100644 index 000000000..6364aec7e --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml @@ -0,0 +1,119 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::OmniInfantry -> infantry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller + +infantry_hardware: + ros__parameters: + board_serial: "" + yaw_motor_zero_point: 40186 + pitch_motor_zero_point: 53650 + +gimbal_controller: + ros__parameters: + upper_limit: -0.5098 + lower_limit: 0.2262 + +yaw_angle_pid_controller: + ros__parameters: + output_max: 10.0 + output_min: -10.0 + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 15.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 40.0 + ki: 0.001 + kd: 0.01 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 20.0 + ki: 0.0 + kd: 0.1 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 660.0 + - 660.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.0 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 1.583 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml index fe106ddf0..d650d044a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml @@ -26,13 +26,10 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - steeringInfantry_hardware: ros__parameters: - usb_pid_top_board: 0x93ac - usb_pid_bottom_board: 0x488d + board_serial_top_board: "(TODO)" + board_serial_bottom_board: "(TODO)" yaw_motor_zero_point: 32285 pitch_motor_zero_point: 6321 left_front_zero_point: 7848 @@ -129,41 +126,3 @@ steering_wheel_controller: k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 - -auto_aim_controller: - ros__parameters: - # capture - use_video: false # If true, use video stream instead of camera. - video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" - exposure_time: 3 - invert_image: false - # identifier - armor_model_path: "/models/mlp.onnx" - # pnp - fx: 1.722231837421459e+03 - fy: 1.724876404292754e+03 - cx: 7.013056440882832e+02 - cy: 5.645821718351237e+02 - k1: -0.064232403853946 - k2: -0.087667493884102 - k3: 0.792381808294582 - # tracker - armor_predict_duration: 500 - # controller - gimbal_predict_duration: 100 - yaw_error: -0.02 - pitch_error: 0.01 - shoot_velocity: 21.0 - predict_sec: 0.050 - # etc - buff_predict_duration: 200 - buff_model_path: "/models/buff_nocolor_v6.onnx" - omni_exposure: 1000.0 - record_fps: 120 - debug: false # Setup in actual using.Debug mode is used when referee is not ready - debug_color: 0 # 0 For blue while 1 for red. mine - debug_robot_id: 4 - debug_buff_mode: false - record: true - raw_img_pub: false # Set false in actual use - image_viewer_type: 0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 0a7372dde..7ba9529db 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -1,36 +1,42 @@ -cmake_minimum_required(VERSION 3.12) +cmake_minimum_required(VERSION 3.24) project(rmcs_core) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() -find_package (ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies () +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +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.0.0/librmcs-sdk-src-3.0.0.zip + URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + DOWNLOAD_EXTRACT_TIMESTAMP TRUE +) +FetchContent_MakeAvailable(librmcs) -file (GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS +file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp ${PROJECT_SOURCE_DIR}/src/*.c ) -ament_auto_add_library ( +ament_auto_add_library( ${PROJECT_NAME} SHARED ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) -include_directories(${PROJECT_SOURCE_DIR}/librmcs) - -include_directories(SYSTEM "/usr/include/libusb-1.0") - -target_link_libraries(${PROJECT_NAME} -lusb-1.0) +target_link_libraries(${PROJECT_NAME} PRIVATE librmcs-sdk) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) -ament_auto_package() \ No newline at end of file +ament_auto_package() diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs deleted file mode 160000 index b82f2eafd..000000000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368b..f7847151c 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,101 +1,29 @@ - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Steering wheel controller. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Gimbal player viewer - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - the recorder of Hero - - - Test plugin. - - - Test plugin. - - - Feedforward pid controller. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - - Test plugin. - - \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp index 91351a181..96d84a2d8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp @@ -1,9 +1,173 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include namespace rmcs_core::hardware::device { -using Bmi088 = librmcs::device::Bmi088; +class Bmi088 { +public: + explicit Bmi088( + double sample_freq, double kp, double ki, double q0 = 1, double q1 = 0, double q2 = 0, + double q3 = 0) + : inv_sample_freq_(1.0 / sample_freq) + , double_kp_(2.0 * kp) + , double_ki_(2.0 * ki) + , q0_(q0) + , q1_(q1) + , q2_(q2) + , q3_(q3) {} -} // namespace rmcs_core::hardware::device \ No newline at end of file + void set_coordinate_mapping( + std::function(double, double, double)> + mapping_function) { + coordinate_mapping_function_ = std::move(mapping_function); + } + + void store_accelerometer_status(int16_t x, int16_t y, int16_t z) { + accelerometer_data_.store({x, y, z}, std::memory_order::relaxed); + } + + void store_gyroscope_status(int16_t x, int16_t y, int16_t z) { + gyroscope_data_.store({x, y, z}, std::memory_order::relaxed); + } + + void update_status() { + auto acc = accelerometer_data_.load(std::memory_order::relaxed); + auto gyro = gyroscope_data_.load(std::memory_order::relaxed); + + auto solve_acc = [](int16_t value) { return value / 32767.0 * 6.0; }; + auto solve_gyro = [](int16_t value) { + return value / 32767.0 * 2000.0 / 180.0 * std::numbers::pi; + }; + + gx_ = solve_gyro(gyro.x), gy_ = solve_gyro(gyro.y), gz_ = solve_gyro(gyro.z); + ax_ = solve_acc(acc.x), ay_ = solve_acc(acc.y), az_ = solve_acc(acc.z); + + if (coordinate_mapping_function_) { + std::tie(gx_, gy_, gz_) = coordinate_mapping_function_(gx_, gy_, gz_); + std::tie(ax_, ay_, az_) = coordinate_mapping_function_(ax_, ay_, az_); + } + + mahony_ahrs_update_imu(ax_, ay_, az_, gx_, gy_, gz_); + } + + double ax() const { return ax_; } + double ay() const { return ay_; } + double az() const { return az_; } + + double gx() const { return gx_; } + double gy() const { return gy_; } + double gz() const { return gz_; } + + double& q0() { return q0_; } + double& q1() { return q1_; } + double& q2() { return q2_; } + double& q3() { return q3_; } + +private: + void mahony_ahrs_update_imu(double ax, double ay, double az, double gx, double gy, double gz) { + // Madgwick's implementation of Mayhony's AHRS algorithm. + // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + + double recip_norm; + double halfvx, halfvy, halfvz; + double halfex, halfey, halfez; + double qa, qb, qc; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer + // normalization) + if ((ax != 0.0) || (ay != 0.0) || (az != 0.0)) { + + // Normalize accelerometer measurement + recip_norm = 1 / std::sqrt((ax * ax) + (ay * ay) + (az * az)); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = (q1_ * q3_) - (q0_ * q2_); + halfvy = (q0_ * q1_) + (q2_ * q3_); + halfvz = (q0_ * q0_) - 0.5 + (q3_ * q3_); + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = (ay * halfvz) - (az * halfvy); + halfey = (az * halfvx) - (ax * halfvz); + halfez = (ax * halfvy) - (ay * halfvx); + + // Compute and apply integral feedback if enabled + if (double_ki_ > 0.0) { + // integral error scaled by Ki + integral_fbx_ += double_ki_ * halfex * (inv_sample_freq_); + integral_fby_ += double_ki_ * halfey * (inv_sample_freq_); + integral_fbz_ += double_ki_ * halfez * (inv_sample_freq_); + // apply integral feedback + gx += integral_fbx_; + gy += integral_fby_; + gz += integral_fbz_; + } else { + // prevent integral windup + integral_fbx_ = 0.0; + integral_fby_ = 0.0; + integral_fbz_ = 0.0; + } + + // Apply proportional feedback + gx += double_kp_ * halfex; + gy += double_kp_ * halfey; + gz += double_kp_ * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5 * (inv_sample_freq_)); // pre-multiply common factors + gy *= (0.5 * (inv_sample_freq_)); + gz *= (0.5 * (inv_sample_freq_)); + qa = q0_; + qb = q1_; + qc = q2_; + q0_ += ((-qb * gx) - (qc * gy) - (q3_ * gz)); + q1_ += ((qa * gx) + (qc * gz) - (q3_ * gy)); + q2_ += ((qa * gy) - (qb * gz) + (q3_ * gx)); + q3_ += ((qa * gz) + (qb * gy) - (qc * gx)); + + // Normalize quaternion + recip_norm = 1 / std::sqrt((q0_ * q0_) + (q1_ * q1_) + (q2_ * q2_) + (q3_ * q3_)); + q0_ *= recip_norm; + q1_ *= recip_norm; + q2_ *= recip_norm; + q3_ *= recip_norm; + } + + double inv_sample_freq_; // The reciprocal of sampling frequency + double double_kp_; // 2 * proportional gain (Kp) + double double_ki_; // 2 * integral gain (Ki) + + struct alignas(8) ImuData { + int16_t x, y, z; + }; + std::atomic accelerometer_data_{ + {.x = 0, .y = 0, .z = 0} + }; + std::atomic gyroscope_data_{ + {.x = 0, .y = 0, .z = 0} + }; + static_assert(std::atomic::is_always_lock_free); + + double ax_ = 0, ay_ = 0, az_ = 0, gx_ = 0, gy_ = 0, gz_ = 0; + + std::function(double, double, double)> + coordinate_mapping_function_; + + // Quaternion of sensor frame relative to auxiliary frame + double q0_, q1_, q2_, q3_; + + // Integral error terms scaled by Ki + double integral_fbx_ = 0.0, integral_fby_ = 0.0, integral_fbz_ = 0.0; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp new file mode 100644 index 000000000..155326089 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +template +requires(std::is_trivial_v) struct ByteConvertible { + alignas(align) T data; + + ByteConvertible() = default; + + constexpr explicit ByteConvertible(const T& data) + : data(data) {} + + constexpr explicit ByteConvertible(std::span bytes) noexcept { + std::memcpy(&data, bytes.data(), sizeof(data)); + } + + constexpr explicit ByteConvertible(std::span bytes) { + if (bytes.size() != sizeof(data)) [[unlikely]] + throw std::invalid_argument("Illegal span size"); + + std::memcpy(&data, bytes.data(), sizeof(data)); + } + + constexpr std::span as_bytes() noexcept { + return std::span{ + reinterpret_cast(&data), sizeof(data)}; + } + + constexpr std::span as_writable_bytes() noexcept { + return std::span{ + reinterpret_cast(&data), sizeof(data)}; + } +}; + +struct CanPacket8 : ByteConvertible, alignof(uint64_t)> { + struct Quarter : ByteConvertible { + using ByteConvertible::ByteConvertible; + }; + + struct PaddingQuarter : Quarter { + PaddingQuarter() + : Quarter(0) {} + }; + + using ByteConvertible::ByteConvertible; + + explicit CanPacket8(uint64_t data) + : ByteConvertible(std::bit_cast(data)) {} + + CanPacket8(Quarter q0, Quarter q1, Quarter q2, Quarter q3) + : ByteConvertible({q0.data, q1.data, q2.data, q3.data}) {} +}; +static_assert(std::atomic::is_always_lock_free); +static_assert(std::atomic::is_always_lock_free); + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index 57a9f7665..bb3f1c932 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -1,20 +1,63 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include + +#include "hardware/device/can_packet.hpp" namespace rmcs_core::hardware::device { -class DjiMotor : public librmcs::device::DjiMotor { +class DjiMotor { public: + enum class Type : uint8_t { kGM6020, kGM6020Voltage, kM3508, kM2006 }; + + struct Config { + explicit Config(Type motor_type) + : motor_type(motor_type) { + switch (motor_type) { + case Type::kGM6020: + case Type::kGM6020Voltage: reduction_ratio = 1.0; break; + case Type::kM3508: reduction_ratio = 3591.0 / 187.0; break; + case Type::kM2006: reduction_ratio = 36.0; break; + } + this->reversed = false; + this->multi_turn_angle_enabled = false; + } + + Config& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + Config& set_reduction_ratio(double value) { return reduction_ratio = value, *this; } + Config& set_reversed() { return reversed = true, *this; } + Config& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } + + Type motor_type; + int encoder_zero_point = 0; + double reduction_ratio; + bool reversed; + bool multi_turn_angle_enabled; + }; + DjiMotor( rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) - : librmcs::device::DjiMotor() { - status_component.register_output(name_prefix + "/angle", angle_, 0.0); - status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); - status_component.register_output(name_prefix + "/torque", torque_, 0.0); - status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + : angle_(0.0) + , velocity_(0.0) + , torque_(0.0) { + status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); command_component.register_input(name_prefix + "/control_torque", control_torque_, false); } @@ -26,17 +69,108 @@ class DjiMotor : public librmcs::device::DjiMotor { configure(config); } + DjiMotor(const DjiMotor&) = delete; + DjiMotor& operator=(const DjiMotor&) = delete; + DjiMotor(DjiMotor&&) = delete; + DjiMotor& operator=(DjiMotor&&) = delete; + + ~DjiMotor() = default; + void configure(const Config& config) { - librmcs::device::DjiMotor::configure(config); + encoder_zero_point_ = config.encoder_zero_point % kRawAngleMax; + if (encoder_zero_point_ < 0) + encoder_zero_point_ += kRawAngleMax; + + const double sign = config.reversed ? -1 : 1; + + raw_angle_to_angle_coefficient_ = + sign / config.reduction_ratio / kRawAngleMax * 2 * std::numbers::pi; + angle_to_raw_angle_coefficient_ = 1 / raw_angle_to_angle_coefficient_; + + raw_velocity_to_velocity_coefficient_ = + sign / config.reduction_ratio / 60 * 2 * std::numbers::pi; + velocity_to_raw_velocity_coefficient_ = 1 / raw_velocity_to_velocity_coefficient_; + + double torque_constant, raw_current_max, current_max; + switch (config.motor_type) { + case Type::kGM6020: + torque_constant = 0.741; + raw_current_max = 16384.0; + current_max = 3.0; + break; + case Type::kGM6020Voltage: + torque_constant = 0.741; + raw_current_max = 25000.0; + current_max = 3.0; + break; + case Type::kM3508: + torque_constant = 0.3 * 187.0 / 3591.0; + raw_current_max = 16384.0; + current_max = 20.0; + break; + case Type::kM2006: + torque_constant = 0.18 * 1.0 / 36.0; + raw_current_max = 16384.0; + current_max = 10.0; + break; + default: std::unreachable(); + } + + raw_current_to_torque_coefficient_ = + sign * config.reduction_ratio * torque_constant / raw_current_max * current_max; + torque_to_raw_current_coefficient_ = 1 / raw_current_to_torque_coefficient_; + + max_torque_ = 1 * config.reduction_ratio * torque_constant * current_max; + + last_raw_angle_ = 0; + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + angle_multi_turn_ = 0; + + *max_torque_output_ = max_torque(); + } - *max_torque_ = max_torque(); + void store_status(std::span can_data) { + if (can_data.size() != 8) [[unlikely]] + return; + can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); } void update_status() { - librmcs::device::DjiMotor::update_status(); - *angle_ = angle(); - *velocity_ = velocity(); - *torque_ = torque(); + const auto feedback = + std::bit_cast(can_data_.load(std::memory_order::relaxed)); + + // Temperature unit: celsius + temperature_ = static_cast(feedback.temperature); + + // Angle unit: rad + const int raw_angle = feedback.angle; + int calibrated_raw_angle = raw_angle - encoder_zero_point_; + if (calibrated_raw_angle < 0) + calibrated_raw_angle += kRawAngleMax; + if (!multi_turn_angle_enabled_) { + angle_ = raw_angle_to_angle_coefficient_ * static_cast(calibrated_raw_angle); + if (angle_ < 0) + angle_ += 2 * std::numbers::pi; + } else { + auto diff = (calibrated_raw_angle - angle_multi_turn_) % kRawAngleMax; + if (diff <= -kRawAngleMax / 2) + diff += kRawAngleMax; + else if (diff > kRawAngleMax / 2) + diff -= kRawAngleMax; + angle_multi_turn_ += diff; + angle_ = raw_angle_to_angle_coefficient_ * static_cast(angle_multi_turn_); + } + last_raw_angle_ = raw_angle; + + // Velocity unit: rad/s + velocity_ = raw_velocity_to_velocity_coefficient_ * static_cast(feedback.velocity); + + // Torque unit: N*m + torque_ = raw_current_to_torque_coefficient_ * static_cast(feedback.current); + + *angle_output_ = angle(); + *velocity_output_ = velocity(); + *torque_output_ = torque(); } double control_torque() const { @@ -46,17 +180,65 @@ class DjiMotor : public librmcs::device::DjiMotor { return 0.0; } - uint16_t generate_command() { - return librmcs::device::DjiMotor::generate_command(control_torque()); + CanPacket8::Quarter generate_command() const { return generate_command(control_torque()); } + + CanPacket8::Quarter generate_command(double control_torque) const { + if (std::isnan(control_torque)) { + return CanPacket8::Quarter{0}; + } + + control_torque = std::clamp(control_torque, -max_torque_, max_torque_); + const double current = std::round(torque_to_raw_current_coefficient_ * control_torque); + const rmcs_utility::be_int16_t control_current = static_cast(current); + + return std::bit_cast(control_current); } + int calibrate_zero_point() { + angle_multi_turn_ = 0; + encoder_zero_point_ = last_raw_angle_; + return encoder_zero_point_; + } + + double angle() const { return angle_; } + double velocity() const { return velocity_; } + double torque() const { return torque_; } + double max_torque() const { return max_torque_; } + double temperature() const { return temperature_; } + private: - rmcs_executor::Component::OutputInterface angle_; - rmcs_executor::Component::OutputInterface velocity_; - rmcs_executor::Component::OutputInterface torque_; - rmcs_executor::Component::OutputInterface max_torque_; + struct alignas(uint64_t) DjiMotorFeedback { + rmcs_utility::be_int16_t angle; + rmcs_utility::be_int16_t velocity; + rmcs_utility::be_int16_t current; + uint8_t temperature; + uint8_t unused; + }; + + std::atomic can_data_; + + static constexpr int kRawAngleMax = 8192; + int encoder_zero_point_, last_raw_angle_; + + bool multi_turn_angle_enabled_; + int64_t angle_multi_turn_; + + double raw_angle_to_angle_coefficient_, angle_to_raw_angle_coefficient_; + double raw_velocity_to_velocity_coefficient_, velocity_to_raw_velocity_coefficient_; + double raw_current_to_torque_coefficient_, torque_to_raw_current_coefficient_; + + double angle_; + double velocity_; + double torque_; + double max_torque_; + double temperature_; + + rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface velocity_output_; + rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface max_torque_output_; rmcs_executor::Component::InputInterface control_torque_; }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device 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 895765750..75152b179 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,7 +1,13 @@ #pragma once +#include +#include +#include + +#include +#include + #include -#include #include #include #include @@ -11,76 +17,174 @@ namespace rmcs_core::hardware::device { -class Dr16 : public librmcs::device::Dr16 { +class Dr16 { public: explicit Dr16(rmcs_executor::Component& component) { component.register_output( - "/remote/joystick/right", joystick_right_, Eigen::Vector2d::Zero()); - component.register_output("/remote/joystick/left", joystick_left_, Eigen::Vector2d::Zero()); + "/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_, rmcs_msgs::Switch::UNKNOWN); - component.register_output("/remote/switch/left", switch_left_, rmcs_msgs::Switch::UNKNOWN); + "/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_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_); + "/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_); - std::memset(&*mouse_, 0, sizeof(*mouse_)); - component.register_output("/remote/keyboard", keyboard_); - std::memset(&*keyboard_, 0, sizeof(*keyboard_)); + 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_); + 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_, rmcs_msgs::Switch::UNKNOWN); + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + } + + void store_status(const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != 6 + 8 + 4) + 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. + + uint64_t part1{}; + std::memcpy(&part1, uart_data, 6); + uart_data += 6; + data_part1_.store(part1, std::memory_order::relaxed); + + uint64_t part2{}; + std::memcpy(&part2, uart_data, 8); + uart_data += 8; + data_part2_.store(part2, std::memory_order::relaxed); + + uint32_t part3{}; + std::memcpy(&part3, uart_data, 4); + uart_data += 4; + data_part3_.store(part3, std::memory_order::relaxed); } void update_status() { - librmcs::device::Dr16::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)); + + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - *joystick_right_ = joystick_right(); - *joystick_left_ = joystick_left(); + auto part2 alignas(uint64_t) = + std::bit_cast(data_part2_.load(std::memory_order::relaxed)); - *switch_right_ = switch_right(); - *switch_left_ = switch_left(); + mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; + mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; - *mouse_velocity_ = mouse_velocity(); - *mouse_wheel_ = mouse_wheel(); + mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; - *mouse_ = mouse(); - *keyboard_ = keyboard(); + mouse_.left = part2.mouse_left; + mouse_.right = part2.mouse_right; - *rotary_knob_ = rotary_knob(); + auto part3 alignas(uint32_t) = + std::bit_cast(data_part3_.load(std::memory_order::relaxed)); + + keyboard_ = 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(); } - Eigen::Vector2d joystick_right() const { - return to_eigen_vector(librmcs::device::Dr16::joystick_right()); - } - Eigen::Vector2d joystick_left() const { - return to_eigen_vector(librmcs::device::Dr16::joystick_left()); - } + 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 }; + + struct [[gnu::packed]] Mouse { + constexpr static Mouse zero() { + constexpr uint8_t zero = 0; + return std::bit_cast(zero); + } + + bool left : 1; + bool right : 1; + }; + static_assert(sizeof(Mouse) == 1); + + struct [[gnu::packed]] Keyboard { + constexpr static Keyboard zero() { + constexpr uint16_t zero = 0; + return std::bit_cast(zero); + } + + 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); + + Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } + Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } rmcs_msgs::Switch switch_right() const { - return std::bit_cast(librmcs::device::Dr16::switch_right()); - } - rmcs_msgs::Switch switch_left() const { - return std::bit_cast(librmcs::device::Dr16::switch_left()); + return std::bit_cast(switch_right_); } + rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } - Eigen::Vector2d mouse_velocity() const { - return to_eigen_vector(librmcs::device::Dr16::mouse_velocity()); - } + Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } - rmcs_msgs::Mouse mouse() const { - return std::bit_cast(librmcs::device::Dr16::mouse()); - } - rmcs_msgs::Keyboard keyboard() const { - return std::bit_cast(librmcs::device::Dr16::keyboard()); - } + rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } + rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + + double rotary_knob() const { return rotary_knob_; } + + double mouse_wheel() const { return mouse_wheel_; } private: static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } @@ -89,7 +193,7 @@ class Dr16 : public librmcs::device::Dr16 { constexpr double divider = 0.7, anti_shake_shift = 0.05; double upper_divider = divider, lower_divider = -divider; - auto& switch_value = *rotary_knob_switch_; + 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) @@ -97,7 +201,7 @@ class Dr16 : public librmcs::device::Dr16 { else if (switch_value == rmcs_msgs::Switch::DOWN) upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - const auto knob_value = -*rotary_knob_; + const auto knob_value = -*rotary_knob_output_; if (knob_value > upper_divider) { switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { @@ -107,20 +211,86 @@ class Dr16 : public librmcs::device::Dr16 { } } - rmcs_executor::Component::OutputInterface joystick_right_; - rmcs_executor::Component::OutputInterface joystick_left_; + 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 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); + std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ + .mouse_velocity_x = 0, + .mouse_velocity_y = 0, + .mouse_velocity_z = 0, + .mouse_left = false, + .mouse_right = false, + })}; + 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(), + .rotary_knob = 0, + })}; + static_assert(decltype(data_part3_)::is_always_lock_free); + + Vector joystick_right_ = Vector::zero(); + Vector joystick_left_ = Vector::zero(); + + Switch switch_right_ = Switch::kUnknown; + Switch switch_left_ = Switch::kUnknown; + + Vector mouse_velocity_ = Vector::zero(); + + Mouse mouse_ = Mouse::zero(); + Keyboard keyboard_ = 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_; - rmcs_executor::Component::OutputInterface switch_left_; + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; - rmcs_executor::Component::OutputInterface mouse_velocity_; - rmcs_executor::Component::OutputInterface mouse_wheel_; + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; - rmcs_executor::Component::OutputInterface mouse_; - rmcs_executor::Component::OutputInterface keyboard_; + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; - rmcs_executor::Component::OutputInterface rotary_knob_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_; + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 19b6d9ded..68b2e4f65 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -1,25 +1,52 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include -#include #include #include #include +#include "hardware/device/can_packet.hpp" + namespace rmcs_core::hardware::device { -class LkMotor : public librmcs::device::LkMotor { +class LkMotor { public: + enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10 }; + + struct Config { + explicit Config(Type type) + : motor_type(type) {} + + Config& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + Config& set_reversed() { return reversed = true, *this; } + Config& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } + + Type motor_type; + int encoder_zero_point = 0; + bool reversed = false; + bool multi_turn_angle_enabled = false; + }; + LkMotor( rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, - const std::string& name_prefix) - : librmcs::device::LkMotor() { - status_component.register_output(name_prefix + "/angle", angle_, 0.0); - status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); - status_component.register_output(name_prefix + "/torque", torque_, 0.0); - status_component.register_output(name_prefix + "/temperature", temperature_, 0.0); - status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + const std::string& name_prefix) { + status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output(name_prefix + "/temperature", temperature_output_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); command_component.register_input( // name_prefix + "/control_torque", control_torque_, false); @@ -39,17 +66,352 @@ class LkMotor : public librmcs::device::LkMotor { } void configure(const Config& config) { - librmcs::device::LkMotor::configure(config); + multi_turn_encoder_count_ = 0; + last_raw_angle_ = 0; + + double current_max; + double torque_constant; + double reduction_ratio; + + switch (config.motor_type) { + case Type::kMG5010Ei10: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 0.90909; + reduction_ratio = 10.0; + + // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. + // This value must be taken directly from the manufacturer's documentation. + // It is not used in calculations and serves as a reference only. + // Avoid calculating it by simply multiplying the maximum current by the torque + // constant, as this approach leads to inaccurate and unreliable results. + max_torque_ = 7.0; + break; + case Type::kMG4010Ei10: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 0.07; + reduction_ratio = 10.0; + max_torque_ = 4.5; + break; + case Type::kMG6012Ei8: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 1.09; + reduction_ratio = 8.0; + max_torque_ = 16.0; + break; + case Type::kMG4005Ei10: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 0.06; + reduction_ratio = 10.0; + max_torque_ = 2.5; + break; + default: std::unreachable(); + } + + // Make sure raw_angle_modulus_ is a power of 2 + encoder_zero_point_ = config.encoder_zero_point & (raw_angle_modulus_ - 1); + + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + + const double sign = config.reversed ? -1.0 : 1.0; + + status_angle_to_angle_coefficient_ = sign / raw_angle_modulus_ * 2 * std::numbers::pi; + angle_to_command_angle_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0; - *max_torque_ = max_torque(); + status_velocity_to_velocity_coefficient_ = sign / reduction_ratio * kDegToRad; + velocity_to_command_velocity_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0; + + status_current_to_torque_coefficient_ = + sign * (current_max / kRawCurrentMax) * torque_constant * reduction_ratio; + torque_to_command_current_coefficient_ = 1 / status_current_to_torque_coefficient_; + + *max_torque_output_ = max_torque(); + } + + void store_status(std::span can_data) { + if (can_data.size() != 8) [[unlikely]] + return; + + const CanPacket8 can_packet{can_data}; + const struct [[gnu::packed]] { + uint8_t command; + uint8_t placeholder[7]; + } feedback alignas(CanPacket8) = std::bit_cast(can_packet); + + // Exclude non-motor status messages + if ((feedback.command & 0xF0) != 0x80) + can_packet_.store(can_packet, std::memory_order::relaxed); } void update_status() { - librmcs::device::LkMotor::update_status(); - *angle_ = angle(); - *velocity_ = velocity(); - *torque_ = torque(); - *temperature_ = temperature(); + const struct [[gnu::packed]] { + uint8_t command; + int8_t temperature; + int16_t current; + int16_t velocity; + uint16_t encoder; + } feedback alignas(CanPacket8) = + std::bit_cast(can_packet_.load(std::memory_order::relaxed)); + + // Temperature unit: celsius + temperature_ = static_cast(feedback.temperature); + + // Angle unit: rad + const auto raw_angle = feedback.encoder; + auto calibrated_raw_angle = feedback.encoder - encoder_zero_point_; + if (calibrated_raw_angle < 0) + calibrated_raw_angle += raw_angle_modulus_; + if (!multi_turn_angle_enabled_) { + angle_ = status_angle_to_angle_coefficient_ * static_cast(calibrated_raw_angle); + if (angle_ < 0) + angle_ += 2 * std::numbers::pi; + } else { + // Calculates the minimal difference between two angles and normalizes it to the range + // (-raw_angle_modulus_/2, raw_angle_modulus_/2]. + // This implementation leverages bitwise operations for efficiency, which is valid only + // when raw_angle_modulus_ is a power of 2. + auto diff = + (calibrated_raw_angle - multi_turn_encoder_count_) & (raw_angle_modulus_ - 1); + if (diff > (raw_angle_modulus_ >> 1)) + diff -= raw_angle_modulus_; + + multi_turn_encoder_count_ += diff; + angle_ = + status_angle_to_angle_coefficient_ * static_cast(multi_turn_encoder_count_); + } + last_raw_angle_ = raw_angle; + + // Velocity unit: rad/s + velocity_ = + status_velocity_to_velocity_coefficient_ * static_cast(feedback.velocity); + + // Torque unit: N*m + torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); + + *angle_output_ = angle(); + *velocity_output_ = velocity(); + *torque_output_ = torque(); + *temperature_output_ = temperature(); + } + + int64_t calibrate_zero_point() { + multi_turn_encoder_count_ = 0; + encoder_zero_point_ = last_raw_angle_; + return encoder_zero_point_; + } + + double angle() const { return angle_; } + double velocity() const { return velocity_; } + double torque() const { return torque_; } + double max_torque() const { return max_torque_; } + double temperature() const { return temperature_; } + + /// @brief Switch the motor from the startup state (default state after power-on) to the + /// shutdown state, clearing the motor's rotation count and previously received control + /// commands. The LED changes from steady on to slow flashing. At this time, the motor can still + /// respond to control commands but will not execute actions. + constexpr static CanPacket8 generate_shutdown_command() { + const struct [[gnu::packed]] { + uint8_t id; + uint8_t placeholder[7]{}; + } command alignas(CanPacket8){.id = 0x80}; + return std::bit_cast(command); + } + + /// @brief Switch the motor from the shutdown state to the startup state. The LED changes from + /// slow flashing to steady on. At this point, sending control commands can control motor + /// actions. + constexpr static CanPacket8 generate_startup_command() { + const struct [[gnu::packed]] { + uint8_t id = 0x88; + uint8_t placeholder[7]{}; + } command alignas(CanPacket8){}; + return std::bit_cast(command); + } + + /// @brief Disable the motor, but do not clear the motor's running state. Sending control + /// commands again can control the motor actions. + constexpr static CanPacket8 generate_disable_command() { + // Note: instead of sending a real disable message here, a torque control message with + // torque set to 0 is sent, because the disable message does not cause the motor to feedback + // its status. + const struct [[gnu::packed]] { + uint8_t id = 0xA1; + uint8_t placeholder0[3]{}; + int16_t current = 0; + uint8_t placeholder1[2]{}; + } command alignas(CanPacket8){}; + return std::bit_cast(command); + } + + /// @brief This command reads the current motor's temperature, motor torque current (MF, MG) / + /// motor output power (MS), speed, and encoder position. + constexpr static CanPacket8 generate_status_request() { + const struct [[gnu::packed]] { + uint8_t id = 0x9C; + uint8_t placeholder[7]{}; + } request alignas(CanPacket8){}; + return std::bit_cast(request); + } + + /// @brief The host sends this command to control the motor's torque current output. + /// @note After receiving the command, the motor responds to the host. The motor's response data + /// is the same as the `generate_status_request` command (only the command byte 0 is different, + /// here it is 0xA1). + CanPacket8 generate_torque_command(double control_torque) const { + if (std::isnan(control_torque)) + return generate_disable_command(); + + /// @param current The value range is -2048~2048, corresponding to the actual torque current + /// range of MF motor -16.5A~16.5A, and the actual torque current range of MG motor + /// -33A~33A. The bus current and the motor's actual torque vary depending on the motor + /// type. + const struct [[gnu::packed]] { + uint8_t id = 0xA1; + uint8_t placeholder0[3]{}; + int16_t current; + uint8_t placeholder1[2]{}; + } command alignas(CanPacket8){.current = to_command_current(control_torque)}; + + return std::bit_cast(command); + } + + CanPacket8 generate_torque_command() const { return generate_torque_command(control_torque()); } + + /// @brief The host sends this command to control the motor's speed, with an optional torque + /// limit. + /// @note Three firmware variants exist: + /// - Version A: 0xA2 only, torque limit field ignored. + /// - Version B: 0xA2 and 0xAD; 0xAD is the dedicated torque-limited variant. + /// - Version C: 0xA2 only, torque limit field honored. + /// The variant cannot be detected at runtime, so this implementation always sends 0xA2 for + /// broad compatibility (A and C), at the cost of not using Version B's dedicated 0xAD. + /// Response layout is the same as `generate_status_request` (command byte = 0xA2). + CanPacket8 + generate_velocity_command(double control_velocity, double torque_limit = kNan) const { + if (std::isnan(control_velocity)) + return generate_disable_command(); + + /// @param torque_limit int16_t type, value range -2048~2048, corresponding to the actual + /// torque current range of MF motor -16.5A~16.5A, and MG motor -33.0A~33.0A. The bus + /// current and motor's actual torque vary depending on the motor type. + /// @param velocity int32_t type, corresponding to the actual speed as 0.01 dps/LSB; + struct [[gnu::packed]] { + uint8_t id = 0xA2; + uint8_t placeholder{}; + int16_t current_limit = 0; + int32_t velocity; + } command alignas(CanPacket8){.velocity = to_command_velocity(control_velocity)}; + + if (!std::isnan(torque_limit)) { + // Keep using 0xA2 here; see the compatibility note above. + command.current_limit = to_command_current(torque_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_velocity_command() const { + return generate_velocity_command(control_velocity()); + } + + /// @brief The host sends this command to control the motor's position (multi-turn angle). + /// @note After receiving the command, the motor responds to the host. The motor's response data + /// is the same as the `generate_status_request` command (only the command byte 0 is + /// different, here it is 0xA3/0xA4). + CanPacket8 generate_angle_command(double control_angle, double velocity_limit = kNan) const { + if (std::isnan(control_angle)) + return generate_disable_command(); + + /// @param angle The actual position corresponds to 0.01 deg/LSB, meaning 36000 + /// represents 360 degrees, and the motor's rotation direction is determined by the + /// difference between the target position and the current position. + /// @param velocity The maximum speed limit for motor rotation, corresponding to an actual + /// speed of 1 dps/LSB, meaning 360 represents 360 dps. + struct [[gnu::packed]] { + uint8_t id = 0xA3; + uint8_t placeholder{}; + uint16_t velocity_limit = 0; + int32_t angle; + } command alignas(CanPacket8){.angle = to_absolute_command_angle(control_angle)}; + + if (!std::isnan(velocity_limit)) { + command.id = 0xA4; + + velocity_limit = std::abs(velocity_to_command_velocity_coefficient_) * (1.0 / 100.0) + * velocity_limit; + velocity_limit = std::round( + std::clamp( + velocity_limit, std::numeric_limits::min(), + std::numeric_limits::max())); + command.velocity_limit = static_cast(velocity_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_angle_command() const { return generate_angle_command(control_angle()); } + + CanPacket8 generate_angle_shift_command( + double control_shift_angle, double velocity_limit = kNan) const { + if (std::isnan(control_shift_angle)) + return generate_disable_command(); + + /// @param angle The actual position corresponds to 0.01 deg/LSB, meaning 36000 + /// represents 360 degrees, and the motor direction of rotation is determined by + /// the sign of this parameter. + /// @param velocity_limit The maximum speed limit for motor rotation, corresponding to an + /// actual speed of 1 dps/LSB, meaning 360 represents 360 dps. + struct [[gnu::packed]] { + uint8_t id = 0xA7; + uint8_t placeholder{}; + uint16_t velocity_limit = 0; + int32_t angle; + } command alignas(CanPacket8){.angle = to_command_angle(control_shift_angle)}; + + if (!std::isnan(velocity_limit)) { + command.id = 0xA8; + + velocity_limit = std::abs(velocity_to_command_velocity_coefficient_) * (1.0 / 100.0) + * velocity_limit; + velocity_limit = std::round( + std::clamp( + velocity_limit, std::numeric_limits::min(), + std::numeric_limits::max())); + command.velocity_limit = static_cast(velocity_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_angle_shift_command() const { + return generate_angle_shift_command(control_angle_shift()); + } + + CanPacket8 generate_command() { + if (first_generate_auto_command_) [[unlikely]] { + first_generate_auto_command_ = false; + + if (!control_angle_shift_.ready()) + control_angle_shift_.bind_directly(kNan); + if (!control_angle_.ready()) + control_angle_.bind_directly(kNan); + if (!control_velocity_.ready()) + control_velocity_.bind_directly(kNan); + if (!control_torque_.ready()) + control_torque_.bind_directly(kNan); + } + + if (!std::isnan(control_angle_shift())) + return generate_angle_shift_command(control_angle_shift(), control_velocity()); + if (!std::isnan(control_angle())) + return generate_angle_command(control_angle(), control_velocity()); + if (!std::isnan(control_velocity())) + return generate_velocity_command(control_velocity(), control_torque()); + return generate_torque_command(control_torque()); } double control_torque() const { @@ -80,56 +442,84 @@ class LkMotor : public librmcs::device::LkMotor { return std::numeric_limits::quiet_NaN(); } - using librmcs::device::LkMotor::generate_torque_command; - uint64_t generate_torque_command() { return generate_torque_command(control_torque()); } - - using librmcs::device::LkMotor::generate_velocity_command; - uint64_t generate_velocity_command() { return generate_velocity_command(control_velocity()); } +private: + int16_t to_command_current(double torque) const { + double current = torque_to_command_current_coefficient_ * torque; + current = std::round(std::clamp(current, -kRawCurrentMax, kRawCurrentMax)); + return static_cast(current); + } - using librmcs::device::LkMotor::generate_angle_command; - uint64_t generate_angle_command() { return generate_angle_command(control_angle()); } + int32_t to_command_velocity(double velocity) const { + velocity = velocity_to_command_velocity_coefficient_ * velocity; + velocity = std::round( + std::clamp( + velocity, std::numeric_limits::min(), + std::numeric_limits::max())); + return static_cast(velocity); + } - using librmcs::device::LkMotor::generate_angle_shift_command; - uint64_t generate_angle_shift_command() { - return generate_angle_shift_command(control_angle_shift()); + int32_t to_command_angle(double angle) const { + angle = angle_to_command_angle_coefficient_ * angle; + angle = std::round( + std::clamp( + angle, std::numeric_limits::min(), std::numeric_limits::max())); + return static_cast(angle); } - uint64_t generate_command() { - if (first_generate_auto_command_) [[unlikely]] { - first_generate_auto_command_ = false; - if (!control_angle_shift_.ready() && !control_angle_.ready() - && !control_velocity_.ready() && !control_torque_.ready()) - throw std::runtime_error{"[LkMotor] No manipulating available!"}; - else { - if (!control_angle_shift_.ready()) - control_angle_shift_.bind_directly(nan_); - if (!control_angle_.ready()) - control_angle_.bind_directly(nan_); - if (!control_velocity_.ready()) - control_velocity_.bind_directly(nan_); - if (!control_torque_.ready()) - control_torque_.bind_directly(nan_); - } - } + int32_t to_absolute_command_angle(double angle) const { + angle = angle_to_command_angle_coefficient_ * angle; + const auto one_turn = std::abs(angle_to_command_angle_coefficient_) * 2 * std::numbers::pi; + // TODO: The offset should be N turns (calculated from the motor's reported multi-turn angle + // vs encoder position at startup), not hardcoded to 1 turn. + angle -= one_turn; + angle += one_turn * static_cast(encoder_zero_point_) / raw_angle_modulus_; + angle = std::round( + std::clamp( + angle, std::numeric_limits::min(), std::numeric_limits::max())); - if (!std::isnan(control_angle_shift())) - return generate_angle_shift_command(control_angle_shift(), control_velocity()); - else if (!std::isnan(control_angle())) - return generate_angle_command(control_angle(), control_velocity()); - else if (!std::isnan(control_velocity())) - return generate_velocity_command(control_velocity(), control_torque()); - else - return generate_torque_command(control_torque()); + return static_cast(angle); } -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + // Limits + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + + static constexpr int kRawCurrentMax = 2048; + int raw_angle_modulus_; + + // Constants + static constexpr double kDegToRad = std::numbers::pi / 180; + static constexpr double kRadToDeg = 180 / std::numbers::pi; + + bool multi_turn_angle_enabled_; + int encoder_zero_point_; + + // Coefficients + double status_angle_to_angle_coefficient_; + double angle_to_command_angle_coefficient_; + + double status_velocity_to_velocity_coefficient_; + double velocity_to_command_velocity_coefficient_; + + double status_current_to_torque_coefficient_; + double torque_to_command_current_coefficient_; + + // Status + std::atomic can_packet_; + + int64_t multi_turn_encoder_count_ = 0; + int last_raw_angle_ = 0; + + double angle_; + double torque_; + double velocity_; + double max_torque_; + double temperature_; - rmcs_executor::Component::OutputInterface angle_; - rmcs_executor::Component::OutputInterface velocity_; - rmcs_executor::Component::OutputInterface torque_; - rmcs_executor::Component::OutputInterface temperature_; - rmcs_executor::Component::OutputInterface max_torque_; + rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface velocity_output_; + rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface temperature_output_; + rmcs_executor::Component::OutputInterface max_torque_output_; rmcs_executor::Component::InputInterface control_torque_; rmcs_executor::Component::InputInterface control_velocity_; @@ -139,4 +529,4 @@ class LkMotor : public librmcs::device::LkMotor { bool first_generate_auto_command_ = true; }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp index 8ee5f160c..8ac884f08 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp @@ -1,11 +1,22 @@ #pragma once +#include +#include +#include #include +#include +#include +#include +#include +#include +#include #include #include #include +#include "hardware/device/can_packet.hpp" + namespace rmcs_core::hardware::device { using rmcs_executor::Component; @@ -22,44 +33,47 @@ class Supercap { "/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); } - void store_status(uint64_t can_data) { - can_data_.store(std::bit_cast(can_data), std::memory_order::relaxed); + void store_status(std::span can_data) { + if (can_data.size() != 8) [[unlikely]] + return; + + can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); } void update_status() { - auto status = can_data_.load(std::memory_order::relaxed); + auto status = std::bit_cast(can_data_.load(std::memory_order::relaxed)); - *chassis_power_ = uint_to_double(status.chassis_power, -100.0, 400.0); - *chassis_voltage_ = uint_to_double(status.chassis_voltage, 0.0, 50.0); + *chassis_power_ = uint_to_double(status.chassis_power, -100.0, 400.0); + *chassis_voltage_ = uint_to_double(status.chassis_voltage, 0.0, 50.0); *supercap_voltage_ = uint_to_double(status.supercap_voltage, 0.0, 50.0); *supercap_enabled_ = status.enabled; } - uint16_t generate_command() const { + CanPacket8::Quarter generate_command() const { SupercapCommand command; command.enabled = *chassis_output_status_; - double power_limit = *supercap_charge_power_limit_; + const double power_limit = *supercap_charge_power_limit_; if (std::isnan(power_limit)) command.power_limit = 0; else command.power_limit = static_cast(std::clamp(power_limit, 0.0, 255.0)); - return std::bit_cast(command); + return std::bit_cast(command); } - uint16_t generate_disable_command() const { + CanPacket8::Quarter generate_disable_command() const { SupercapCommand command; - command.enabled = false; - double power_limit = *supercap_charge_power_limit_; + command.enabled = false; + const double power_limit = *supercap_charge_power_limit_; if (std::isnan(power_limit)) command.power_limit = 0; else command.power_limit = static_cast(std::clamp(power_limit, 0.0, 255.0)); - return std::bit_cast(command); + return std::bit_cast(command); } double chassis_power() { return *chassis_power_; } @@ -70,9 +84,11 @@ class Supercap { private: static constexpr double uint_to_double(std::unsigned_integral auto value, double min, double max) { - double span = max - min; - double offset = min; - return (double)value / (double)decltype(value)(-1) * span + offset; + const double span = max - min; + const double offset = min; + return (static_cast(value) + / static_cast(std::numeric_limits::max()) * span) + + offset; } struct __attribute__((packed, aligned(8))) SupercapStatus { @@ -82,7 +98,7 @@ class Supercap { uint8_t enabled; uint8_t unused; }; - std::atomic can_data_{}; + std::atomic can_data_; static_assert(decltype(can_data_)::is_always_lock_free); struct __attribute__((packed, aligned(2))) SupercapCommand { @@ -99,4 +115,4 @@ class Supercap { Component::InputInterface supercap_charge_power_limit_; }; -} // namespace rmcs_core::hardware::device \ No newline at end of file +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp deleted file mode 100644 index f4592d7f2..000000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp +++ /dev/null @@ -1,312 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/gy614.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class Infantry - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { -public: - Infantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} - , logger_(get_logger()) - , infantry_command_( - create_partner_component(get_component_name() + "_command", *this)) - , chassis_wheel_motors_( - {*this, *infantry_command_, "/chassis/left_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_back_wheel"}, - {*this, *infantry_command_, "/chassis/left_back_wheel"}) - , supercap_(*this, *infantry_command_) - , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") - , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") - , 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} - , bmi088_(1000, 0.2, 0.0) - , gy614_(*this, "/friction_wheels/temperature") - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(13.) - .enable_multi_turn_angle()); - - gimbal_yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.set_encoder_zero_point( - static_cast(get_parameter("yaw_motor_zero_point").as_int()))); - gimbal_pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.set_encoder_zero_point( - static_cast(get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(1.)); - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); - - register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - register_output("/tf", tf_); - - 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_imu_link{ - // std::numbers::pi / 2, 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(-y, x, z); - }); - - using namespace rmcs_description; - tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); - - constexpr double gimbal_center_height = 0.32059; - constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; - tf_->set_transform( - Eigen::Translation3d{0, 0, gimbal_center_height}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - transmit_buffer_.add_uart1_transmission(buffer, size); - return size; - }; - } - - ~Infantry() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { - update_motors(); - update_imu(); - dr16_.update_status(); - gy614_.update_status(); - supercap_.update_status(); - } - - void command_update() { - uint16_t can_commands[4]; - - can_commands[0] = gimbal_yaw_motor_.generate_command(); - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = chassis_wheel_motors_[0].generate_command(); - can_commands[1] = chassis_wheel_motors_[1].generate_command(); - can_commands[2] = chassis_wheel_motors_[2].generate_command(); - can_commands[3] = chassis_wheel_motors_[3].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = gimbal_bullet_feeder_.generate_command(); - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.trigger_transmission(); - } - -private: - void update_motors() { - using namespace rmcs_description; - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - tf_->set_state(chassis_wheel_motors_[0].angle()); - tf_->set_state(chassis_wheel_motors_[1].angle()); - tf_->set_state(chassis_wheel_motors_[2].angle()); - tf_->set_state(chassis_wheel_motors_[3].angle()); - - gimbal_yaw_motor_.update_status(); - tf_->set_state(gimbal_yaw_motor_.angle()); - gimbal_pitch_motor_.update_status(); - tf_->set_state(gimbal_pitch_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - } - - void update_imu() { - bmi088_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - logger_, "[gimbal calibration] New yaw offset: %d", - gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - logger_, "[gimbal calibration] New pitch offset: %d", - gimbal_pitch_motor_.calibrate_zero_point()); - } - -protected: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, - uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x201) { - auto& motor = chassis_wheel_motors_[0]; - motor.store_status(can_data); - } else if (can_id == 0x202) { - auto& motor = chassis_wheel_motors_[1]; - motor.store_status(can_data); - } else if (can_id == 0x203) { - auto& motor = chassis_wheel_motors_[2]; - motor.store_status(can_data); - } else if (can_id == 0x204) { - auto& motor = chassis_wheel_motors_[3]; - motor.store_status(can_data); - } else if (can_id == 0x205) { - gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x206) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x300) { - supercap_.store_status(can_data); - } - } - - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, - uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x202) { - gimbal_bullet_feeder_.store_status(can_data); - } else if (can_id == 0x203) { - gimbal_left_friction_.store_status(can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(can_data); - } else if (can_id == 0x206) { - gimbal_pitch_motor_.store_status(can_data); - } - } - - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); - } - - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - gy614_.store_status(data, length); - } - - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); - } - - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_gyroscope_status(x, y, z); - } - -private: - rclcpp::Logger logger_; - - class InfantryCommand : public rmcs_executor::Component { - public: - explicit InfantryCommand(Infantry& infantry) - : infantry_(infantry) {} - - void update() override { infantry_.command_update(); } - - Infantry& infantry_; - }; - std::shared_ptr infantry_command_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::DjiMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor gimbal_bullet_feeder_; - - device::Dr16 dr16_; - device::Bmi088 bmi088_; - device::Gy614 gy614_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - - OutputInterface tf_; - - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Infantry, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/mecanum_hero.cpp similarity index 54% rename from rmcs_ws/src/rmcs_core/src/hardware/hero.cpp rename to rmcs_ws/src/rmcs_core/src/hardware/mecanum_hero.cpp index d0abbf477..3aabc5e7a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/mecanum_hero.cpp @@ -1,18 +1,37 @@ #include +#include +#include +#include +#include #include +#include +#include +#include +#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/gy614.hpp" @@ -22,18 +41,20 @@ namespace rmcs_core::hardware { -class Hero +class MecanumHero : public rmcs_executor::Component , public rclcpp::Node { public: - Hero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + MecanumHero() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , command_component_( create_partner_component(get_component_name() + "_command", *this)) { - using namespace rmcs_description; register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + 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) { @@ -41,14 +62,17 @@ class Hero }); top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); } - ~Hero() override = default; + MecanumHero(const MecanumHero&) = delete; + MecanumHero& operator=(const MecanumHero&) = delete; + MecanumHero(MecanumHero&&) = delete; + MecanumHero& operator=(MecanumHero&&) = delete; + + ~MecanumHero() override = default; void update() override { top_board_->update(); @@ -72,51 +96,54 @@ class Hero class HeroCommand : public rmcs_executor::Component { public: - explicit HeroCommand(Hero& hero) + explicit HeroCommand(MecanumHero& hero) : hero_(hero) {} void update() override { hero_.command_update(); } - Hero& hero_; + private: + MecanumHero& hero_; }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::client::CBoard { + class TopBoard final : private librmcs::agent::CBoard { + friend class MecanumHero; + public: - friend class Hero; - explicit TopBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) + explicit TopBoard( + MecanumHero& hero, HeroCommand& hero_command, std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) , tf_(hero.tf_) , imu_(1000, 0.2, 0.0) , gy614_(hero, "/friction_wheels/temperature") , benewake_(hero, "/gimbal/auto_aim/laser_distance") , gimbal_pitch_motor_( hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} .set_encoder_zero_point( static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) // TODO: Bad CAN ID sequence, needs to be adjusted. , gimbal_friction_wheels_( {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( + 1.)}, {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()}, {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( + 1.)}, {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()}) , gimbal_scope_motor_( hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::M2006}) + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) , gimbal_player_viewer_motor_( hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10}) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10}) { imu_.set_coordinate_mapping([](double x, double y, double z) { // Get the mapping with the following code. @@ -141,11 +168,12 @@ class Hero }); } - ~TopBoard() final { - stop_handling_events(); - event_thread_.join(); - external_imu_thread_.request_stop(); - } + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; void update() { imu_.update_status(); @@ -176,75 +204,96 @@ class Hero for (auto& motor : gimbal_friction_wheels_) motor.update_status(); + gimbal_scope_motor_.update_status(); gimbal_player_viewer_motor_.update_status(); } void command_update() { - uint16_t batch_commands[4]; - for (int i = 0; i < 4; i++) - batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); + 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(), + }); - batch_commands[0] = gimbal_scope_motor_.generate_command(); - batch_commands[1] = 0; - batch_commands[2] = 0; - batch_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x1ff, std::bit_cast(batch_commands)); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); - transmit_buffer_.add_can2_transmission( - 0x141, gimbal_player_viewer_motor_.generate_angle_command( - gimbal_player_viewer_motor_.control_angle())); + builder.can1_transmit({ + .can_id = 0x1ff, + .can_data = + device::CanPacket8{ + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_player_viewer_motor_ + .generate_angle_command(gimbal_player_viewer_motor_.control_angle()) + .as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(can_data); + gimbal_friction_wheels_[1].store_status(data.can_data); } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(can_data); + gimbal_friction_wheels_[2].store_status(data.can_data); } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(can_data); + gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + gimbal_scope_motor_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_pitch_motor_.store_status(data.can_data); } else if (can_id == 0x141) { - gimbal_player_viewer_motor_.store_status(can_data); + gimbal_player_viewer_motor_.store_status(data.can_data); } } - void uart1_receive_callback(const std::byte* data, uint8_t length) override { - benewake_.store_status(data, length); + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + benewake_.store_status(data.uart_data.data(), data.uart_data.size()); } - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - gy614_.store_status(data, length); + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + gy614_.store_status(data.uart_data.data(), data.uart_data.size()); } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } void external_imu_thread_main( @@ -256,7 +305,7 @@ class Hero while (!stop_token.stop_requested()) { if (external_imu_.store_status(serial) && fps_counter.count()) { - bool available = fps_counter.fps() > 350.0; + const bool available = fps_counter.fps() > 350.0; if (!available) RCLCPP_WARN(logger, "External IMU low FPS: %.2f", fps_counter.fps()); else if (!external_imu_available_.load(std::memory_order::relaxed)) @@ -288,62 +337,63 @@ class Hero device::DjiMotor gimbal_scope_motor_; device::LkMotor gimbal_player_viewer_motor_; - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - rmcs_core::hardware::device::Hipnuc external_imu_; std::atomic external_imu_available_ = false; std::jthread external_imu_thread_; }; - class BottomBoard final : private librmcs::client::CBoard { + class BottomBoard final : private librmcs::agent::CBoard { + friend class MecanumHero; + public: - friend class Hero; - explicit BottomBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) + explicit BottomBoard( + MecanumHero& hero, HeroCommand& hero_command, std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) , imu_(1000, 0.2, 0.0) , tf_(hero.tf_) , dr16_(hero) , chassis_wheel_motors_( {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}}, {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}}, {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}}, {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}) + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}}) , supercap_(hero, hero_command) , gimbal_yaw_motor_( hero, hero_command, "/gimbal/yaw", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} .set_encoder_zero_point( static_cast(hero.get_parameter("yaw_motor_zero_point").as_int()))) , gimbal_bullet_feeder_( hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} .set_reversed() - .enable_multi_turn_angle()) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + .enable_multi_turn_angle()) { hero.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, 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) { - transmit_buffer_.add_uart1_transmission(buffer, 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); } - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; void update() { imu_.update_status(); @@ -364,75 +414,94 @@ class Hero } void command_update() { - uint16_t batch_commands[4]; - - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can1_transmission( - 0x141, gimbal_bullet_feeder_.generate_torque_command( - gimbal_bullet_feeder_.control_torque())); + 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(), + }); - transmit_buffer_.add_can2_transmission(0x141, gimbal_yaw_motor_.generate_command()); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bullet_feeder_ + .generate_torque_command(gimbal_bullet_feeder_.control_torque()) + .as_bytes(), + }); - batch_commands[0] = 0; - batch_commands[1] = 0; - batch_commands[2] = 0; - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(batch_commands)); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + chassis_wheel_motors_[0].store_status(data.can_data); } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(can_data); + chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(can_data); + chassis_wheel_motors_[2].store_status(data.can_data); } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(can_data); + chassis_wheel_motors_[3].store_status(data.can_data); } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(can_data); + gimbal_bullet_feeder_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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_yaw_motor_.store_status(can_data); + gimbal_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x300) { - supercap_.store_status(can_data); + supercap_.store_status(data.can_data); } } - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + 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 std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); + 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(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } device::Bmi088 imu_; @@ -449,11 +518,8 @@ class Hero device::LkMotor gimbal_bullet_feeder_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; OutputInterface tf_; @@ -468,4 +534,4 @@ class Hero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Hero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::MecanumHero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp similarity index 55% rename from rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp rename to rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 8c29292ff..234d5aa70 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -1,14 +1,27 @@ +#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" @@ -16,14 +29,16 @@ namespace rmcs_core::hardware { -class TunnelInfantry +class OmniInfantry : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::client::CBoard { + , private librmcs::agent::CBoard { public: - TunnelInfantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} + OmniInfantry() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -39,36 +54,34 @@ class TunnelInfantry , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") , dr16_{*this} - , bmi088_(1000, 0.2, 0.0) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + , bmi088_(1000, 0.2, 0.0) { for (auto& motor : chassis_wheel_motors_) motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(13.) .enable_multi_turn_angle()); gimbal_yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} .set_reversed() .set_encoder_zero_point( static_cast(get_parameter("yaw_motor_zero_point").as_int()))); gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10} + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() .set_encoder_zero_point( - static_cast(get_parameter("pitch_motor_zero_point").as_int())) - .set_reversed()); + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reversed() .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -83,10 +96,10 @@ class TunnelInfantry // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; // std::cout << mapping << std::endl; - return std::make_tuple(-y, x, z); + return std::make_tuple(y, -x, z); }); - using namespace rmcs_description; + using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); constexpr double gimbal_center_height = 0.32059; @@ -109,19 +122,23 @@ class TunnelInfantry register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, 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) { - transmit_buffer_.add_uart1_transmission(buffer, size); + start_transmit().uart1_transmit({ + .uart_data = std::span{buffer, size} + }); return size; }; } - ~TunnelInfantry() override { - stop_handling_events(); - event_thread_.join(); - } + OmniInfantry(const OmniInfantry&) = delete; + OmniInfantry& operator=(const OmniInfantry&) = delete; + OmniInfantry(OmniInfantry&&) = delete; + OmniInfantry& operator=(OmniInfantry&&) = delete; + + ~OmniInfantry() override = default; void update() override { update_motors(); @@ -131,34 +148,58 @@ class TunnelInfantry } void command_update() { - uint16_t can_commands[4]; - - can_commands[0] = gimbal_yaw_motor_.generate_command(); - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); - can_commands[0] = chassis_wheel_motors_[0].generate_command(); - can_commands[1] = chassis_wheel_motors_[1].generate_command(); - can_commands[2] = chassis_wheel_motors_[2].generate_command(); - can_commands[3] = chassis_wheel_motors_[3].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + builder.can1_transmit({ + .can_id = 0x145, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); + 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(), + }); - can_commands[0] = 0; - can_commands[1] = gimbal_bullet_feeder_.generate_command(); - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .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(), + } + .as_bytes(), + }); } private: void update_motors() { - using namespace rmcs_description; + using namespace rmcs_description; // NOLINT(google-build-using-namespace) for (auto& motor : chassis_wheel_motors_) motor.update_status(); tf_->set_state(chassis_wheel_motors_[0].angle()); @@ -178,83 +219,81 @@ class TunnelInfantry void update_imu() { bmi088_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { RCLCPP_INFO( - logger_, "[gimbal calibration] New yaw offset: %d", + logger_, "[gimbal calibration] New yaw offset: %ld", gimbal_yaw_motor_.calibrate_zero_point()); RCLCPP_INFO( logger_, "[gimbal calibration] New pitch offset: %ld", gimbal_pitch_motor_.calibrate_zero_point()); } -protected: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, - uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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) { auto& motor = chassis_wheel_motors_[0]; - motor.store_status(can_data); + motor.store_status(data.can_data); } else if (can_id == 0x202) { auto& motor = chassis_wheel_motors_[1]; - motor.store_status(can_data); + motor.store_status(data.can_data); } else if (can_id == 0x203) { auto& motor = chassis_wheel_motors_[2]; - motor.store_status(can_data); + motor.store_status(data.can_data); } else if (can_id == 0x204) { auto& motor = chassis_wheel_motors_[3]; - motor.store_status(can_data); - } else if (can_id == 0x205) { - gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x206) { - gimbal_pitch_motor_.store_status(can_data); + motor.store_status(data.can_data); + } else if (can_id == 0x145) { + gimbal_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x300) { - supercap_.store_status(can_data); + supercap_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, - uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_pitch_motor_.store_status(data.can_data); } else if (can_id == 0x202) { - gimbal_bullet_feeder_.store_status(can_data); + gimbal_bullet_feeder_.store_status(data.can_data); } else if (can_id == 0x203) { - gimbal_left_friction_.store_status(can_data); + gimbal_left_friction_.store_status(data.can_data); } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(can_data); + gimbal_right_friction_.store_status(data.can_data); } } - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + 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 std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); + 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(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); } private: @@ -262,12 +301,13 @@ class TunnelInfantry class InfantryCommand : public rmcs_executor::Component { public: - explicit InfantryCommand(TunnelInfantry& infantry) + explicit InfantryCommand(OmniInfantry& infantry) : infantry_(infantry) {} void update() override { infantry_.command_update(); } - TunnelInfantry& infantry_; + private: + OmniInfantry& infantry_; }; std::shared_ptr infantry_command_; @@ -276,7 +316,7 @@ class TunnelInfantry device::DjiMotor chassis_wheel_motors_[4]; device::Supercap supercap_; - device::DjiMotor gimbal_yaw_motor_; + device::LkMotor gimbal_yaw_motor_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; @@ -291,15 +331,12 @@ class TunnelInfantry OutputInterface tf_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::TunnelInfantry, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::OmniInfantry, 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 index 16f1bccae..d30c1ce06 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,16 +1,29 @@ +#include +#include #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" @@ -23,14 +36,16 @@ class SteeringHero , public rclcpp::Node { public: SteeringHero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , command_component_( create_partner_component( get_component_name() + "_command", *this)) { - using namespace rmcs_description; register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + 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) { @@ -38,15 +53,18 @@ class SteeringHero }); top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); + *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 { @@ -105,58 +123,61 @@ class SteeringHero void update() override { hero_.command_update(); } + private: SteeringHero& hero_; }; std::shared_ptr command_component_; - class TopBoard final : private librmcs::client::CBoard { + class TopBoard final : private librmcs::agent::CBoard { public: friend class SteeringHero; - explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) + 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::MG5010E_I10} + 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::MG5010E_I10} + 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::M3508}.set_reduction_ratio(1.)}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( + 1.)}, {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( + 1.)}, {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + 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::M3508} + 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::MG5010E_I10} + 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::M2006}) + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) , gimbal_player_viewer_motor_( hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10} + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} .set_encoder_zero_point( static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + .set_reversed()) { imu_.set_coordinate_mapping([](double x, double y, double z) { // Get the mapping with the following code. @@ -175,14 +196,16 @@ class SteeringHero hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); } - ~TopBoard() final { - stop_handling_events(); - event_thread_.join(); - } + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; void update() { imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); @@ -211,76 +234,104 @@ class SteeringHero } void command_update() { - uint16_t batch_commands[4]{}; - - for (int i = 0; i < 4; i++) - batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); + 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(), + }); - transmit_buffer_.add_can1_transmission( - 0x141, gimbal_bullet_feeder_.generate_torque_command( - gimbal_bullet_feeder_.control_torque())); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bullet_feeder_ + .generate_torque_command(gimbal_bullet_feeder_.control_torque()) + .as_bytes(), + }); - batch_commands[0] = gimbal_scope_motor_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(batch_commands)); + 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(), + }); - transmit_buffer_.add_can2_transmission( - 0x143, gimbal_player_viewer_motor_.generate_velocity_command( - gimbal_player_viewer_motor_.control_velocity())); + builder.can2_transmit({ + .can_id = 0x143, + .can_data = + gimbal_player_viewer_motor_ + .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); - transmit_buffer_.add_can2_transmission(0x141, gimbal_top_yaw_motor_.generate_command()); - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_friction_wheels_[0].store_status(data.can_data); } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(can_data); + gimbal_friction_wheels_[1].store_status(data.can_data); } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(can_data); + gimbal_friction_wheels_[2].store_status(data.can_data); } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(can_data); + gimbal_friction_wheels_[3].store_status(data.can_data); } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(can_data); + gimbal_bullet_feeder_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_top_yaw_motor_.store_status(data.can_data); } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); + gimbal_pitch_motor_.store_status(data.can_data); } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(can_data); + gimbal_player_viewer_motor_.store_status(data.can_data); } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(can_data); + gimbal_scope_motor_.store_status(data.can_data); } } - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - benewake_.store_status(data, length); + 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(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } OutputInterface& tf_; @@ -299,76 +350,74 @@ class SteeringHero device::DjiMotor gimbal_scope_motor_; device::LkMotor gimbal_player_viewer_motor_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; - class BottomBoard final : private librmcs::client::CBoard { + class BottomBoard final : private librmcs::agent::CBoard { public: friend class SteeringHero; explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) + 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::GM6020} + 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::GM6020} + 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::GM6020} + 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::GM6020} + 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::M3508} + 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::M3508} + 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::M3508} + 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::M3508} + 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::MG6012E_I8} + 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()))) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + 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_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, 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) { - transmit_buffer_.add_uart1_transmission(buffer, size); + start_transmit().uart1_transmit({ + .uart_data = std::span{buffer, size} + }); return size; }; @@ -380,10 +429,12 @@ class SteeringHero "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); } - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; void update() { imu_.update_status(); @@ -402,79 +453,105 @@ class SteeringHero } void command_update() { - uint16_t batch_commands[4]{}; - - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); + 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(), + }); - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_steering_motors_[i].generate_command(); - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(batch_commands)); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); - transmit_buffer_.add_can2_transmission( - 0x141, gimbal_bottom_yaw_motor_.generate_command()); + 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(), + }); - transmit_buffer_.trigger_transmission(); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); } private: - void can1_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + chassis_wheel_motors_[0].store_status(data.can_data); } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(can_data); + chassis_wheel_motors_[1].store_status(data.can_data); } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(can_data); + chassis_wheel_motors_[2].store_status(data.can_data); } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(can_data); + chassis_wheel_motors_[3].store_status(data.can_data); } else if (can_id == 0x300) { - supercap_.store_status(can_data); + supercap_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + chassis_steering_motors_[0].store_status(data.can_data); } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(can_data); + chassis_steering_motors_[1].store_status(data.can_data); } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(can_data); + chassis_steering_motors_[2].store_status(data.can_data); } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(can_data); + chassis_steering_motors_[3].store_status(data.can_data); } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(can_data); + gimbal_bottom_yaw_motor_.store_status(data.can_data); } } - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + 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 std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); + 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(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } device::Bmi088 imu_; @@ -493,11 +570,8 @@ class SteeringHero device::LkMotor gimbal_bottom_yaw_motor_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; OutputInterface tf_; @@ -514,4 +588,4 @@ class SteeringHero #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) \ No newline at end of file +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 index 91a1cb680..2b30039e1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp @@ -1,20 +1,32 @@ - +#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" -#include "librmcs/client/cboard.hpp" namespace rmcs_core::hardware { @@ -29,7 +41,6 @@ class SteeringInfantry , command_component_( create_partner_component( get_component_name() + "_command", *this)) { - using namespace rmcs_description; register_output("/tf", tf_); gimbal_calibrate_subscription_ = create_subscription( @@ -42,16 +53,20 @@ class SteeringInfantry }); top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - using namespace rmcs_description; - tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); + 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 { @@ -91,45 +106,43 @@ class SteeringInfantry } class SteeringInfantryCommand : public rmcs_executor::Component { public: - explicit SteeringInfantryCommand(SteeringInfantry& steeringInfantry) - : steeringInfantry(steeringInfantry) {} + explicit SteeringInfantryCommand(SteeringInfantry& steering_infantry) + : steering_infantry(steering_infantry) {} - void update() override { steeringInfantry.command_update(); } + void update() override { steering_infantry.command_update(); } - SteeringInfantry& steeringInfantry; + SteeringInfantry& steering_infantry; }; - class TopBoard final : private librmcs::client::CBoard { + class TopBoard final : private librmcs::agent::CBoard { public: friend class SteeringInfantry; explicit TopBoard( - SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, - int usb_pid = -1) - : CBoard(usb_pid) - , tf_(steeringInfantry.tf_) + 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_(steeringInfantry, steeringInfantry_command, "/gimbal/pitch") + , gimbal_pitch_motor_(steering_infantry, steering_infantry_command, "/gimbal/pitch") , gimbal_left_friction_( - steeringInfantry, steeringInfantry_command, "/gimbal/left_friction") + steering_infantry, steering_infantry_command, "/gimbal/left_friction") , gimbal_right_friction_( - steeringInfantry, steeringInfantry_command, "/gimbal/right_friction") - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + steering_infantry, steering_infantry_command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("pitch_motor_zero_point").as_int()))); + steering_infantry.get_parameter("pitch_motor_zero_point").as_int()))); gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(1.) .set_reversed()); - steeringInfantry.register_output( + steering_infantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - steeringInfantry.register_output( + steering_infantry.register_output( "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); bmi088_.set_coordinate_mapping([](double x, double y, double z) { @@ -145,13 +158,17 @@ class SteeringInfantry return std::make_tuple(x, y, z); }); } - ~TopBoard() { - stop_handling_events(); - event_thread_.join(); - } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() override = default; + void update() { bmi088_.update_status(); - Eigen::Quaterniond gimbal_bmi088_pose{ + const Eigen::Quaterniond gimbal_bmi088_pose{ bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; tf_->set_transform( @@ -169,43 +186,52 @@ class SteeringInfantry } void command_update() { - uint16_t can_commands[4]; - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.add_can2_transmission( - 0x142, gimbal_pitch_motor_.generate_velocity_command( - gimbal_pitch_motor_.control_velocity())); + 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(), + }); - transmit_buffer_.trigger_transmission(); + 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( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_left_friction_.store_status(data.can_data); } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(can_data); + gimbal_right_friction_.store_status(data.can_data); } } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + 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(can_data); + gimbal_pitch_motor_.store_status(data.can_data); } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - bmi088_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); } OutputInterface& tf_; @@ -216,101 +242,103 @@ class SteeringInfantry device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - - TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; - class BottomBoard final : private librmcs::client::CBoard { + + class BottomBoard final : private librmcs::agent::CBoard { public: friend class SteeringInfantry; explicit BottomBoard( - SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, - int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) + SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, + std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) , imu_(1000, 0.2, 0.0) - , tf_(steeringInfantry.tf_) - , dr16_(steeringInfantry) - , gimbal_yaw_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/yaw") + , tf_(steering_infantry.tf_) + , dr16_(steering_infantry) + , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") , gimbal_bullet_feeder_( - steeringInfantry, steeringInfantry_command, "/gimbal/bullet_feeder") + steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") , chassis_wheel_motors_( - {steeringInfantry, steeringInfantry_command, "/chassis/left_front_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/left_back_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_back_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_front_wheel"}) + {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_( - {steeringInfantry, steeringInfantry_command, "/chassis/left_front_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/left_back_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_back_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_front_steering"}) - , supercap_(steeringInfantry, steeringInfantry_command) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { + {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) { - steeringInfantry.register_output("/referee/serial", referee_serial_); + 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_multi( - [&buffer](std::byte byte) { *buffer++ = byte; }, 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) { - transmit_buffer_.add_uart1_transmission(buffer, size); + start_transmit().uart1_transmit({ + .uart_data = std::span{buffer, size} + }); return size; }; gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("yaw_motor_zero_point").as_int()))); + steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M2006} + 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::M3508} + 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::GM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_reversed() .set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("left_front_zero_point").as_int())) + 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::GM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_reversed() .set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("left_back_zero_point").as_int())) + 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::GM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_reversed() .set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("right_back_zero_point").as_int())) + 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::GM6020} + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} .set_reversed() .set_encoder_zero_point( static_cast( - steeringInfantry.get_parameter("right_front_zero_point").as_int())) + steering_infantry.get_parameter("right_front_zero_point").as_int())) .enable_multi_turn_angle()); - steeringInfantry.register_output( + steering_infantry.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); } - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } + + 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(); @@ -328,85 +356,153 @@ class SteeringInfantry gimbal_bullet_feeder_.update_status(); } + void command_update() { - uint16_t batch_commands[4]; - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - transmit_buffer_.add_can1_transmission(0x1FF, gimbal_bullet_feeder_.generate_command()); - if (can_transmission_mode) { - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission( - 0x1FE, std::bit_cast(batch_commands)); - transmit_buffer_.trigger_transmission(); - for (int i = 0; i < 4; i++) { - batch_commands[i] = chassis_steer_motors_[i].generate_command(); - } - transmit_buffer_.add_can2_transmission( - 0x1FE, std::bit_cast(batch_commands)); - transmit_buffer_.trigger_transmission(); + 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 { - transmit_buffer_.add_can2_transmission( - 0x142, gimbal_yaw_motor_.generate_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy())); - transmit_buffer_.trigger_transmission(); + 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; + can_transmission_mode_ = !can_transmission_mode_; } - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); + 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( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + + 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(can_data); + chassis_wheel_motors_[0].store_status(data.can_data); else if (can_id == 0x202) - chassis_wheel_motors_[1].store_status(can_data); + chassis_wheel_motors_[1].store_status(data.can_data); else if (can_id == 0x203) - chassis_wheel_motors_[2].store_status(can_data); + chassis_wheel_motors_[2].store_status(data.can_data); else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(can_data); + chassis_wheel_motors_[3].store_status(data.can_data); else if (can_id == 0x205) - gimbal_bullet_feeder_.store_status(can_data); + gimbal_bullet_feeder_.store_status(data.can_data); else if (can_id == 0x300) - supercap_.store_status(can_data); + supercap_.store_status(data.can_data); } - void can2_receive_callback( - uint32_t can_id, uint64_t can_data, bool is_extended_can_id, - bool is_remote_transmission, uint8_t can_data_length) override { - if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + + 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(can_data); + gimbal_yaw_motor_.store_status(data.can_data); else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(can_data); + chassis_steer_motors_[0].store_status(data.can_data); else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(can_data); + chassis_steer_motors_[1].store_status(data.can_data); else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(can_data); + chassis_steer_motors_[2].store_status(data.can_data); else if (can_id == 0x208) - chassis_steer_motors_[3].store_status(can_data); + chassis_steer_motors_[3].store_status(data.can_data); } - void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + 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(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); } - private: - bool can_transmission_mode = true; + bool can_transmission_mode_ = true; device::Bmi088 imu_; OutputInterface& tf_; OutputInterface powermeter_control_enabled_; @@ -419,12 +515,9 @@ class SteeringInfantry device::DjiMotor chassis_steer_motors_[4]; device::Supercap supercap_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; }; OutputInterface tf_; @@ -441,4 +534,4 @@ class SteeringInfantry #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp new file mode 100644 index 000000000..859e27a97 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp @@ -0,0 +1,201 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +template +requires(std::is_integral_v || std::is_floating_point_v) +[[nodiscard]] inline T swap_endian(const T& value) noexcept { + static_assert( + sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8, + "Endian swap is only defined for 2, 4, and 8-byte types"); + + const auto value_bytes = std::bit_cast>(value); + std::array result_bytes; + + if constexpr (sizeof(T) == 8) { // 64-bit + result_bytes[0] = value_bytes[7]; + result_bytes[1] = value_bytes[6]; + result_bytes[2] = value_bytes[5]; + result_bytes[3] = value_bytes[4]; + result_bytes[4] = value_bytes[3]; + result_bytes[5] = value_bytes[2]; + result_bytes[6] = value_bytes[1]; + result_bytes[7] = value_bytes[0]; + } else if constexpr (sizeof(T) == 4) { // 32-bit + result_bytes[0] = value_bytes[3]; + result_bytes[1] = value_bytes[2]; + result_bytes[2] = value_bytes[1]; + result_bytes[3] = value_bytes[0]; + } else if constexpr (sizeof(T) == 2) { // 16-bit + result_bytes[0] = value_bytes[1]; + result_bytes[1] = value_bytes[0]; + } else { + return 0; + } + + return std::bit_cast(result_bytes); +} + +template +requires(std::is_integral_v || std::is_floating_point_v) struct EndianContainer final { + std::array value_buffer; + + [[nodiscard]] static T transform(const T& value) noexcept { + if constexpr (std::endian::native == target_endian) { + return value; + } else { + return swap_endian(value); + } + } + + [[nodiscard]] static auto encode(T value) noexcept -> std::array { + return std::bit_cast>(transform(value)); + } + + [[nodiscard]] static T decode(const std::array& buffer) noexcept { + return transform(std::bit_cast(buffer)); + } + + EndianContainer() = default; + + // Storage in + EndianContainer(const T& value) noexcept // NOLINT(google-explicit-constructor) + : value_buffer(encode(value)) {} + + template + explicit EndianContainer(U const& value) noexcept + : value_buffer(encode(T(value))) {} + + // Storage out + template + operator U() const noexcept { // NOLINT(google-explicit-constructor) + return U(decode(value_buffer)); + } + + operator T() const noexcept { // NOLINT(google-explicit-constructor) + return decode(value_buffer); + } + + template + bool operator==(U const& o) const noexcept { + return U(*this) == o; + } + template + bool operator!=(U const& o) const noexcept { + return U(*this) != o; + } + + // Arithmetic assignment operators + EndianContainer& operator++() noexcept /* prefix */ { + *this = T(*this) + T(1); + return *this; + } + EndianContainer operator++(int) noexcept /* suffix */ { + EndianContainer t(*this); + *this = T(*this) + T(1); + return t; + } + EndianContainer& operator--() noexcept /* prefix */ { + *this = T(*this) - T(1); + return *this; + } + EndianContainer operator--(int) noexcept /* suffix */ { + EndianContainer t(*this); + *this = T(*this) - T(1); + return t; + } + + // Compound assignment operators + EndianContainer& operator+=(const T& value) noexcept { + *this = T(*this) + value; + return *this; + } + EndianContainer& operator-=(const T& value) noexcept { + *this = T(*this) - value; + return *this; + } + EndianContainer& operator*=(const T& value) noexcept { + *this = T(*this) * value; + return *this; + } + EndianContainer& operator/=(const T& value) noexcept { + *this = T(*this) / value; + return *this; + } + EndianContainer& operator%=(const T& value) noexcept requires std::is_integral_v { + *this = T(*this) % value; + return *this; + } + EndianContainer& operator&=(const T& value) noexcept requires std::is_integral_v { + *this = T(*this) & value; + return *this; + } + EndianContainer& operator|=(const T& value) noexcept requires std::is_integral_v { + *this = T(*this) | value; + return *this; + } + EndianContainer& operator^=(const T& value) noexcept requires std::is_integral_v { + *this = T(*this) ^ value; + return *this; + } + EndianContainer& operator<<=(const T& value) noexcept requires std::is_integral_v { + *this = T(T(*this) << value); + return *this; + } + EndianContainer& operator>>=(const T& value) noexcept requires std::is_integral_v { + *this = T(T(*this) >> value); + return *this; + } + friend std::ostream& operator<<(std::ostream& out, const EndianContainer value) { + out << T(value); + return out; + } + friend std::istream& operator>>(std::istream& in, EndianContainer& value) { + T val{}; + if (in >> val) { + value = val; + } + return in; + } +}; + +template +requires(std::is_integral_v || std::is_floating_point_v) +// NOLINTNEXTLINE(readability-identifier-naming) +using little_endian_t = EndianContainer; + +template +requires(std::is_integral_v || std::is_floating_point_v) +// NOLINTNEXTLINE(readability-identifier-naming) +using big_endian_t = EndianContainer; + +using le_int16_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_int32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_int64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using le_uint16_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_uint32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_uint64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using le_float32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_float64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using be_int16_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_int32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_int64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +using be_uint16_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_uint32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_uint64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +using be_float32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_float64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +} // namespace rmcs_utility 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 1c62c5914..2c829c64a 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 @@ -1,141 +1,218 @@ #pragma once -#include -#include - #include #include +#include +#include +#include +#include +#include namespace rmcs_utility { +// Lock-free Single-Producer/Single-Consumer (SPSC) ring buffer +// Inspired by Linux kfifo. template class RingBuffer { public: - constexpr explicit RingBuffer(size_t size) { + /*! + * @brief Construct an SPSC ring buffer + * @param size Minimum capacity requested. Actual capacity is rounded up + * to the next power of two and clamped to at least 2. + * @note This data structure is single-producer/single-consumer. Only one + * thread may push, and only one thread may pop, at a time. + */ + explicit RingBuffer(size_t size) { if (size <= 2) size = 2; else size = round_up_to_next_power_of_2(size); - mask = size - 1; + mask_ = size - 1; storage_ = new Storage[size]; - }; + } + + RingBuffer(const RingBuffer&) = delete; + RingBuffer& operator=(const RingBuffer&) = delete; + RingBuffer(RingBuffer&&) = delete; + RingBuffer& operator=(RingBuffer&&) = delete; + /*! + * @brief Destructor + * Destroys all elements remaining in the buffer and frees storage. + */ ~RingBuffer() { clear(); delete[] storage_; } - size_t max_size() const { return mask + 1; } + /*! + * @brief Capacity of the ring buffer + * @return Total number of slots (power of two) + */ + size_t max_size() const { return mask_ + 1; } /*! - * \brief Check how many elements can be read from the buffer - * \return Number of elements that can be read + * @brief Number of elements currently readable + * @return Count of elements available to the consumer + * @note Uses acquire on producer index and relaxed on consumer index to + * ensure visibility of constructed elements to the consumer. */ size_t readable() const { - return in_.load(std::memory_order::acquire) - out_.load(std::memory_order::relaxed); + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return in - out; } /*! - * \brief Check how many elements can be written into the buffer - * \return Number of free slots that can be be written + * @brief Number of free slots for producer + * @return Count of slots available to write + * @note Uses relaxed on producer index and acquire on consumer index to + * avoid overrun while allowing the producer to run without contention. */ - size_t writeable() const { - return max_size() - - (in_.load(std::memory_order::relaxed) - out_.load(std::memory_order::acquire)); + size_t writable() const { + const auto in = in_.load(std::memory_order::relaxed); + const auto out = out_.load(std::memory_order::acquire); + return max_size() - (in - out); } /*! - * \brief Gets the first element in the buffer on consumed side - * - * It is safe to use and modify item contents only on consumer side - * - * \return Pointer to first element, nullptr if buffer was empty + * @brief Peek the first element (consumer side) + * @return Pointer to the first element, or nullptr if empty + * @warning Do not call from producer thread. The pointer remains valid + * until the element is popped or overwritten. */ - T* front() { - auto out = out_.load(std::memory_order::relaxed); + T* peek_front() { + const auto out = out_.load(std::memory_order::relaxed); if (out == in_.load(std::memory_order::acquire)) return nullptr; - else - return std::launder(reinterpret_cast(storage_[out & mask].data)); + + return std::launder(reinterpret_cast(storage_[out & mask_].data)); } /*! - * \brief Gets the last element in the buffer on consumed side - * - * It is safe to use and modify item contents only on consumer side - * - * \return Pointer to last element, nullptr if buffer was empty + * @brief Peek the last produced element (consumer side) + * @return Pointer to the last element, or nullptr if empty + * @warning Do not call from producer thread. The pointer remains valid + * until the element is popped or overwritten. */ - T* back() { - auto in = in_.load(std::memory_order::acquire); + T* peek_back() { + const auto in = in_.load(std::memory_order::acquire); if (in == out_.load(std::memory_order::relaxed)) return nullptr; - else - return std::launder(reinterpret_cast(storage_[in & mask].data)); + + return std::launder(reinterpret_cast(storage_[(in - 1) & mask_].data)); } + /*! + * @brief Batch-construct elements at the tail (producer) + * @tparam F Functor with signature `void(std::byte* storage)` that constructs + * a `T` in-place via placement-new. + * @param count Maximum number of elements to construct (defaults to as many as fit) + * @return Number of elements actually constructed + * @note Producer-only. Publishes with release semantics. + */ template - requires requires(F f, std::byte* storage) { f(storage); } - size_t emplace_back_multi(F construct_functor, size_t count = -1) { - auto in = in_.load(std::memory_order::relaxed); - auto out = out_.load(std::memory_order::acquire); + requires requires(F& f, std::byte* storage) { + { f(storage) } noexcept; + } + size_t emplace_back_n(F construct_functor, size_t count = std::numeric_limits::max()) { + const auto in = in_.load(std::memory_order::relaxed); + const auto out = out_.load(std::memory_order::acquire); - auto writeable = max_size() - (in - out); + const auto writable = max_size() - (in - out); - if (count > writeable) - count = writeable; + if (count > writable) + count = writable; if (!count) return 0; - auto offset = in & mask; - auto slice = std::min(count, max_size() - offset); + const auto offset = in & mask_; + const auto slice = std::min(count, max_size() - offset); for (size_t i = 0; i < slice; i++) construct_functor(storage_[offset + i].data); for (size_t i = 0; i < count - slice; i++) construct_functor(storage_[i].data); - std::atomic_signal_fence(std::memory_order::release); in_.store(in + count, std::memory_order::release); return count; } + /*! + * @brief Construct one element in-place at the tail (producer) + * @return true if pushed, false if buffer is full + */ template bool emplace_back(Args&&... args) { - return emplace_back_multi( - [&](std::byte* storage) { new (storage) T{std::forward(args...)}; }, 1); + return emplace_back_n( + [&](std::byte* storage) noexcept(noexcept(T{std::forward(args)...})) { + new (storage) T{std::forward(args)...}; + }, + 1); } + /*! + * @brief Batch-push using a generator (producer) + * @tparam F Functor returning a `T` to be stored + * @param count Maximum number to generate/push + * @return Number of elements actually pushed + */ template - requires requires(F f) { T{f()}; } size_t push_back_multi(F generator, size_t count = -1) { - return emplace_back_multi([&](std::byte* storage) { new (storage) T{generator()}; }, count); + requires requires(F& f) { + { f() } noexcept; + { T{f()} } noexcept; + } size_t push_back_n(F generator, size_t count = std::numeric_limits::max()) { + return emplace_back_n( + [&](std::byte* storage) noexcept(noexcept(T{generator()})) { + new (storage) T{generator()}; + }, + count); } + /*! + * @brief Push a copy of value (producer) + * @return true if pushed, false if buffer is full + */ bool push_back(const T& value) { - return emplace_back_multi([&](std::byte* storage) { new (storage) T{value}; }, 1); + return emplace_back_n( + [&](std::byte* storage) noexcept(noexcept(T{value})) { new (storage) T{value}; }, 1); } + /*! + * @brief Push by moving value (producer) + * @return true if pushed, false if buffer is full + */ bool push_back(T&& value) { - return emplace_back_multi( - [&](std::byte* storage) { new (storage) T{std::move(value)}; }, 1); + return emplace_back_n( + [&](std::byte* storage) noexcept(noexcept(T{std::move(value)})) { + new (storage) T{std::move(value)}; + }, + 1); } + /*! + * @brief Batch-pop elements from the head (consumer) + * @tparam F Functor with signature `void(T)` receiving moved-out elements + * @param count Maximum number of elements to pop (defaults to all available) + * @return Number of elements actually popped + * @note Consumer-only. Consumes with release on `out_` and destroys elements. + */ template - requires requires(F f, T t) { f(std::move(t)); } - size_t pop_front_multi(F callback_functor, size_t count = -1) { - auto in = in_.load(std::memory_order::acquire); - auto out = out_.load(std::memory_order::relaxed); - - auto readable = in - out; - if (count > readable) - count = readable; + requires requires(F& f, T& t) { + { f(std::move(t)) } noexcept; + } size_t pop_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; - auto offset = out & mask; - auto slice = std::min(count, max_size() - offset); + 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)); @@ -147,26 +224,35 @@ class RingBuffer { for (size_t i = 0; i < count - slice; i++) process(storage_[i].data); - std::atomic_signal_fence(std::memory_order::release); out_.store(out + count, std::memory_order::release); return count; } + /*! + * @brief Pop one element (consumer) + * @return true if an element was popped, false if empty + */ template - requires requires(F f, T t) { f(std::move(t)); } bool pop_front(F&& callback_functor) { - return pop_front_multi(std::forward(callback_functor), 1); + requires requires(F& f, T& t) { + { f(std::move(t)) } noexcept; + } bool pop_front(F&& callback_functor) { + return pop_front_n(std::forward(callback_functor), 1); } /*! - * \brief Clear buffer - * \return Number of elements that be erased + * @brief Clear the buffer by consuming all elements + * @return Number of elements that were erased */ size_t clear() { - return pop_front_multi([](T&&) {}); + return pop_front_n([](const T&) noexcept {}); } private: + /*! + * @brief Round up to next power of two + * @note Assumes n > 0. Handles 32/64-bit size_t. + */ constexpr static size_t round_up_to_next_power_of_2(size_t n) { n--; n |= n >> 1; @@ -174,17 +260,18 @@ class RingBuffer { n |= n >> 4; n |= n >> 8; n |= n >> 16; - n |= n >> 32; + if constexpr (sizeof(size_t) > 4) + n |= n >> 32; n++; return n; } - size_t mask; - - std::atomic in_{0}, out_{0}; + size_t mask_; struct Storage { alignas(T) std::byte data[sizeof(T)]; }* storage_; + + std::atomic in_{0}, out_{0}; }; -}; // namespace rmcs_utility +} // namespace rmcs_utility