From 2fdab682b4160c39eb33808f84e0b269333f63ea Mon Sep 17 00:00:00 2001 From: Yukikaze2233 Date: Sat, 9 May 2026 00:51:32 +0800 Subject: [PATCH 01/90] refactor(deformable-chassis): merge refactored implementation and clean up dead code - Replace deformable_chassis.cpp with refactored version from refactor/deformable-infantry branch - Add IMU auto-calibration for pitch/roll offset - Inline JointFeedbackSource/JointIndex/JointFeedbackFrame types, removing dependency on deformable_joint_layer.hpp - Rename suspension PID variables to original naming convention (pitch_kp_, pitch_ki_, pitch_kd_, roll_kp_, roll_ki_, roll_kd_) - Remove unused SuspensionPhase enum - Remove 8 dead suspension output interfaces (suspension_mode/suspension_torque always false/NaN) - Fix config parameter names to match existing YAML config - Delete deformable_joint_layer.hpp (merged into deformable_chassis.cpp) --- .../controller/chassis/deformable_chassis.cpp | 1860 +++++++++-------- .../chassis/deformable_joint_layer.hpp | 172 -- 2 files changed, 1000 insertions(+), 1032 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index c9c61ace..0d3b36ca 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include #include @@ -19,997 +19,1137 @@ #include #include "controller/pid/pid_calculator.hpp" -#include "deformable_joint_layer.hpp" namespace rmcs_core::controller::chassis { -enum class SuspensionPhase : uint8_t { kInactive, kArming, kActive, kReleasing }; +class DeformableChassis + : public rmcs_executor::Component + , public rclcpp::Node { +public: + enum class JointFeedbackSource : uint8_t { kLegacyEncoderAngle, kMotorAngle }; + enum JointIndex : size_t { + kLeftFront = 0, + kLeftBack = 1, + kRightBack = 2, + kRightFront = 3, + kJointCount = 4, + }; -struct AttitudeBias { - double pitch_force = 0.0; - double roll_force = 0.0; -}; + struct JointFeedbackFrame { + std::array motor_angles{}; + std::array physical_angles{}; + std::array physical_velocities{}; + std::array joint_torques{}; + std::array eso_z2{}; + std::array eso_z3{}; + }; -struct LegControlState { - SuspensionPhase phase = SuspensionPhase::kInactive; - double support_force = 0.0; - double contact_confidence = 1.0; - double filtered_contact_confidence = 1.0; - double phase_elapsed = 0.0; - bool requested_deploy = false; - bool output_active = false; - bool contact_latched = false; -}; + struct AttitudePidAxis { + double kp = 20.0; + double ki = 0.0; + double kd = 0.0; + double integral = 0.0; + double integral_limit = std::numeric_limits::infinity(); + double output_limit = std::numeric_limits::infinity(); -struct LegCommand { - double requested_target_angle = std::numeric_limits::quiet_NaN(); - double final_target_angle = std::numeric_limits::quiet_NaN(); - double target_velocity = 0.0; - double target_acceleration = 0.0; - bool suspension_mode = false; - double suspension_torque = std::numeric_limits::quiet_NaN(); -}; + void reset() { integral = 0.0; } -struct AttitudePidAxis { - double kp = 20.0; - double ki = 0.0; - double kd = 0.0; - double integral = 0.0; - double integral_limit = std::numeric_limits::infinity(); - double output_limit = std::numeric_limits::infinity(); - - void reset() { integral = 0.0; } + double update(double error, double rate, double dt) { + if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { + reset(); + return std::numeric_limits::quiet_NaN(); + } - double update(double error, double rate, double dt) { - if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { - reset(); - return std::numeric_limits::quiet_NaN(); + integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); + const double output = kp * error + ki * integral - kd * rate; + return std::clamp(output, -output_limit, output_limit); } - integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); - return std::clamp(kp * error + ki * integral - kd * rate, -output_limit, output_limit); - } -}; + }; -struct SuspensionParams { - double mass, rod_length, Kz, pitch_kp, pitch_ki, pitch_kd, roll_kp, roll_ki, roll_kd, D_leg; - double com_height, wheel_base_half_x, wheel_base_half_y; - double gravity_comp_gain, control_acceleration_limit; - double preload_angle, entry_offset, ride_height_offset, hold_travel; - double activation_velocity_threshold; - double target_physical_velocity_limit, target_physical_acceleration_limit; - double torque_limit; - double pitch_angle_diff_limit, roll_angle_diff_limit, pid_integral_limit; -}; + DeformableChassis() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 0.0) + , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + + , min_angle_(get_parameter_or("min_angle", 15.0)) + , max_angle_(get_parameter_or("max_angle", 55.0)) + , left_front_joint_offset_(get_parameter_or("left_front_joint_offset", 0.0)) + , left_back_joint_offset_(get_parameter_or("left_back_joint_offset", 0.0)) + , right_front_joint_offset_(get_parameter_or("right_front_joint_offset", 0.0)) + , right_back_joint_offset_(get_parameter_or("right_back_joint_offset", 0.0)) + , target_physical_velocity_limit_( + std::max( + deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), + 1e-6)) + , target_physical_acceleration_limit_( + std::max( + deg_to_rad( + std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), + 1e-6)) + , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) + , pitch_kp_(get_parameter_or("active_suspension_pitch_kp", 200.0)) + , pitch_ki_(get_parameter_or("active_suspension_pitch_ki", 0.0)) + , pitch_kd_(get_parameter_or("active_suspension_pitch_kd", 20.0)) + , roll_kp_(get_parameter_or("active_suspension_roll_kp", 200.0)) + , roll_ki_(get_parameter_or("active_suspension_roll_ki", 0.0)) + , roll_kd_(get_parameter_or("active_suspension_roll_kd", 20.0)) + , suspension_velocity_limit_( + std::max( + deg_to_rad( + std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6)) + , suspension_acceleration_limit_( + std::max( + deg_to_rad( + std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6)) + , pitch_diff_limit_( + std::abs(get_parameter_or( + "active_suspension_pitch_angle_diff_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , roll_diff_limit_( + std::abs(get_parameter_or( + "active_suspension_roll_angle_diff_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , pid_integral_limit_( + std::abs(get_parameter_or( + "active_suspension_pid_integral_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , chassis_imu_calibration_wait_time_( + std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) + , chassis_imu_calibration_sample_time_( + std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6)) { + + following_velocity_controller_.output_max = angular_velocity_max_; + following_velocity_controller_.output_min = -angular_velocity_max_; + pitch_pid_.kp = pitch_kp_; + pitch_pid_.ki = pitch_ki_; + pitch_pid_.kd = pitch_kd_; + pitch_pid_.integral_limit = pid_integral_limit_; + pitch_pid_.output_limit = pitch_diff_limit_; + roll_pid_.kp = roll_kp_; + roll_pid_.ki = roll_ki_; + roll_pid_.kd = roll_kd_; + roll_pid_.integral_limit = pid_integral_limit_; + roll_pid_.output_limit = roll_diff_limit_; -// --- ChassisVelocityControl --- -struct ChassisVelocityControl { - static constexpr double kTranslationalVelocityMax = 10.0; - static constexpr double kAngularVelocityMax = 10.0; + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + register_input("/predefined/update_rate", update_rate_); - void configure(double spin_ratio) { - spin_ratio_ = std::clamp(spin_ratio, 0.0, 1.0); - following_velocity_controller_.output_max = kAngularVelocityMax; - following_velocity_controller_.output_min = -kAngularVelocityMax; - } + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - void set_spin_forward(bool forward) { spinning_forward_ = forward; } + register_input("/chassis/left_front_joint/angle", left_front_joint_angle_, false); + register_input("/chassis/left_back_joint/angle", left_back_joint_angle_, false); + register_input("/chassis/right_front_joint/angle", right_front_joint_angle_, false); + register_input("/chassis/right_back_joint/angle", right_back_joint_angle_, false); + + register_input( + "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); + register_input( + "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); + register_input( + "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); + register_input( + "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); + register_input( + "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, + false); + register_input( + "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, + false); + register_input( + "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, + false); + register_input( + "/chassis/right_front_joint/physical_velocity", right_front_joint_physical_velocity_, + false); + register_input("/chassis/left_front_joint/torque", left_front_joint_torque_, false); + register_input("/chassis/left_back_joint/torque", left_back_joint_torque_, false); + register_input("/chassis/right_back_joint/torque", right_back_joint_torque_, false); + register_input("/chassis/right_front_joint/torque", right_front_joint_torque_, false); + + register_input( + "/chassis/left_front_joint/encoder_angle", left_front_joint_encoder_angle_, false); + register_input( + "/chassis/left_back_joint/encoder_angle", left_back_joint_encoder_angle_, false); + register_input( + "/chassis/right_front_joint/encoder_angle", right_front_joint_encoder_angle_, false); + register_input( + "/chassis/right_back_joint/encoder_angle", right_back_joint_encoder_angle_, false); + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); + register_input("/chassis/imu/roll", chassis_imu_roll_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - Eigen::Vector2d compute_translational( - const Eigen::Vector2d& joystick_right, const rmcs_msgs::Keyboard& keyboard, - double gimbal_yaw_angle) { - Eigen::Vector2d tv = - Eigen::Rotation2Dd{gimbal_yaw_angle} - * (joystick_right + Eigen::Vector2d{keyboard.w - keyboard.s, keyboard.a - keyboard.d}); - if (tv.norm() > 1.0) - tv.normalize(); - return tv * kTranslationalVelocityMax; - } + register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); - struct AngularResult { - double angular_velocity = 0.0; - double chassis_angle = std::numeric_limits::quiet_NaN(); - double chassis_control_angle = std::numeric_limits::quiet_NaN(); - }; + register_output("/chassis/angle", chassis_angle_, nan_); + register_output("/chassis/control_angle", chassis_control_angle_, nan_); - AngularResult compute_angular( - rmcs_msgs::ChassisMode mode, double gimbal_yaw_angle, double gimbal_yaw_angle_error, - bool apply_toggle_forward) { - AngularResult result; - switch (mode) { - case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN: - if (apply_toggle_forward) - spinning_forward_ = !spinning_forward_; - result.angular_velocity = std::clamp( - spin_ratio_ * (spinning_forward_ ? kAngularVelocityMax : -kAngularVelocityMax), - -kAngularVelocityMax, kAngularVelocityMax); - break; - case rmcs_msgs::ChassisMode::STEP_DOWN: - result.angular_velocity = following_velocity_controller_.update(calc_angle_err_( - result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, - std::numbers::pi)); - break; - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { - double e = calc_angle_err_( - result.chassis_control_angle, gimbal_yaw_angle_error, gimbal_yaw_angle, - 2 * std::numbers::pi); - if (e > std::numbers::pi) - e -= 2 * std::numbers::pi; - result.angular_velocity = following_velocity_controller_.update(e); - break; - } - default: break; - } - result.chassis_angle = 2 * std::numbers::pi - gimbal_yaw_angle; - return result; + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + + register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); + register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); + register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); + register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); + + register_output( + "/chassis/left_front_joint/target_angle", left_front_joint_target_angle_, nan_); + register_output( + "/chassis/left_back_joint/target_angle", left_back_joint_target_angle_, nan_); + register_output( + "/chassis/right_back_joint/target_angle", right_back_joint_target_angle_, nan_); + register_output( + "/chassis/right_front_joint/target_angle", right_front_joint_target_angle_, nan_); + + register_output( + "/chassis/left_front_joint/target_physical_angle", + left_front_joint_target_physical_angle_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_angle", + left_back_joint_target_physical_angle_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_angle", + right_back_joint_target_physical_angle_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_angle", + right_front_joint_target_physical_angle_, nan_); + register_output( + "/chassis/left_front_joint/target_physical_velocity", + left_front_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_velocity", + left_back_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_velocity", + right_back_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_velocity", + right_front_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/left_front_joint/target_physical_acceleration", + left_front_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_acceleration", + left_back_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_acceleration", + right_back_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_acceleration", + right_front_joint_target_physical_acceleration_, nan_); + register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, nan_); + + *mode_ = rmcs_msgs::ChassisMode::AUTO; + chassis_control_velocity_->vector << nan_, nan_, nan_; + + current_target_angle_ = max_angle_; + lf_current_target_angle_ = max_angle_; + lb_current_target_angle_ = max_angle_; + rf_current_target_angle_ = max_angle_; + rb_current_target_angle_ = max_angle_; + + const bool left_front_joint_offset = has_parameter("left_front_joint_offset"); + const bool left_back_joint_offset = has_parameter("left_back_joint_offset"); + const bool right_front_joint_offset = has_parameter("right_front_joint_offset"); + const bool right_back_joint_offset = has_parameter("right_back_joint_offset"); + + const bool has_any_joint_offset = left_front_joint_offset || left_back_joint_offset + || right_front_joint_offset || right_back_joint_offset; + const bool has_all_joint_offsets = left_front_joint_offset && left_back_joint_offset + && right_front_joint_offset && right_back_joint_offset; + if (has_any_joint_offset && !has_all_joint_offsets) + throw std::runtime_error( + "deformable chassis joint offsets must be configured for all four joints or " + "removed entirely"); + + joint_feedback_source_ = has_all_joint_offsets ? JointFeedbackSource::kLegacyEncoderAngle + : JointFeedbackSource::kMotorAngle; } - void update_acceleration_estimate( - const Eigen::Vector2d& translational_velocity, double dt, double limit) { - if (!translational_velocity.array().isFinite().all()) { - control_acceleration_estimate_.setZero(); - last_translational_velocity_.setZero(); - last_valid_ = false; - return; + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } - if (!last_valid_) { - last_translational_velocity_ = translational_velocity; - control_acceleration_estimate_.setZero(); - last_valid_ = true; - return; + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } - const Eigen::Vector2d cap = Eigen::Vector2d::Constant(limit); - control_acceleration_estimate_ = - ((translational_velocity - last_translational_velocity_) / dt) - .cwiseMax(-cap) - .cwiseMin(cap); - last_translational_velocity_ = translational_velocity; + if (!left_front_joint_torque_.ready()) + left_front_joint_torque_.make_and_bind_directly(0.0); + if (!left_back_joint_torque_.ready()) + left_back_joint_torque_.make_and_bind_directly(0.0); + if (!right_back_joint_torque_.ready()) + right_back_joint_torque_.make_and_bind_directly(0.0); + if (!right_front_joint_torque_.ready()) + right_front_joint_torque_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_.ready()) + chassis_imu_pitch_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_.ready()) + chassis_imu_roll_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_rate_.ready()) + chassis_imu_pitch_rate_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_rate_.ready()) + chassis_imu_roll_rate_.make_and_bind_directly(0.0); + validate_joint_feedback_inputs(); } - void reset_acceleration_estimate() { - control_acceleration_estimate_.setZero(); - last_translational_velocity_.setZero(); - last_valid_ = false; - } + void update() override { + using rmcs_msgs::Switch; + + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; - Eigen::Vector2d control_acceleration_estimate() const { return control_acceleration_estimate_; } - bool spinning_forward() const { return spinning_forward_; } + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_velocity_control(); + update_lift_target_toggle(keyboard); + run_joint_intent_pipeline_(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } private: - double spin_ratio_ = 1.0; - bool spinning_forward_ = true; - pid::PidCalculator following_velocity_controller_{10.0, 0.0, 0.0}; - Eigen::Vector2d control_acceleration_estimate_ = Eigen::Vector2d::Zero(); - Eigen::Vector2d last_translational_velocity_ = Eigen::Vector2d::Zero(); - bool last_valid_ = false; - - double - calc_angle_err_(double& cca, double yaw_error, double yaw_angle, double alignment) const { - cca = yaw_error; - if (cca < 0) - cca += 2 * std::numbers::pi; - double e = cca + yaw_angle; - if (e >= 2 * std::numbers::pi) - e -= 2 * std::numbers::pi; - while (e > alignment / 2) { - cca -= alignment; - if (cca < 0) - cca += 2 * std::numbers::pi; - e -= alignment; - } - return e; + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double translational_velocity_max_ = 10.0; + static constexpr double angular_velocity_max_ = 30.0; + static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; + + static double wrap_deg(double deg) { + deg = std::fmod(deg, 360.0); + if (deg >= 180.0) + deg -= 360.0; + if (deg < -180.0) + deg += 360.0; + return deg; } -}; -// --- ActiveSuspension --- -struct ActiveSuspension { - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr double kGravity = 9.81; - static constexpr double kMaxAttitudeRad = 30.0 * std::numbers::pi / 180.0; - static constexpr double kMinForceArmSin = 0.1; - static constexpr double kContactConfidenceEnterThreshold = 0.55; - static constexpr double kContactConfidenceExitThreshold = 0.35; - static constexpr double kContactConfidenceFilterAlpha = 0.25; - static constexpr double kMinimumArmingTime = 0.02; - static constexpr std::array kPitchSigns = {-1.0, 1.0, 1.0, -1.0}; - static constexpr std::array kRollSigns = {1.0, 1.0, -1.0, -1.0}; - - void load_params(rclcpp::Node& node, double min_angle, double max_angle) { - params_ = SuspensionParams{ - .mass = node.get_parameter_or("active_suspension_mass", 22.5), - .rod_length = node.get_parameter_or("active_suspension_rod_length", 0.150), - .Kz = node.get_parameter_or("active_suspension_Kz", 150.0), - - .pitch_kp = node.get_parameter_or("active_suspension_pitch_kp", 200.0), - .pitch_ki = node.get_parameter_or("active_suspension_pitch_ki", 0.0), - .pitch_kd = node.get_parameter_or("active_suspension_pitch_kd", 20.0), - - .roll_kp = node.get_parameter_or("active_suspension_roll_kp", 200.0), - .roll_ki = node.get_parameter_or("active_suspension_roll_ki", 0.0), - .roll_kd = node.get_parameter_or("active_suspension_roll_kd", 20.0), - - .D_leg = node.get_parameter_or("active_suspension_D_leg", 10.0), - .com_height = node.get_parameter_or("active_suspension_com_height", 0.15), - .wheel_base_half_x = node.get_parameter_or( - "active_suspension_wheel_base_half_x", 0.2341741 / std::numbers::sqrt2), - .wheel_base_half_y = node.get_parameter_or( - "active_suspension_wheel_base_half_y", 0.2341741 / std::numbers::sqrt2), - .gravity_comp_gain = node.get_parameter_or("active_suspension_gravity_comp_gain", 1.0), - .control_acceleration_limit = std::abs( - node.get_parameter_or("active_suspension_control_acceleration_limit", 6.0)), - .preload_angle = - std::abs(node.get_parameter_or("active_suspension_preload_angle_deg", 8.0)) - * std::numbers::pi / 180.0, - .entry_offset = - std::abs(node.get_parameter_or( - "active_suspension_entry_offset_deg", - node.get_parameter_or("active_suspension_enter_deploy_tolerance_deg", 1.5))) - * std::numbers::pi / 180.0, - .ride_height_offset = - std::abs(node.get_parameter_or("active_suspension_ride_height_offset_deg", 0.0)) - * std::numbers::pi / 180.0, - .hold_travel = - std::abs(node.get_parameter_or( - "active_suspension_hold_travel_deg", - node.get_parameter_or("active_suspension_exit_deploy_tolerance_deg", 3.0))) - * std::numbers::pi / 180.0, - .activation_velocity_threshold = - node.get_parameter_or("active_suspension_activation_velocity_threshold_deg", 15.0) - * std::numbers::pi / 180.0, - .target_physical_velocity_limit = - std::max( - node.get_parameter_or( - "active_suspension_target_velocity_limit_deg", - node.get_parameter_or("target_physical_velocity_limit", 180.0)), - 1e-6) - * std::numbers::pi / 180.0, - .target_physical_acceleration_limit = - std::max( - node.get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - node.get_parameter_or("target_physical_acceleration_limit", 720.0)), - 1e-6) - * std::numbers::pi / 180.0, - .torque_limit = std::abs(node.get_parameter_or("active_suspension_torque_limit", 80.0)), - .pitch_angle_diff_limit = - std::abs(node.get_parameter_or( - "active_suspension_pitch_angle_diff_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - .roll_angle_diff_limit = - std::abs(node.get_parameter_or( - "active_suspension_roll_angle_diff_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - .pid_integral_limit = - std::abs(node.get_parameter_or( - "active_suspension_pid_integral_limit_deg", max_angle - min_angle)) - * std::numbers::pi / 180.0, - }; - - pitch_pid_.kp = params_.pitch_kp; - pitch_pid_.ki = params_.pitch_ki; - pitch_pid_.kd = params_.pitch_kd; - - roll_pid_.kp = params_.roll_kp; - roll_pid_.ki = params_.roll_ki; - roll_pid_.kd = params_.roll_kd; - - pitch_pid_.integral_limit = params_.pid_integral_limit; - pitch_pid_.output_limit = params_.pitch_angle_diff_limit; - roll_pid_.integral_limit = params_.pid_integral_limit; - roll_pid_.output_limit = params_.roll_angle_diff_limit; - - enabled_ = node.get_parameter_or("active_suspension_enable", false); - calib_wait_ = std::max(node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); - calib_sample_ = - std::max(node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); - } - - bool enabled() const { return enabled_; } - void set_enabled(bool v) { enabled_ = v; } - - void update( - const JointFeedbackFrame& fb, double imu_pitch, double imu_roll, double imu_pitch_rate, - double imu_roll_rate, double dt, bool requested, double min_angle_deg, double max_angle_deg, - const Eigen::Vector2d& control_accel, - std::array& current_target_physical_angles, - std::array& out_suspension_mode, - std::array& out_suspension_torque) { - static constexpr auto deg_to_rad = [](double d) { return d * std::numbers::pi / 180.0; }; - - clear_outputs_(out_suspension_mode, out_suspension_torque); - prepare_commands_(); - - if (!requested) { - reset_state_(); - current_target_physical_angles = requested_target_angles_; - publish_outputs_(out_suspension_mode, out_suspension_torque); - return; - } + void validate_joint_feedback_inputs() const { + const bool ready = + joint_feedback_source_ == JointFeedbackSource::kMotorAngle + ? left_front_joint_angle_.ready() && left_back_joint_angle_.ready() + && right_front_joint_angle_.ready() && right_back_joint_angle_.ready() + : left_front_joint_encoder_angle_.ready() && left_back_joint_encoder_angle_.ready() + && right_front_joint_encoder_angle_.ready() + && right_back_joint_encoder_angle_.ready(); - const double deploy = deg_to_rad(min_angle_deg); - const double entry = deploy + params_.entry_offset; - const double ride_height = - std::clamp(deploy + params_.ride_height_offset, deploy, deg_to_rad(max_angle_deg)); - const double support_zero_angle = deploy - params_.preload_angle; - const double release = ride_height + params_.hold_travel; - - AttitudeBias bias = - compute_attitude_bias_(imu_pitch, imu_roll, imu_pitch_rate, imu_roll_rate, dt); - if (!std::isfinite(bias.pitch_force) || !std::isfinite(bias.roll_force)) { - reset_state_(); - current_target_physical_angles.fill(deploy); - publish_outputs_(out_suspension_mode, out_suspension_torque); + if (ready) return; - } - update_leg_contact_estimates_(fb); - update_leg_states_(fb, entry, release, dt); - compute_leg_support_intents_(fb, bias, support_zero_angle, ride_height, control_accel); - current_target_physical_angles = target_angles_; - publish_outputs_(out_suspension_mode, out_suspension_torque); - } - - void update_imu_calibration( - bool symmetric_targets, double imu_pitch, double imu_roll, double dt) { - if (!symmetric_targets) { - calib_hold_ = 0.0; - calib_count_ = 0; - calib_pitch_sum_ = 0.0; - calib_roll_sum_ = 0.0; - calib_done_ = false; - return; - } - if (!std::isfinite(imu_pitch) || !std::isfinite(imu_roll)) - return; - calib_hold_ += dt; - if (calib_hold_ < calib_wait_) - return; - double end = calib_wait_ + calib_sample_; - if (calib_hold_ < end) { - calib_pitch_sum_ += imu_pitch; - calib_roll_sum_ += imu_roll; - ++calib_count_; - return; - } - if (calib_done_) - return; - calib_done_ = true; - if (calib_count_ == 0) - return; - imu_pitch_offset_ = calib_pitch_sum_ / static_cast(calib_count_); - imu_roll_offset_ = calib_roll_sum_ / static_cast(calib_count_); + throw std::runtime_error( + joint_feedback_source_ == JointFeedbackSource::kMotorAngle + ? "missing V2 joint feedback interfaces: expected /chassis/*_joint/angle" + : "missing legacy joint feedback interfaces: expected /chassis/*_joint/" + "encoder_angle"); } - void reset_calibration() { - calib_hold_ = 0.0; - calib_count_ = 0; - calib_pitch_sum_ = 0.0; - calib_roll_sum_ = 0.0; - calib_done_ = false; - } + double joint_angle_deg( + const InputInterface& joint_angle, + const InputInterface& joint_encoder_angle, double joint_offset, + double legacy_fixed_compensation) const { + if (joint_feedback_source_ == JointFeedbackSource::kMotorAngle) + return wrap_deg(*joint_angle * rad_to_deg_); - void reset_all() { - reset_state_(); - reset_calibration(); + return wrap_deg(joint_offset) - wrap_deg(*joint_encoder_angle) + legacy_fixed_compensation; } - double target_vel_limit() const { return params_.target_physical_velocity_limit; } - double target_accel_limit() const { return params_.target_physical_acceleration_limit; } - double control_accel_limit() const { return params_.control_acceleration_limit; } + void update_mode_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + auto mode = *mode_; + if (switch_left == rmcs_msgs::Switch::DOWN) + return; -private: - SuspensionParams params_{}; - bool enabled_ = false; - AttitudePidAxis pitch_pid_, roll_pid_; - double imu_pitch_offset_ = 0.0, imu_roll_offset_ = 0.0; - double calib_wait_ = 0.0, calib_sample_ = 0.0; - double calib_hold_ = 0.0; - size_t calib_count_ = 0; - double calib_pitch_sum_ = 0.0, calib_roll_sum_ = 0.0; - bool calib_done_ = false; - std::array suspension_active_{}; - std::array leg_states_{}; - std::array leg_commands_{}; - std::array requested_target_angles_{}; - std::array target_angles_{}; - - static double deg_to_rad_(double d) { return d * std::numbers::pi / 180.0; } - - void clear_outputs_( - std::array& modes, std::array& torques) { - modes.fill(false); - torques.fill(kNaN); - } - void publish_outputs_( - std::array& modes, std::array& torques) { - for (size_t i = 0; i < kJointCount; ++i) { - modes[i] = leg_commands_[i].suspension_mode; - torques[i] = leg_commands_[i].suspension_torque; + if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE + && switch_right == rmcs_msgs::Switch::DOWN) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::LAUNCH_RAMP; + } else if (!last_keyboard_.z && keyboard.z) { + mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; } - } - void reset_state_() { - suspension_active_.fill(false); - for (size_t i = 0; i < kJointCount; ++i) { - leg_states_[i] = LegControlState{}; - leg_commands_[i] = LegCommand{}; - } + *mode_ = mode; } - void prepare_commands_() { - for (size_t i = 0; i < kJointCount; ++i) { - leg_commands_[i] = LegCommand{ - .requested_target_angle = requested_target_angles_[i], - .final_target_angle = target_angles_[i], - }; - leg_states_[i].output_active = false; - leg_states_[i].support_force = 0.0; - } + // JointFeedbackAdapter: normalize motor / physical / legacy encoder feedback into one frame. + JointFeedbackFrame read_joint_feedback_frame_() const { + JointFeedbackFrame joint_feedback; + update_current_joint_feedback( + joint_feedback.motor_angles, joint_feedback.physical_angles, + joint_feedback.physical_velocities, joint_feedback.joint_torques); + return joint_feedback; } - static LegFeedback leg_feedback_at_(const JointFeedbackFrame& fb, size_t i) { - return { - .motor_angle = fb.motor_angles[i], - .physical_angle = fb.physical_angles[i], - .physical_velocity = fb.physical_velocities[i], - .joint_torque = fb.joint_torques[i], - .eso_z2 = fb.eso_z2[i], - .eso_z3 = fb.eso_z3[i]}; + bool prone_override_requested_() const { return keyboard_.ready() && keyboard_->ctrl; } + + bool suspension_requested_by_switch_() const { + return switch_left_.ready() && switch_right_.ready() + && *switch_left_ == rmcs_msgs::Switch::DOWN && *switch_right_ == rmcs_msgs::Switch::UP; } - double estimate_contact_(const LegFeedback& lf) const { - double c = 1.0; - if (std::isfinite(lf.eso_z3)) - c -= std::clamp(std::abs(lf.eso_z3) / 80.0, 0.0, 0.5); - if (std::isfinite(lf.joint_torque)) - c += std::clamp(std::abs(lf.joint_torque) / 20.0, 0.0, 0.3); - if (std::isfinite(lf.physical_velocity)) - c -= std::clamp(std::abs(lf.physical_velocity) / 10.0, 0.0, 0.2); - return std::clamp(c, 0.0, 1.0); + bool suspension_requested_by_input_() const { + return active_suspension_enable_ + && (prone_override_requested_() || suspension_requested_by_switch_()); } - bool contact_ready_(const LegControlState& s) const { - return s.contact_latched - || s.filtered_contact_confidence >= kContactConfidenceEnterThreshold; + bool symmetric_joint_target_requested_() const { + constexpr double epsilon = 1e-6; + return std::abs(lf_current_target_angle_ - lb_current_target_angle_) <= epsilon + && std::abs(lf_current_target_angle_ - rb_current_target_angle_) <= epsilon + && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; } - void update_leg_contact_estimates_(const JointFeedbackFrame& fb) { - for (size_t i = 0; i < kJointCount; ++i) { - auto& s = leg_states_[i]; - s.contact_confidence = estimate_contact_(leg_feedback_at_(fb, i)); - s.filtered_contact_confidence = std::clamp( - (1.0 - kContactConfidenceFilterAlpha) * s.filtered_contact_confidence - + kContactConfidenceFilterAlpha * s.contact_confidence, - 0.0, 1.0); - if (s.filtered_contact_confidence >= kContactConfidenceEnterThreshold) - s.contact_latched = true; - else if (s.filtered_contact_confidence <= kContactConfidenceExitThreshold) - s.contact_latched = false; + bool refresh_requested_joint_targets_from_deploy_state_() { + requested_target_physical_angles_rad_[kLeftFront] = deg_to_rad(lf_current_target_angle_); + requested_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); + requested_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); + requested_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); + + const bool prone_override = prone_override_requested_(); + if (suspension_requested_by_input_()) { + requested_target_physical_angles_rad_.fill(deg_to_rad(min_angle_)); } + + current_target_physical_angles_rad_ = requested_target_physical_angles_rad_; + return prone_override; } - void update_leg_states_(const JointFeedbackFrame& fb, double entry, double release, double dt) { - for (size_t i = 0; i < kJointCount; ++i) { - auto lf = leg_feedback_at_(fb, i); - bool rd = - std::isfinite(requested_target_angles_[i]) && requested_target_angles_[i] <= entry; - update_suspension_state_(i, lf, rd, entry, release, dt); - } + void reset_attitude_correction_state_() { + pitch_pid_.reset(); + roll_pid_.reset(); + joint_suspension_active_.fill(false); } - void compute_leg_support_intents_( - const JointFeedbackFrame& fb, const AttitudeBias& bias, double support_zero_angle, - double ride_height, const Eigen::Vector2d& control_accel) { - for (size_t i = 0; i < kJointCount; ++i) { - auto lf = leg_feedback_at_(fb, i); - auto& s = leg_states_[i]; - if (s.phase != SuspensionPhase::kActive) - continue; - s.support_force = - compute_leg_support_force_(i, lf, bias, support_zero_angle, control_accel); - leg_commands_[i].final_target_angle = ride_height; - leg_commands_[i].suspension_mode = true; - leg_commands_[i].suspension_torque = - leg_force_to_torque_(s.support_force, lf.physical_angle); - } + void reset_chassis_imu_calibration_window_() { + chassis_imu_calibration_hold_elapsed_ = 0.0; + chassis_imu_calibration_sample_count_ = 0; + chassis_imu_pitch_sum_ = 0.0; + chassis_imu_roll_sum_ = 0.0; + chassis_imu_calibration_completed_for_window_ = false; } - AttitudeBias compute_attitude_bias_( - double pitch, double roll, double pitch_rate, double roll_rate, double dt) { - double corrected_pitch = - std::clamp(pitch - imu_pitch_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); - double corrected_roll = - std::clamp(roll - imu_roll_offset_, -kMaxAttitudeRad, kMaxAttitudeRad); - double pitch_force = pitch_pid_.update(-corrected_pitch, pitch_rate, dt); - double roll_force = roll_pid_.update(corrected_roll, -roll_rate, dt); - if (!std::isfinite(pitch_force) || !std::isfinite(roll_force)) - return {kNaN, kNaN}; - return {pitch_force, roll_force}; - } - - double compute_leg_support_force_( - size_t i, const LegFeedback& lf, const AttitudeBias& bias, double support_zero_angle, - const Eigen::Vector2d& control_accel) const { - double f = params_.gravity_comp_gain * params_.mass * kGravity / 4.0 - + params_.Kz * (lf.physical_angle - support_zero_angle) - + params_.D_leg * lf.physical_velocity; - f += kPitchSigns[i] * bias.pitch_force + kRollSigns[i] * bias.roll_force; - if (params_.com_height > 0.0 && params_.wheel_base_half_x > 1e-6 - && params_.wheel_base_half_y > 1e-6) { - f += kPitchSigns[i] * params_.mass * control_accel.x() * params_.com_height - / (4.0 * params_.wheel_base_half_x); - f += kRollSigns[i] * params_.mass * control_accel.y() * params_.com_height - / (4.0 * params_.wheel_base_half_y); + void update_chassis_imu_calibration_() { + if (!symmetric_joint_target_requested_()) { + reset_chassis_imu_calibration_window_(); + return; } - return std::max(f, 0.0); - } - - double leg_force_to_torque_(double force, double angle) const { - return std::clamp( - force * params_.rod_length * std::max(std::sin(angle), kMinForceArmSin), - -params_.torque_limit, params_.torque_limit); - } - - void update_suspension_state_( - size_t i, const LegFeedback& lf, bool rd, double entry, double release, double dt) { - auto& s = leg_states_[i]; - s.phase_elapsed += (s.requested_deploy != rd) ? 0.0 : dt; - s.requested_deploy = rd; - if (!rd || !std::isfinite(lf.physical_angle) || !std::isfinite(lf.physical_velocity)) { - s.phase = SuspensionPhase::kInactive; - suspension_active_[i] = false; - s.output_active = false; - s.phase_elapsed = 0.0; - s.contact_latched = false; + + const double raw_pitch = *chassis_imu_pitch_; + const double raw_roll = *chassis_imu_roll_; + if (!std::isfinite(raw_pitch) || !std::isfinite(raw_roll)) + return; + + chassis_imu_calibration_hold_elapsed_ += update_dt(); + if (chassis_imu_calibration_hold_elapsed_ < chassis_imu_calibration_wait_time_) + return; + + const double calibration_end_time = + chassis_imu_calibration_wait_time_ + chassis_imu_calibration_sample_time_; + if (chassis_imu_calibration_hold_elapsed_ < calibration_end_time) { + chassis_imu_pitch_sum_ += raw_pitch; + chassis_imu_roll_sum_ += raw_roll; + ++chassis_imu_calibration_sample_count_; return; } - bool eok = lf.physical_angle <= entry; - bool vok = std::abs(lf.physical_velocity) <= params_.activation_velocity_threshold; - bool cok = contact_ready_(s); - switch (s.phase) { - case SuspensionPhase::kInactive: - suspension_active_[i] = false; - s.output_active = false; - if (eok) { - s.phase = SuspensionPhase::kArming; - s.phase_elapsed = 0.0; - } - break; - case SuspensionPhase::kArming: - suspension_active_[i] = false; - s.output_active = false; - if (!eok) { - s.phase = SuspensionPhase::kInactive; - s.phase_elapsed = 0.0; - break; + + if (chassis_imu_calibration_completed_for_window_) + return; + + chassis_imu_calibration_completed_for_window_ = true; + if (chassis_imu_calibration_sample_count_ == 0) { + RCLCPP_WARN( + get_logger(), + "[chassis imu calibration] skipped because no valid samples were collected"); + return; + } + + chassis_imu_pitch_offset_ = + chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_); + chassis_imu_roll_offset_ = + chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_); + RCLCPP_INFO( + get_logger(), + "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " + "(samples=%zu)", + chassis_imu_pitch_offset_ * rad_to_deg_, chassis_imu_roll_offset_ * rad_to_deg_, + chassis_imu_calibration_sample_count_); + } + + void update_current_joint_feedback( + std::array& current_motor_angles, + std::array& current_physical_angles, + std::array& current_physical_velocities, + std::array& current_joint_torques) const { + const std::array*, kJointCount> motor_angle_inputs{ + &left_front_joint_angle_, &left_back_joint_angle_, &right_back_joint_angle_, + &right_front_joint_angle_}; + const std::array*, kJointCount> physical_angle_inputs{ + &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, + &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; + const std::array*, kJointCount> physical_velocity_inputs{ + &left_front_joint_physical_velocity_, &left_back_joint_physical_velocity_, + &right_back_joint_physical_velocity_, &right_front_joint_physical_velocity_}; + const std::array*, kJointCount> torque_inputs{ + &left_front_joint_torque_, &left_back_joint_torque_, &right_back_joint_torque_, + &right_front_joint_torque_}; + current_motor_angles.fill(nan_); + current_physical_angles.fill(nan_); + current_physical_velocities.fill(nan_); + current_joint_torques.fill(nan_); + + for (size_t i = 0; i < kJointCount; ++i) { + if (motor_angle_inputs[i]->ready() && std::isfinite(*(*motor_angle_inputs[i]))) { + current_motor_angles[i] = *(*motor_angle_inputs[i]); + current_physical_angles[i] = motor_to_physical_angle(current_motor_angles[i]); } - if (vok && std::isfinite(lf.motor_angle) - && (cok || s.phase_elapsed >= kMinimumArmingTime)) { - suspension_active_[i] = true; - s.phase = SuspensionPhase::kActive; - s.output_active = true; - s.phase_elapsed = 0.0; + + if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { + current_physical_angles[i] = *(*physical_angle_inputs[i]); } - break; - case SuspensionPhase::kActive: - if (lf.physical_angle > release || (!cok && s.phase_elapsed >= kMinimumArmingTime)) { - suspension_active_[i] = false; - s.phase = SuspensionPhase::kReleasing; - s.output_active = false; - s.phase_elapsed = 0.0; - break; + if (physical_velocity_inputs[i]->ready() + && std::isfinite(*(*physical_velocity_inputs[i]))) { + current_physical_velocities[i] = *(*physical_velocity_inputs[i]); } - suspension_active_[i] = true; - s.output_active = true; - break; - case SuspensionPhase::kReleasing: - suspension_active_[i] = false; - s.output_active = false; - if (!eok || s.phase_elapsed >= kMinimumArmingTime) { - s.phase = SuspensionPhase::kInactive; - s.phase_elapsed = 0.0; + if (torque_inputs[i]->ready() && std::isfinite(*(*torque_inputs[i]))) { + current_joint_torques[i] = *(*torque_inputs[i]); } - break; } } -}; -// ============================================================ -// DeformableChassis — thin orchestrator -// ============================================================ -class DeformableChassis - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableChassis() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - - const double spin_ratio = std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0); - const double min_angle = get_parameter_or("min_angle", 15.0); - const double max_angle = get_parameter_or("max_angle", 55.0); - const double vel_limit = std::max( - std::abs(get_parameter_or("target_physical_velocity_limit", 180.0)) * std::numbers::pi - / 180.0, - 1e-6); - const double accel_limit = std::max( - std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0)) - * std::numbers::pi / 180.0, - 1e-6); - - velocity_control_.configure(spin_ratio); - suspension_.load_params(*this, min_angle, max_angle); - trajectory_.init(min_angle, max_angle, vel_limit, accel_limit); - - for (size_t i = 0; i < kJointCount; ++i) - joint_offsets_[i] = - get_parameter_or(std::string(kJointNames[i]) + "_joint_offset", 0.0); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); - register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); - register_input("/predefined/update_rate", update_rate_); - register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); - register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - - auto reg_joint_input = [this](size_t i) { - const auto& name = kJointNames[i]; - auto& j = joints_[i]; - register_input(joint_path_(name, "angle"), j.angle, false); - register_input(joint_path_(name, "physical_angle"), j.physical_angle, false); - register_input(joint_path_(name, "physical_velocity"), j.physical_velocity, false); - register_input(joint_path_(name, "torque"), j.torque, false); - register_input(joint_path_(name, "encoder_angle"), j.encoder_angle, false); - register_input(joint_path_(name, "eso_z2"), j.eso_z2, false); - register_input(joint_path_(name, "eso_z3"), j.eso_z3, false); - }; - for (size_t i = 0; i < kJointCount; ++i) - reg_joint_input(i); + bool initialize_joint_target_states_from_feedback( + const std::array& current_motor_angles, + const std::array& current_physical_angles) { + for (size_t i = 0; i < kJointCount; ++i) { + if (!std::isfinite(current_motor_angles[i]) + || !std::isfinite(current_physical_angles[i])) + return false; + } - register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); - register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + joint_target_angle_state_rad_ = current_motor_angles; + joint_target_physical_angle_state_rad_ = current_physical_angles; + joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + requested_target_physical_angles_rad_ = current_physical_angles; + current_target_physical_angles_rad_ = current_physical_angles; + joint_target_active_ = true; + return true; + } - register_output("/gimbal/scope/control_torque", scope_motor_control_torque_, kNaN); - register_output("/chassis/angle", chassis_angle_, kNaN); - register_output("/chassis/control_angle", chassis_control_angle_, kNaN); - register_output("/chassis/control_mode", mode_); - register_output("/chassis/control_velocity", chassis_control_velocity_); + void update_active_suspension_(const JointFeedbackFrame&) { + if (!suspension_requested_by_input_()) { + reset_attitude_correction_state_(); + return; + } - auto reg_joint_output = [this](size_t i) { - const auto& name = kJointNames[i]; - auto& j = joints_[i]; - register_output(joint_path_(name, "control_angle_error"), angle_errors_[i], kNaN); - register_output(joint_path_(name, "target_angle"), j.target_angle, kNaN); - register_output( - joint_path_(name, "target_physical_angle"), j.target_physical_angle, kNaN); - register_output( - joint_path_(name, "target_physical_velocity"), j.target_physical_velocity, kNaN); - register_output( - joint_path_(name, "target_physical_acceleration"), j.target_physical_acceleration, - kNaN); - register_output(joint_path_(name, "suspension_torque"), j.suspension_torque, kNaN); - }; - for (size_t i = 0; i < kJointCount; ++i) { - reg_joint_output(i); - register_output( - joint_path_(kJointNames[i], "suspension_mode"), suspension_modes_[i], false); + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + const double base_target_angle = deg_to_rad(min_angle_); + const double max_target_angle = deg_to_rad(max_angle_); + const double corrected_pitch = + std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); + const double corrected_roll = + std::clamp(*chassis_imu_roll_ - chassis_imu_roll_offset_, -max_attitude, max_attitude); + const double corrected_pitch_rate = *chassis_imu_pitch_rate_; + const double corrected_roll_rate = *chassis_imu_roll_rate_; + + const double dt = update_dt(); + const double pitch_angle_diff = + pitch_pid_.update(-corrected_pitch, corrected_pitch_rate, dt); + const double roll_angle_diff = + roll_pid_.update(corrected_roll, -corrected_roll_rate, dt); + if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { + reset_attitude_correction_state_(); + current_target_physical_angles_rad_.fill(base_target_angle); + return; } - register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, kNaN); + // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left + // pair. Every leg starts from min_angle and only receives additive corrections so at least + // one leg always stays at min_angle. + const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); + const double back_pitch_add = std::max(pitch_angle_diff, 0.0); + const double left_roll_add = std::max(roll_angle_diff, 0.0); + const double right_roll_add = std::max(-roll_angle_diff, 0.0); + + current_target_physical_angles_rad_[kLeftFront] = + std::clamp(base_target_angle + front_pitch_add + left_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kLeftBack] = + std::clamp(base_target_angle + back_pitch_add + left_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kRightBack] = + std::clamp(base_target_angle + back_pitch_add + right_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kRightFront] = + std::clamp(base_target_angle + front_pitch_add + right_roll_add, base_target_angle, + max_target_angle); + + joint_suspension_active_.fill(true); + } + + void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; - chassis_control_velocity_->vector << kNaN, kNaN, kNaN; + reset_attitude_correction_state_(); + reset_chassis_imu_calibration_window_(); + + chassis_control_velocity_->vector << nan_, nan_, nan_; + *chassis_angle_ = nan_; + *chassis_control_angle_ = nan_; + + current_target_angle_ = max_angle_; + lf_current_target_angle_ = current_target_angle_; + lb_current_target_angle_ = current_target_angle_; + rb_current_target_angle_ = current_target_angle_; + rf_current_target_angle_ = current_target_angle_; + joint_target_active_ = false; + + *scope_motor_control_torque = nan_; + + *lf_angle_error_ = nan_; + *lb_angle_error_ = nan_; + *rf_angle_error_ = nan_; + *rb_angle_error_ = nan_; + + *left_front_joint_target_angle_ = nan_; + *left_back_joint_target_angle_ = nan_; + *right_back_joint_target_angle_ = nan_; + *right_front_joint_target_angle_ = nan_; + + *left_front_joint_target_physical_angle_ = nan_; + *left_back_joint_target_physical_angle_ = nan_; + *right_back_joint_target_physical_angle_ = nan_; + *right_front_joint_target_physical_angle_ = nan_; + *left_front_joint_target_physical_velocity_ = nan_; + *left_back_joint_target_physical_velocity_ = nan_; + *right_back_joint_target_physical_velocity_ = nan_; + *right_front_joint_target_physical_velocity_ = nan_; + *left_front_joint_target_physical_acceleration_ = nan_; + *left_back_joint_target_physical_acceleration_ = nan_; + *right_back_joint_target_physical_acceleration_ = nan_; + *right_front_joint_target_physical_acceleration_ = nan_; + + *processed_encoder_angle_ = nan_; + } - int offset_count = 0; - for (size_t i = 0; i < kJointCount; ++i) - if (has_parameter(std::string(kJointNames[i]) + "_joint_offset")) - ++offset_count; - if (offset_count > 0 && offset_count != static_cast(kJointCount)) - throw std::runtime_error( - "joint offsets must be configured for all four joints or removed entirely"); - joint_feedback_source_ = (offset_count == static_cast(kJointCount)) - ? JointFeedbackSource::kLegacyEncoderAngle - : JointFeedbackSource::kMotorAngle; + void update_velocity_control() { + const Eigen::Vector2d translational_velocity = update_translational_velocity_control(); + const double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, angular_velocity; } - void before_updating() override { - auto ensure = [this](auto& field, double value, const char* name) { - if (!field.ready()) { - field.make_and_bind_directly(value); - RCLCPP_WARN(get_logger(), "Failed to fetch \"%s\". Set to %.1f.", name, value); - } - }; - ensure(gimbal_yaw_angle_, 0.0, "/gimbal/yaw/angle"); - ensure(gimbal_yaw_angle_error_, 0.0, "/gimbal/yaw/control_angle_error"); - for (auto& j : joints_) - ensure(j.torque, 0.0, "joint torque"); - ensure(chassis_imu_pitch_, 0.0, "chassis imu pitch"); - ensure(chassis_imu_roll_, 0.0, "chassis imu roll"); - ensure(chassis_imu_pitch_rate_, 0.0, "chassis imu pitch_rate"); - ensure(chassis_imu_roll_rate_, 0.0, "chassis imu roll_rate"); - validate_joint_feedback_inputs_(); + double update_dt() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return default_dt_; } - void update() override { - using rmcs_msgs::Switch; - const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; + Eigen::Vector2d update_translational_velocity_control() { const auto keyboard = *keyboard_; - do { - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls_(); - break; - } - update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_velocity_control_(keyboard); - update_lift_target_toggle_(keyboard); - run_joint_intent_pipeline_(); - } while (false); - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; - } + const Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; -private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - static constexpr double kRadToDeg = 180.0 / std::numbers::pi; + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * ((*joystick_right_) + keyboard_move); - static std::string joint_path_(const char* name, const char* suffix) { - char b[128]; - std::snprintf(b, sizeof(b), "/chassis/%s_joint/%s", name, suffix); - return {b}; + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max_; + return translational_velocity; } - void validate_joint_feedback_inputs_() const { - bool ok = true; - for (const auto& j : joints_) { - if (joint_feedback_source_ == JointFeedbackSource::kMotorAngle) { - if (!j.angle.ready()) - ok = false; - } else { - if (!j.encoder_angle.ready()) - ok = false; + double update_angular_velocity_control() { + double angular_velocity = 0.0; + double chassis_control_angle = nan_; + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: break; + + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = + spin_ratio_ * (spinning_forward_ ? angular_velocity_max_ : -angular_velocity_max_); + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + } break; + + case rmcs_msgs::ChassisMode::STEP_DOWN: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // In step-down mode, front/back can both be used for alignment. + constexpr double alignment = std::numbers::pi; + while (err > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + err -= alignment; } + + angular_velocity = following_velocity_controller_.update(err); + } break; + + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + } break; + + default: break; } - if (ok) - return; - throw std::runtime_error( - joint_feedback_source_ == JointFeedbackSource::kMotorAngle - ? "missing V2 joint feedback inputs: /chassis/*_joint/angle" - : "missing legacy joint feedback inputs: /chassis/*_joint/encoder_angle"); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; } - // --- helpers --- - bool suspension_requested_() const { - return suspension_.enabled() - && (keyboard_->ctrl - || (switch_left_.ready() && switch_right_.ready() - && *switch_left_ == rmcs_msgs::Switch::DOWN - && *switch_right_ == rmcs_msgs::Switch::UP)); + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = *gimbal_yaw_angle_error_; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + + double err = chassis_control_angle + *gimbal_yaw_angle_; + if (err >= 2 * std::numbers::pi) + err -= 2 * std::numbers::pi; + + return err; } - // --- mode --- - void update_mode_from_inputs_( - rmcs_msgs::Switch sl, rmcs_msgs::Switch sr, const rmcs_msgs::Keyboard& kb) { - auto m = *mode_; - if (sl == rmcs_msgs::Switch::DOWN) - return; - if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && sr == rmcs_msgs::Switch::DOWN) { - m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::STEP_DOWN - : rmcs_msgs::ChassisMode::SPIN; - } else if (!last_keyboard_.c && kb.c) { - m = (m == rmcs_msgs::ChassisMode::SPIN) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::SPIN; - } else if (!last_keyboard_.x && kb.x) { - m = (m == rmcs_msgs::ChassisMode::LAUNCH_RAMP) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; - } else if (!last_keyboard_.z && kb.z) { - m = (m == rmcs_msgs::ChassisMode::STEP_DOWN) ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; + void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { + constexpr double rotary_knob_edge_threshold = 0.7; + + const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; + const bool rotary_knob_toggle_condition = + last_rotary_knob_ < rotary_knob_edge_threshold + && *rotary_knob_ >= rotary_knob_edge_threshold; + const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + + if (apply_symmetric_target) { + lf_current_target_angle_ = current_target_angle_; + lb_current_target_angle_ = current_target_angle_; + rb_current_target_angle_ = current_target_angle_; + rf_current_target_angle_ = current_target_angle_; } - *mode_ = m; - } - // --- velocity --- - void update_velocity_control_(const rmcs_msgs::Keyboard& kb) { - auto tv = velocity_control_.compute_translational(*joystick_right_, kb, *gimbal_yaw_angle_); - bool toggle = (last_keyboard_.c != kb.c && *mode_ != rmcs_msgs::ChassisMode::SPIN); - auto ar = velocity_control_.compute_angular( - *mode_, *gimbal_yaw_angle_, *gimbal_yaw_angle_error_, toggle); - double dt = update_dt_(); - velocity_control_.update_acceleration_estimate(tv, dt, suspension_.control_accel_limit()); - *chassis_angle_ = ar.chassis_angle; - *chassis_control_angle_ = ar.chassis_control_angle; - chassis_control_velocity_->vector << tv, ar.angular_velocity; - } + if (rotary_knob_toggle_condition || keyboard_toggle_condition) { + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + apply_symmetric_target = true; + } else if (front_high_rear_low) { + lf_current_target_angle_ = max_angle_; + rf_current_target_angle_ = max_angle_; + lb_current_target_angle_ = min_angle_; + rb_current_target_angle_ = min_angle_; + apply_symmetric_target = false; + } else if (front_low_rear_high) { + lf_current_target_angle_ = min_angle_; + rf_current_target_angle_ = min_angle_; + lb_current_target_angle_ = max_angle_; + rb_current_target_angle_ = max_angle_; + apply_symmetric_target = false; + } - double update_dt_() const { - if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) - return 1.0 / *update_rate_; - return 1e-3; + last_rotary_knob_ = *rotary_knob_; } - // --- lift toggle --- - void update_lift_target_toggle_(rmcs_msgs::Keyboard keyboard) { - constexpr double kRotaryKnobEdgeThreshold = 0.7; + // Chassis owns the high-level joint intent pipeline: read feedback, generate deploy targets, + // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. + void run_joint_intent_pipeline_() { + const auto joint_feedback = read_joint_feedback_frame_(); + + if (!joint_target_active_ + && !initialize_joint_target_states_from_feedback( + joint_feedback.motor_angles, joint_feedback.physical_angles)) { + publish_nan_joint_targets(); + return; + } - const bool keyboard_toggle = !last_keyboard_.q && keyboard.q; - const bool rotary_knob_toggle = last_rotary_knob_ < kRotaryKnobEdgeThreshold - && *rotary_knob_ >= kRotaryKnobEdgeThreshold; + update_chassis_imu_calibration_(); + const bool prone_override = refresh_requested_joint_targets_from_deploy_state_(); + scope_motor_control(prone_override); + update_active_suspension_(joint_feedback); + update_joint_target_trajectory(); + publish_joint_target_angles(joint_feedback.physical_angles); + } - if (apply_symmetric_target_) - trajectory_.fill_symmetric_targets(); + static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - if (rotary_knob_toggle || keyboard_toggle) { - trajectory_.set_target_angle( - std::abs(trajectory_.target_angle() - trajectory_.max_angle()) < 1e-6 - ? trajectory_.min_angle() - : trajectory_.max_angle()); - apply_symmetric_target_ = true; - } + static double physical_to_motor_angle(double physical_angle_rad) { + return joint_zero_physical_angle_rad_ - physical_angle_rad; + } - last_rotary_knob_ = *rotary_knob_; + static double motor_to_physical_angle(double motor_angle_rad) { + return joint_zero_physical_angle_rad_ - motor_angle_rad; } - // --- reset --- - void reset_all_controls_() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; - velocity_control_.reset_acceleration_estimate(); - suspension_.reset_all(); - chassis_control_velocity_->vector << kNaN, kNaN, kNaN; - *chassis_angle_ = kNaN; - *chassis_control_angle_ = kNaN; - trajectory_.reset(trajectory_.max_angle()); - *scope_motor_control_torque_ = kNaN; - for (auto& e : angle_errors_) - *e = kNaN; - for (auto& j : joints_) { - *j.target_angle = kNaN; - *j.target_physical_angle = kNaN; - *j.target_physical_velocity = kNaN; - *j.target_physical_acceleration = kNaN; - *j.suspension_torque = kNaN; + void scope_motor_control(bool prone_override = false) { + const bool prone_target_active = prone_override; + if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { + *scope_motor_control_torque = -0.3; + // if (*scope_motor_velocity <= std::abs(0.1)){ + // *scope_motor_control_torque = 0.18 * 1.0 / 36.0; + // } + } else { + *scope_motor_control_torque = 0.3; + // if (*scope_motor_velocity <= std::abs(0.1)){ + // *scope_motor_control_torque = -0.18 * 1.0 / 36.0; + // } } - for (auto& m : suspension_modes_) - *m = false; - *processed_encoder_angle_ = kNaN; - } - - // --- feedback --- - JointFeedbackFrame read_joint_feedback_() const { - JointFeedbackFrame f; - f.motor_angles.fill(kNaN); - f.physical_angles.fill(kNaN); - f.physical_velocities.fill(kNaN); - f.joint_torques.fill(kNaN); - f.eso_z2.fill(kNaN); - f.eso_z3.fill(kNaN); + } + + bool publish_current_joint_target_angles() { + const std::array*, kJointCount> motor_angle_inputs{ + &left_front_joint_angle_, &left_back_joint_angle_, &right_back_joint_angle_, + &right_front_joint_angle_}; + + std::array current_motor_angles{}; + std::array current_physical_angles{}; for (size_t i = 0; i < kJointCount; ++i) { - const auto& j = joints_[i]; - if (j.angle.ready() && std::isfinite(*j.angle)) { - f.motor_angles[i] = *j.angle; - f.physical_angles[i] = 1.090830782496456 - f.motor_angles[i]; + if (!motor_angle_inputs[i]->ready() || !std::isfinite(*(*motor_angle_inputs[i]))) { + return false; } - if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) - f.physical_angles[i] = *j.physical_angle; - if (j.physical_velocity.ready() && std::isfinite(*j.physical_velocity)) - f.physical_velocities[i] = *j.physical_velocity; - if (j.torque.ready() && std::isfinite(*j.torque)) - f.joint_torques[i] = *j.torque; - if (j.eso_z2.ready() && std::isfinite(*j.eso_z2)) - f.eso_z2[i] = *j.eso_z2; - if (j.eso_z3.ready() && std::isfinite(*j.eso_z3)) - f.eso_z3[i] = *j.eso_z3; + current_motor_angles[i] = *(*motor_angle_inputs[i]); + current_physical_angles[i] = motor_to_physical_angle(current_motor_angles[i]); } - return f; + + joint_target_angle_state_rad_ = current_motor_angles; + joint_target_physical_angle_state_rad_ = current_physical_angles; + joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + requested_target_physical_angles_rad_ = current_physical_angles; + current_target_physical_angles_rad_ = current_physical_angles; + joint_target_active_ = true; + return true; } - // --- main pipeline --- - void run_joint_intent_pipeline_() { - auto feedback = read_joint_feedback_(); + void update_joint_target_trajectory() { + const double dt = update_dt(); + for (size_t i = 0; i < kJointCount; ++i) { + double& angle_state = joint_target_physical_angle_state_rad_[i]; + double& velocity_state = joint_target_physical_velocity_state_rad_[i]; + double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; + const double target_angle = current_target_physical_angles_rad_[i]; + const double velocity_limit = joint_suspension_active_[i] + ? suspension_velocity_limit_ + : target_physical_velocity_limit_; + const double acceleration_limit = + joint_suspension_active_[i] ? suspension_acceleration_limit_ + : target_physical_acceleration_limit_; + + if (!std::isfinite(target_angle) || !std::isfinite(angle_state)) { + continue; + } - suspension_.update_imu_calibration( - trajectory_.symmetric_requested(), *chassis_imu_pitch_, *chassis_imu_roll_, - update_dt_()); + const double position_error = target_angle - angle_state; + const double stopping_distance = + velocity_state * velocity_state / (2.0 * acceleration_limit); - if (!trajectory_.active() - && !trajectory_.initialize_from_feedback( - feedback.motor_angles, feedback.physical_angles)) { - reset_all_controls_(); + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { + desired_velocity = std::copysign(velocity_limit, position_error); + } + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = + std::clamp(velocity_error / dt, -acceleration_limit, acceleration_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -velocity_limit, velocity_limit); + angle_state += velocity_state * dt; + + const double next_error = target_angle - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target_angle; + velocity_state = 0.0; + acceleration_state = 0.0; + } + + joint_target_angle_state_rad_[i] = physical_to_motor_angle(angle_state); + } + } + + void publish_joint_target_angles( + const std::array& current_physical_angles) { + if (!joint_target_active_) { + publish_nan_joint_targets(); return; } - if (apply_symmetric_target_) - trajectory_.fill_symmetric_targets(); - trajectory_.refresh_deploy_targets( - suspension_requested_(), keyboard_->ctrl, trajectory_.min_angle()); + *left_front_joint_target_angle_ = joint_target_angle_state_rad_[kLeftFront]; + *left_back_joint_target_angle_ = joint_target_angle_state_rad_[kLeftBack]; + *right_back_joint_target_angle_ = joint_target_angle_state_rad_[kRightBack]; + *right_front_joint_target_angle_ = joint_target_angle_state_rad_[kRightFront]; + + *left_front_joint_target_physical_angle_ = + joint_target_physical_angle_state_rad_[kLeftFront]; + *left_back_joint_target_physical_angle_ = joint_target_physical_angle_state_rad_[kLeftBack]; + *right_back_joint_target_physical_angle_ = + joint_target_physical_angle_state_rad_[kRightBack]; + *right_front_joint_target_physical_angle_ = + joint_target_physical_angle_state_rad_[kRightFront]; + + *left_front_joint_target_physical_velocity_ = + joint_target_physical_velocity_state_rad_[kLeftFront]; + *left_back_joint_target_physical_velocity_ = + joint_target_physical_velocity_state_rad_[kLeftBack]; + *right_back_joint_target_physical_velocity_ = + joint_target_physical_velocity_state_rad_[kRightBack]; + *right_front_joint_target_physical_velocity_ = + joint_target_physical_velocity_state_rad_[kRightFront]; + + *left_front_joint_target_physical_acceleration_ = + joint_target_physical_acceleration_state_rad_[kLeftFront]; + *left_back_joint_target_physical_acceleration_ = + joint_target_physical_acceleration_state_rad_[kLeftBack]; + *right_back_joint_target_physical_acceleration_ = + joint_target_physical_acceleration_state_rad_[kRightBack]; + *right_front_joint_target_physical_acceleration_ = + joint_target_physical_acceleration_state_rad_[kRightFront]; + + *lf_angle_error_ = std::isfinite(current_physical_angles[kLeftFront]) + ? current_physical_angles[kLeftFront] + - joint_target_physical_angle_state_rad_[kLeftFront] + : nan_; + *lb_angle_error_ = std::isfinite(current_physical_angles[kLeftBack]) + ? current_physical_angles[kLeftBack] + - joint_target_physical_angle_state_rad_[kLeftBack] + : nan_; + *rb_angle_error_ = std::isfinite(current_physical_angles[kRightBack]) + ? current_physical_angles[kRightBack] + - joint_target_physical_angle_state_rad_[kRightBack] + : nan_; + *rf_angle_error_ = std::isfinite(current_physical_angles[kRightFront]) + ? current_physical_angles[kRightFront] + - joint_target_physical_angle_state_rad_[kRightFront] + : nan_; + + bool all_joint_angles_finite = true; + double physical_angle_sum = 0.0; + for (double current_physical_angle : current_physical_angles) { + if (!std::isfinite(current_physical_angle)) { + all_joint_angles_finite = false; + break; + } + physical_angle_sum += current_physical_angle; + } - scope_motor_control_(keyboard_->ctrl); + *processed_encoder_angle_ = all_joint_angles_finite ? rad_to_deg_ * physical_angle_sum + / static_cast(kJointCount) + : nan_; + } - auto target_physical = trajectory_.current_physical(); - std::array sus_modes, sus_torques; - sus_modes.fill(false); - sus_torques.fill(kNaN); + void publish_nan_joint_targets() { + reset_attitude_correction_state_(); - suspension_.update( - feedback, *chassis_imu_pitch_, *chassis_imu_roll_, *chassis_imu_pitch_rate_, - *chassis_imu_roll_rate_, update_dt_(), suspension_requested_(), trajectory_.min_angle(), - trajectory_.max_angle(), velocity_control_.control_acceleration_estimate(), - target_physical, sus_modes, sus_torques); + *left_front_joint_target_angle_ = nan_; + *left_back_joint_target_angle_ = nan_; + *right_back_joint_target_angle_ = nan_; + *right_front_joint_target_angle_ = nan_; - trajectory_.update_trajectory( - update_dt_(), suspension_requested_(), suspension_.target_vel_limit(), - suspension_.target_accel_limit()); + *left_front_joint_target_physical_angle_ = nan_; + *left_back_joint_target_physical_angle_ = nan_; + *right_back_joint_target_physical_angle_ = nan_; + *right_front_joint_target_physical_angle_ = nan_; - for (size_t i = 0; i < kJointCount; ++i) { - *joints_[i].target_angle = trajectory_.target_angles()[i]; - *joints_[i].target_physical_angle = trajectory_.target_physical_angles()[i]; - *joints_[i].target_physical_velocity = trajectory_.target_velocities()[i]; - *joints_[i].target_physical_acceleration = trajectory_.target_accelerations()[i]; - *joints_[i].suspension_torque = sus_torques[i]; - *suspension_modes_[i] = static_cast(sus_modes[i]); - } + *left_front_joint_target_physical_velocity_ = nan_; + *left_back_joint_target_physical_velocity_ = nan_; + *right_back_joint_target_physical_velocity_ = nan_; + *right_front_joint_target_physical_velocity_ = nan_; - if (apply_symmetric_target_ && trajectory_.symmetric_requested()) - trajectory_.fill_symmetric_targets(); + *left_front_joint_target_physical_acceleration_ = nan_; + *left_back_joint_target_physical_acceleration_ = nan_; + *right_back_joint_target_physical_acceleration_ = nan_; + *right_front_joint_target_physical_acceleration_ = nan_; - double sum = 0.0; - int cnt = 0; - for (const auto& j : joints_) { - if (j.physical_angle.ready() && std::isfinite(*j.physical_angle)) { - sum += *j.physical_angle; - ++cnt; - } - } - *processed_encoder_angle_ = (cnt > 0) ? kRadToDeg * sum / cnt : kNaN; - } + *lf_angle_error_ = nan_; + *lb_angle_error_ = nan_; + *rb_angle_error_ = nan_; + *rf_angle_error_ = nan_; - void scope_motor_control_(bool prone_override) { - if (prone_override && *mode_ != rmcs_msgs::ChassisMode::SPIN) - *scope_motor_control_torque_ = -0.3; - else - *scope_motor_control_torque_ = 0.3; } - // --- member variables --- - InputInterface joystick_right_, joystick_left_; - InputInterface switch_right_, switch_left_; +private: + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; InputInterface mouse_velocity_; InputInterface mouse_; InputInterface keyboard_; - InputInterface rotary_knob_, update_rate_; - double last_rotary_knob_ = 0.0; + InputInterface rotary_knob_; + InputInterface update_rate_; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + double last_rotary_knob_ = 0.0; InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; + OutputInterface mode_; OutputInterface chassis_control_velocity_; - ChassisVelocityControl velocity_control_; - ActiveSuspension suspension_; - JointTrajectoryPlanner trajectory_; - bool apply_symmetric_target_ = true; - - std::array joints_{}; - std::array angle_errors_; - std::array, kJointCount> suspension_modes_; - InputInterface chassis_imu_pitch_, chassis_imu_roll_, chassis_imu_pitch_rate_, - chassis_imu_roll_rate_; - OutputInterface scope_motor_control_torque_, processed_encoder_angle_; - - std::array joint_offsets_{}; // FIXME: Unused Var + bool spinning_forward_ = true; + bool apply_symmetric_target = true; + pid::PidCalculator following_velocity_controller_; + const double spin_ratio_; + + InputInterface left_front_joint_angle_; + InputInterface left_back_joint_angle_; + InputInterface right_front_joint_angle_; + InputInterface right_back_joint_angle_; + + InputInterface left_front_joint_physical_angle_; + InputInterface left_back_joint_physical_angle_; + InputInterface right_front_joint_physical_angle_; + InputInterface right_back_joint_physical_angle_; + InputInterface left_front_joint_physical_velocity_; + InputInterface left_back_joint_physical_velocity_; + InputInterface right_front_joint_physical_velocity_; + InputInterface right_back_joint_physical_velocity_; + InputInterface left_front_joint_torque_; + InputInterface left_back_joint_torque_; + InputInterface right_front_joint_torque_; + InputInterface right_back_joint_torque_; + + InputInterface left_front_joint_encoder_angle_; + InputInterface left_back_joint_encoder_angle_; + InputInterface right_front_joint_encoder_angle_; + InputInterface right_back_joint_encoder_angle_; + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + OutputInterface scope_motor_control_torque; + + OutputInterface lf_angle_error_; + OutputInterface lb_angle_error_; + OutputInterface rf_angle_error_; + OutputInterface rb_angle_error_; + + OutputInterface left_front_joint_target_angle_; + OutputInterface left_back_joint_target_angle_; + OutputInterface right_back_joint_target_angle_; + OutputInterface right_front_joint_target_angle_; + + OutputInterface left_front_joint_target_physical_angle_; + OutputInterface left_back_joint_target_physical_angle_; + OutputInterface right_back_joint_target_physical_angle_; + OutputInterface right_front_joint_target_physical_angle_; + OutputInterface left_front_joint_target_physical_velocity_; + OutputInterface left_back_joint_target_physical_velocity_; + OutputInterface right_back_joint_target_physical_velocity_; + OutputInterface right_front_joint_target_physical_velocity_; + OutputInterface left_front_joint_target_physical_acceleration_; + OutputInterface left_back_joint_target_physical_acceleration_; + OutputInterface right_back_joint_target_physical_acceleration_; + OutputInterface right_front_joint_target_physical_acceleration_; + + OutputInterface processed_encoder_angle_; + + double min_angle_; + double max_angle_; + double left_front_joint_offset_; + double left_back_joint_offset_; + double right_front_joint_offset_; + double right_back_joint_offset_; JointFeedbackSource joint_feedback_source_ = JointFeedbackSource::kLegacyEncoderAngle; + + double current_target_angle_; + double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, + rf_current_target_angle_; + + std::array requested_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; + + bool joint_target_active_ = false; + std::array joint_target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_physical_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_physical_velocity_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array joint_target_physical_acceleration_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + + double target_physical_velocity_limit_; + double target_physical_acceleration_limit_; + bool active_suspension_enable_; + double pitch_kp_; + double pitch_ki_; + double pitch_kd_; + double roll_kp_; + double roll_ki_; + double roll_kd_; + double suspension_velocity_limit_; + double suspension_acceleration_limit_; + double pitch_diff_limit_; + double roll_diff_limit_; + double pid_integral_limit_; + std::array joint_suspension_active_ = {false, false, false, false}; + AttitudePidAxis pitch_pid_; + AttitudePidAxis roll_pid_; + double chassis_imu_pitch_offset_ = 0.0; + double chassis_imu_roll_offset_ = 0.0; + double chassis_imu_calibration_wait_time_; + double chassis_imu_calibration_sample_time_; + double chassis_imu_calibration_hold_elapsed_ = 0.0; + size_t chassis_imu_calibration_sample_count_ = 0; + double chassis_imu_pitch_sum_ = 0.0; + double chassis_imu_roll_sum_ = 0.0; + bool chassis_imu_calibration_completed_for_window_ = false; + static constexpr double default_dt_ = 1e-3; + static constexpr double joint_zero_physical_angle_rad_ = 1.090830782496456; + + static constexpr double pi_ = std::numbers::pi; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp deleted file mode 100644 index beeb1d2f..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_layer.hpp +++ /dev/null @@ -1,172 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -namespace rmcs_core::controller::chassis { - -enum class JointFeedbackSource : uint8_t { kLegacyEncoderAngle, kMotorAngle }; - -enum JointIndex : size_t { - kLeftFront = 0, - kLeftBack = 1, - kRightBack = 2, - kRightFront = 3, - kJointCount = 4 -}; - -inline constexpr std::array kJointNames{ - "left_front", "left_back", "right_back", "right_front"}; - -struct JointFeedbackFrame { - std::array motor_angles{}; - std::array physical_angles{}; - std::array physical_velocities{}; - std::array joint_torques{}; - std::array eso_z2{}; - std::array eso_z3{}; -}; - -struct LegFeedback { - double motor_angle = std::numeric_limits::quiet_NaN(); - double physical_angle = std::numeric_limits::quiet_NaN(); - double physical_velocity = std::numeric_limits::quiet_NaN(); - double joint_torque = std::numeric_limits::quiet_NaN(); - double eso_z2 = std::numeric_limits::quiet_NaN(); - double eso_z3 = std::numeric_limits::quiet_NaN(); -}; - -struct JointIO { - using In = rmcs_executor::Component::InputInterface; - using Out = rmcs_executor::Component::OutputInterface; - In angle, physical_angle, physical_velocity, torque, encoder_angle, eso_z2, eso_z3; - Out target_angle, target_physical_angle, target_physical_velocity, target_physical_acceleration; - Out suspension_torque; -}; - -struct JointTrajectoryPlanner { - static constexpr double kJointZeroPhysicalAngleRad = 1.090830782496456; - - void init(double min_angle, double max_angle, double velocity_limit, double acceleration_limit) { - min_angle_ = min_angle; - max_angle_ = max_angle; - velocity_limit_ = velocity_limit; - acceleration_limit_ = acceleration_limit; - } - - void set_target_angle(double angle) { current_target_angle_ = angle; } - double target_angle() const { return current_target_angle_; } - - bool initialize_from_feedback( - const std::array& motor_angles, - const std::array& physical_angles) { - for (size_t i = 0; i < kJointCount; ++i) - if (!std::isfinite(motor_angles[i]) || !std::isfinite(physical_angles[i])) - return false; - target_motor_state_ = motor_angles; - target_physical_state_ = physical_angles; - target_velocity_state_.fill(0.0); - target_acceleration_state_.fill(0.0); - requested_physical_ = physical_angles; - current_physical_ = physical_angles; - active_ = true; - return true; - } - - void sync_from_feedback(size_t index, double motor_angle, double physical_angle) { - target_motor_state_[index] = motor_angle; - target_physical_state_[index] = physical_angle; - target_velocity_state_[index] = 0.0; - target_acceleration_state_[index] = 0.0; - } - - bool active() const { return active_; } - void set_active(bool value) { active_ = value; } - - void fill_symmetric_targets() { per_joint_targets_.fill(current_target_angle_); } - - bool symmetric_requested() const { - for (size_t i = 1; i < kJointCount; ++i) - if (std::abs(per_joint_targets_[0] - per_joint_targets_[i]) > 1e-6) - return false; - return true; - } - - void refresh_deploy_targets(bool deploy_requested, bool /*prone_override*/, double deploy_angle) { - for (size_t i = 0; i < kJointCount; ++i) - requested_physical_[i] = per_joint_targets_[i] * std::numbers::pi / 180.0; - if (deploy_requested) - requested_physical_.fill(deploy_angle * std::numbers::pi / 180.0); - current_physical_ = requested_physical_; - } - - void update_trajectory( - double delta_time, bool use_suspension_limits, - double suspension_velocity_limit, double suspension_acceleration_limit) { - double velocity_limit = use_suspension_limits ? suspension_velocity_limit : velocity_limit_; - double acceleration_limit = - use_suspension_limits ? suspension_acceleration_limit : acceleration_limit_; - for (size_t i = 0; i < kJointCount; ++i) { - double target = current_physical_[i]; - double current_position = target_physical_state_[i]; - double current_velocity = target_velocity_state_[i]; - double error = target - current_position; - double max_velocity = std::sqrt(2.0 * acceleration_limit * std::abs(error)); - double command_velocity = std::copysign(std::min(max_velocity, velocity_limit), error); - double delta_velocity = command_velocity - current_velocity; - double command_acceleration = std::clamp(delta_velocity / delta_time, -acceleration_limit, acceleration_limit); - target_acceleration_state_[i] = command_acceleration; - target_velocity_state_[i] = - std::clamp(current_velocity + command_acceleration * delta_time, -velocity_limit, velocity_limit); - target_physical_state_[i] += target_velocity_state_[i] * delta_time; - target_motor_state_[i] = kJointZeroPhysicalAngleRad - target_physical_state_[i]; - } - } - - const std::array& target_angles() const { return target_motor_state_; } - const std::array& target_physical_angles() const { - return target_physical_state_; - } - const std::array& target_velocities() const { - return target_velocity_state_; - } - const std::array& target_accelerations() const { - return target_acceleration_state_; - } - const std::array& current_physical() const { return current_physical_; } - - void reset(double angle) { - current_target_angle_ = angle; - per_joint_targets_.fill(angle); - active_ = false; - target_motor_state_.fill(0.0); - target_physical_state_.fill(0.0); - target_velocity_state_.fill(0.0); - target_acceleration_state_.fill(0.0); - } - - double min_angle() const { return min_angle_; } - double max_angle() const { return max_angle_; } - -private: - double min_angle_ = 15.0; - double max_angle_ = 55.0; - double velocity_limit_ = 1.0; - double acceleration_limit_ = 1.0; - double current_target_angle_ = 55.0; - std::array per_joint_targets_{55.0, 55.0, 55.0, 55.0}; - bool active_ = false; - std::array target_motor_state_{}; - std::array target_physical_state_{}; - std::array target_velocity_state_{}; - std::array target_acceleration_state_{}; - std::array requested_physical_{}; - std::array current_physical_{}; -}; - -} // namespace rmcs_core::controller::chassis From e33df491d9414cf52f9556a185a794948079de77 Mon Sep 17 00:00:00 2001 From: Yukikaze2233 Date: Sun, 10 May 2026 14:43:32 +0800 Subject: [PATCH 02/90] feat(ui): integrate pitch HUD into deformable infantry referee display - Add pitch_hud.hpp widget (pitch scale with gimbal/chassis indicators) - Register /chassis/imu/pitch and /gimbal/pitch/angle inputs - Call pitch_hud_.update() in deformable_infantry_ui update loop --- .../referee/app/ui/deformable_infantry_ui.cpp | 18 ++ .../src/referee/app/ui/widget/pitch_hud.hpp | 207 ++++++++++++++++++ 2 files changed, 225 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 269a5682..8e7a9393 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -12,6 +13,7 @@ #include "referee/app/ui/shape/shape.hpp" #include "referee/app/ui/widget/crosshair_circle.hpp" #include "referee/app/ui/widget/deformable_chassis_top_view.hpp" +#include "referee/app/ui/widget/pitch_hud.hpp" #include "referee/app/ui/widget/status_ring.hpp" namespace rmcs_core::referee::app::ui { @@ -32,6 +34,7 @@ class DeformableInfantry {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , pitch_hud_(PitchHud::Config{1540, y_center, 180, 30.0, 5.0}) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { double deformable_leg_min_angle_deg = 8.0; @@ -48,6 +51,7 @@ class DeformableInfantry register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); + register_input("/chassis/imu/pitch", chassis_pitch_, false); register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); @@ -76,6 +80,8 @@ class DeformableInfantry register_input("/remote/mouse", mouse_); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_, false); + register_input("/referee/game/stage", game_stage_); } @@ -91,6 +97,13 @@ class DeformableInfantry status_ring_.update_battery_power(*chassis_voltage_); status_ring_.update_auto_aim_enable(mouse_->right == 1); + pitch_hud_.update( + (gimbal_pitch_angle_.ready() && std::isfinite(*gimbal_pitch_angle_)) + ? *gimbal_pitch_angle_ + : std::numeric_limits::quiet_NaN(), + (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) + ? *chassis_pitch_ + : std::numeric_limits::quiet_NaN()); } private: @@ -160,6 +173,9 @@ class DeformableInfantry InputInterface mouse_; + InputInterface gimbal_pitch_angle_; + InputInterface chassis_pitch_; + InputInterface game_stage_; CrossHairCircle crosshair_circle_; @@ -171,6 +187,8 @@ class DeformableInfantry Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; + PitchHud pitch_hud_; + Integer time_reminder_; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp new file mode 100644 index 00000000..ebeea1e4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp @@ -0,0 +1,207 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { + +class PitchHud { +public: + struct Config { + uint16_t center_x = 1540; + uint16_t center_y = y_center; + uint16_t half_height_px = 180; + double half_span_deg = 30.0; + double tick_step_deg = 5.0; + }; + + PitchHud() { set_config(Config{}); } + + explicit PitchHud(Config config) { set_config(config); } + + void set_config(Config config) { + config.tick_step_deg = std::max(config.tick_step_deg, 1.0); + config.half_span_deg = std::max(config.half_span_deg, config.tick_step_deg); + config.half_height_px = std::max(config.half_height_px, 40); + config_ = config; + initialize_(); + } + + void update(double gimbal_pitch, double chassis_pitch) { + if (std::isfinite(gimbal_pitch)) + update_gimbal_pitch_indicator_(pitch_to_hud_y_(gimbal_pitch)); + else + set_indicator_visible_(gimbal_pitch_indicator_, false); + + if (std::isfinite(chassis_pitch)) + update_chassis_pitch_indicator_(pitch_to_hud_y_(chassis_pitch)); + else + set_indicator_visible_(chassis_pitch_indicator_, false); + } + +private: + static constexpr std::size_t tick_capacity_ = 25; + + void initialize_() { + tick_count_ = std::clamp( + static_cast( + std::floor(2.0 * config_.half_span_deg / config_.tick_step_deg + 1.0e-6)) + + 1, + std::size_t{1}, tick_capacity_); + + axis_.set_color(Shape::Color::YELLOW); + axis_.set_width(2); + axis_.set_x(config_.center_x); + axis_.set_y(axis_top_y_()); + axis_.set_x2(config_.center_x); + axis_.set_y2(axis_bottom_y_()); + axis_.set_visible(true); + + for (std::size_t i = 0; i < ticks_.size(); ++i) { + auto& tick = ticks_[i]; + if (i >= tick_count_) { + tick.set_visible(false); + continue; + } + + const double tick_deg = -config_.half_span_deg + static_cast(i) * config_.tick_step_deg; + const int rounded_tick_deg = static_cast(std::lround(tick_deg)); + if (rounded_tick_deg == 0) { + tick.set_visible(false); + continue; + } + + const bool is_major = (std::abs(rounded_tick_deg) % 10 == 0); + const uint16_t tick_length = is_major ? 18 : 10; + const uint16_t tick_y = pitch_to_hud_y_(deg_to_rad_(tick_deg)); + + tick.set_color(Shape::Color::YELLOW); + tick.set_width(2); + tick.set_x(config_.center_x); + tick.set_y(tick_y); + tick.set_x2(config_.center_x + tick_length); + tick.set_y2(tick_y); + tick.set_visible(true); + } + + for (auto& line : gimbal_pitch_indicator_) { + line.set_color(Shape::Color::YELLOW); + line.set_width(2); + line.set_visible(false); + } + for (auto& line : chassis_pitch_indicator_) { + line.set_color(Shape::Color::YELLOW); + line.set_width(2); + line.set_visible(false); + } + } + + uint16_t axis_top_y_() const { + return clamp_to_screen_y_( + static_cast(config_.center_y) - static_cast(config_.half_height_px)); + } + + uint16_t axis_bottom_y_() const { + return clamp_to_screen_y_( + static_cast(config_.center_y) + static_cast(config_.half_height_px)); + } + + static constexpr double deg_to_rad_(double degrees) { + return degrees * std::numbers::pi / 180.0; + } + + static uint16_t clamp_to_screen_y_(double y) { + return static_cast(std::clamp( + std::lround(y), 0l, static_cast(screen_height - 1))); + } + + uint16_t pitch_to_hud_y_(double pitch_rad) const { + const double clamped_pitch = + std::clamp(pitch_rad, -deg_to_rad_(config_.half_span_deg), + deg_to_rad_(config_.half_span_deg)); + const double normalized = clamped_pitch / deg_to_rad_(config_.half_span_deg); + return clamp_to_screen_y_( + static_cast(config_.center_y) + + normalized * static_cast(config_.half_height_px)); + } + + void update_gimbal_pitch_indicator_(uint16_t y) { + const uint16_t tip_x = config_.center_x - 8; + const uint16_t back_x = config_.center_x - 20; + constexpr uint16_t half_height = 8; + const uint16_t top_y = clamp_to_screen_y_(static_cast(y) - half_height); + const uint16_t bottom_y = clamp_to_screen_y_(static_cast(y) + half_height); + + gimbal_pitch_indicator_[0].set_x(back_x); + gimbal_pitch_indicator_[0].set_y(top_y); + gimbal_pitch_indicator_[0].set_x2(tip_x); + gimbal_pitch_indicator_[0].set_y2(y); + + gimbal_pitch_indicator_[1].set_x(back_x); + gimbal_pitch_indicator_[1].set_y(bottom_y); + gimbal_pitch_indicator_[1].set_x2(tip_x); + gimbal_pitch_indicator_[1].set_y2(y); + + gimbal_pitch_indicator_[2].set_x(back_x); + gimbal_pitch_indicator_[2].set_y(top_y); + gimbal_pitch_indicator_[2].set_x2(back_x); + gimbal_pitch_indicator_[2].set_y2(bottom_y); + + set_indicator_visible_(gimbal_pitch_indicator_, true); + } + + void update_chassis_pitch_indicator_(uint16_t y) { + const uint16_t front_x = config_.center_x - 20; + const uint16_t rear_x = config_.center_x - 34; + constexpr uint16_t front_half_height = 12; + constexpr uint16_t rear_half_height = 8; + const uint16_t front_top_y = clamp_to_screen_y_(static_cast(y) - front_half_height); + const uint16_t front_bottom_y = + clamp_to_screen_y_(static_cast(y) + front_half_height); + const uint16_t rear_top_y = clamp_to_screen_y_(static_cast(y) - rear_half_height); + const uint16_t rear_bottom_y = + clamp_to_screen_y_(static_cast(y) + rear_half_height); + + chassis_pitch_indicator_[0].set_x(rear_x); + chassis_pitch_indicator_[0].set_y(rear_top_y); + chassis_pitch_indicator_[0].set_x2(front_x); + chassis_pitch_indicator_[0].set_y2(front_top_y); + + chassis_pitch_indicator_[1].set_x(front_x); + chassis_pitch_indicator_[1].set_y(front_top_y); + chassis_pitch_indicator_[1].set_x2(front_x); + chassis_pitch_indicator_[1].set_y2(front_bottom_y); + + chassis_pitch_indicator_[2].set_x(front_x); + chassis_pitch_indicator_[2].set_y(front_bottom_y); + chassis_pitch_indicator_[2].set_x2(rear_x); + chassis_pitch_indicator_[2].set_y2(rear_bottom_y); + + chassis_pitch_indicator_[3].set_x(rear_x); + chassis_pitch_indicator_[3].set_y(rear_bottom_y); + chassis_pitch_indicator_[3].set_x2(rear_x); + chassis_pitch_indicator_[3].set_y2(rear_top_y); + + set_indicator_visible_(chassis_pitch_indicator_, true); + } + + template + static void set_indicator_visible_(std::array& indicator, bool visible) { + for (auto& line : indicator) + line.set_visible(visible); + } + + Config config_{}; + std::size_t tick_count_ = 0; + Line axis_; + std::array ticks_; + std::array gimbal_pitch_indicator_; + std::array chassis_pitch_indicator_; +}; + +} // namespace rmcs_core::referee::app::ui From b9acd1ffb10dad035ac086ff337bb8e34ce11f2c Mon Sep 17 00:00:00 2001 From: Yukikaze2233 Date: Sun, 10 May 2026 22:03:53 +0800 Subject: [PATCH 03/90] feat: add omni-b variant, unify calibration and CAN strategy across 3 chassis - Add deformable-infantry-omni-b: omni chassis + steering gimbal, no separate IMU board - Replace per-chassis calibrate topics with unified /rmcs/service/robot_status - Adopt two-frame alternating CAN transmission (even: 0x200+0x142, odd: 0x141) - Align BottomBoard constructor formatting across omni/omni-b/steering - Add --link-default to build-rmcs-cross for convenience symlinks - Add pitch HUD widget to referee UI --- .script/build-rmcs-cross | 42 +- .script/complete/_build-rmcs-cross | 1 + docs/zh-cn/cross_build.md | 28 +- .../config/deformable-infantry-omni-b.yaml | 345 ++++++++ .../config/deformable-infantry-omni.yaml | 2 +- .../config/deformable-infantry-steering.yaml | 19 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/chassis/deformable_chassis.cpp | 52 ++ .../hardware/deformable-infantry-omni-b.cpp | 777 ++++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 3 +- .../hardware/deformable-infantry-steering.cpp | 174 ++-- 11 files changed, 1330 insertions(+), 114 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp diff --git a/.script/build-rmcs-cross b/.script/build-rmcs-cross index 21c987ec..488054da 100755 --- a/.script/build-rmcs-cross +++ b/.script/build-rmcs-cross @@ -7,15 +7,17 @@ set -euo pipefail usage() { cat <<'EOF' Usage: - build-rmcs-cross --target-arch [colcon build args...] + build-rmcs-cross --target-arch [--link-default] [colcon build args...] Examples: build-rmcs-cross --target-arch arm64 + build-rmcs-cross --target-arch arm64 --link-default build-rmcs-cross --target-arch amd64 --packages-up-to rmcs_executor EOF } target_arch="" +link_default=0 colcon_args=() while (($# > 0)); do @@ -33,6 +35,10 @@ while (($# > 0)); do target_arch="${1#*=}" shift ;; + --link-default) + link_default=1 + shift + ;; -h | --help) usage exit 0 @@ -150,6 +156,30 @@ build_base="build-cross-${RMCS_TARGET_ARCH}" install_base="install-cross-${RMCS_TARGET_ARCH}" log_base="log-cross-${RMCS_TARGET_ARCH}" +check_default_linkable() { + local target="$1" + local link_name="$2" + + if [[ ! -d "${target}" ]]; then + echo "> ERROR: Cross build output not found: ${workspace}/${target}" + exit 1 + fi + + if [[ -e "${link_name}" && ! -L "${link_name}" ]]; then + echo "> ERROR: Cannot link ${workspace}/${link_name} -> ${target}." + echo "> ${workspace}/${link_name} exists and is not a symlink. Move or remove it first." + exit 1 + fi +} + +link_default_base() { + local target="$1" + local link_name="$2" + + ln -sfnT "${target}" "${link_name}" + echo "> Linked ${workspace}/${link_name} -> ${target}" +} + CLICOLOR_FORCE=1 NINJA_STATUS="" \ colcon \ --log-base "${log_base}" \ @@ -163,3 +193,13 @@ CLICOLOR_FORCE=1 NINJA_STATUS="" \ -DRMCS_TARGET_ARCH="${RMCS_TARGET_ARCH}" \ -DRMCS_SYSROOT="${RMCS_SYSROOT}" \ -DRMCS_TARGET_TRIPLET="${RMCS_TARGET_TRIPLET}" + +if ((link_default)); then + check_default_linkable "${build_base}" build + check_default_linkable "${install_base}" install + check_default_linkable "${log_base}" log + + link_default_base "${build_base}" build + link_default_base "${install_base}" install + link_default_base "${log_base}" log +fi diff --git a/.script/complete/_build-rmcs-cross b/.script/complete/_build-rmcs-cross index 18004636..34d609c2 100644 --- a/.script/complete/_build-rmcs-cross +++ b/.script/complete/_build-rmcs-cross @@ -2,4 +2,5 @@ _arguments \ '--target-arch=[Cross compile target architecture]:target:(arm64 amd64)' \ + '--link-default[Link build/install/log to cross build output directories]' \ '*:colcon build args:' diff --git a/docs/zh-cn/cross_build.md b/docs/zh-cn/cross_build.md index 226a3aa7..b197541b 100644 --- a/docs/zh-cn/cross_build.md +++ b/docs/zh-cn/cross_build.md @@ -31,13 +31,13 @@ build-rmcs-cross --target-arch arm64 ``` -适用于 `linux/amd64` 的 `latest-full` 变体。 +适用于 `linux/arm64` 的 `latest-full` 变体。 ```bash build-rmcs-cross --target-arch amd64 ``` -适用于 `linux/arm64` 的 `latest-full` 变体。 +适用于 `linux/amd64` 的 `latest-full` 变体。 例如，可追加常见 `colcon build` 参数： @@ -45,6 +45,30 @@ build-rmcs-cross --target-arch amd64 build-rmcs-cross --target-arch arm64 --packages-up-to rmcs_executor ``` +若需要让现有 `sync-remote`、`env_setup` 等继续使用默认目录，可在 cross 构建成功后自动链接默认目录： + +```bash +build-rmcs-cross --target-arch arm64 --link-default +``` + +链接方向等价于在 `rmcs_ws` 下执行： + +```bash +ln -sfn build-cross-arm64 build +ln -sfn install-cross-arm64 install +ln -sfn log-cross-arm64 log +``` + +也就是创建或更新默认目录名，让 `build`、`install`、`log` 这三个软链接分别指向对应的 `*-cross-arm64` 目录。 + +切回 native 构建时，直接运行： + +```bash +build-rmcs +``` + +`build-rmcs` 会自动识别上述 cross 默认软链接，删除软链接并恢复成普通目录。 + ## 4. 构建环境隔离约束 `build-rmcs-cross` 会显式清理并重建以下环境，避免 host/target 串用： diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml new file mode 100644 index 00000000..18d5bd50 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -0,0 +1,345 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DeformableInfantryOmniB -> deformable_infantry + + - 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::DeformableInfantry -> referee_ui_infantry + + - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_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::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller + + - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller + - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + + +deformable_infantry: + ros__parameters: + serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + left_front_zero_point: 7173 + left_back_zero_point: 5167 + right_back_zero_point: 3098 + right_front_zero_point: 6485 + yaw_motor_zero_point: 39442 + pitch_motor_zero_point: 56556 + debug_log_supercap: false + debug_log_wheel_motor: false + debug_log_deformable_joint_motor: false + +chassis_controller: + ros__parameters: + # Deploy geometry / chassis-owned joint intent + min_angle: 20.0 + max_angle: 50.0 + active_suspension_enable: true + spin_ratio: 1.0 + + # IMU attitude correction at min-angle stance. + active_suspension_pitch_kp: 8.0 + active_suspension_pitch_ki: 0.35 + active_suspension_pitch_kd: 0.28 + + active_suspension_roll_kp: 8.0 + active_suspension_roll_ki: 0.35 + active_suspension_roll_kd: 0.28 + + active_suspension_pitch_angle_diff_limit_deg: 45.0 + active_suspension_roll_angle_diff_limit_deg: 45.0 + active_suspension_pid_integral_limit_deg: 20.0 + + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 + + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 + +gimbal_controller: + ros__parameters: + inertia: 1.0 # kg·m² + friction: 1.65 # Nm/(rad/s) + + upper_limit: -0.61 # -35 deg + lower_limit: 0.05 # 6 deg + use_encoder_pitch: true + + yaw_angle_kp: 10.0 + yaw_angle_ki: 0.0 + yaw_angle_kd: 0.0 + + yaw_velocity_kp: 8.0 + yaw_velocity_ki: 0.0 + yaw_velocity_kd: 0.0 + + pitch_angle_kp: 40.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + + pitch_velocity_kp: 3.0 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + pitch_torque_control: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 600.0 + - 600.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 30.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + 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.00 + 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.00 + 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.4 + ki: 0.0 + kd: 0.0 + +deformable_chassis_controller: + ros__parameters: + mass: 23.0 + moment_of_inertia: 1.0 + chassis_radius: 0.2341741 + rod_length: 0.150 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + +lf_joint_controller: + ros__parameters: + # Joint-local servo inputs produced by chassis intent generation + measurement_angle: /chassis/left_front_joint/physical_angle + measurement_velocity: /chassis/left_front_joint/physical_velocity + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + mode_input: /chassis/left_front_joint/suspension_mode + suspension_torque: /chassis/left_front_joint/suspension_torque + control: /chassis/left_front_joint/control_torque + + # Normal ADRC servo mode + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + + # Suspension ADRC servo mode + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + + # Joint-local feedforward / limit shaping + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +lb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/left_back_joint/physical_angle + measurement_velocity: /chassis/left_back_joint/physical_velocity + setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity + mode_input: /chassis/left_back_joint/suspension_mode + suspension_torque: /chassis/left_back_joint/suspension_torque + control: /chassis/left_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rb_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_back_joint/physical_angle + measurement_velocity: /chassis/right_back_joint/physical_velocity + setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity + mode_input: /chassis/right_back_joint/suspension_mode + suspension_torque: /chassis/right_back_joint/suspension_torque + control: /chassis/right_back_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 + +rf_joint_controller: + ros__parameters: + # Same joint-servo layout as lf_joint_controller + measurement_angle: /chassis/right_front_joint/physical_angle + measurement_velocity: /chassis/right_front_joint/physical_velocity + setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity + mode_input: /chassis/right_front_joint/suspension_mode + suspension_torque: /chassis/right_front_joint/suspension_torque + control: /chassis/right_front_joint/control_torque + dt: 0.001 + b0: -1.0 + kt: 1.0 + td_h: 0.001 + td_r: 50.0 + eso_w0: 250.0 + eso_auto_beta: true + k1: 30.0 + k2: 17.0 + alpha1: 0.75 + alpha2: 0.7 + delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index ac97a8e3..dd76a55d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -9,7 +9,7 @@ rmcs_executor: - 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::referee::app::ui::DeformableInfantry -> referee_ui_infantry - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_controller diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index b1ec953d..761d2265 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -34,22 +34,9 @@ rmcs_executor: value_broadcaster: ros__parameters: forward_list: - - /chassis/left_front_joint/torque - - /chassis/left_back_joint/torque - - /chassis/right_front_joint/torque - - /chassis/right_back_joint/torque - - /chassis/left_front_joint/suspension_mode - - /chassis/left_back_joint/suspension_mode - - /chassis/right_front_joint/suspension_mode - - /chassis/right_back_joint/suspension_mode - - /chassis/left_front_joint/suspension_torque - - /chassis/left_back_joint/suspension_torque - - /chassis/right_front_joint/suspension_torque - - /chassis/right_back_joint/suspension_torque - - /chassis/left_front_joint/control_torque - - /chassis/left_back_joint/control_torque - - /chassis/right_front_joint/control_torque - - /chassis/right_back_joint/control_torque + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + deformable_infantry: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f314d28f..1438f8a8 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -6,6 +6,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 0d3b36ca..96828c2c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -255,6 +255,27 @@ class DeformableChassis right_front_joint_target_physical_acceleration_, nan_); register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, nan_); + register_output( + "/chassis/left_front_joint/suspension_mode", left_front_joint_suspension_mode_, false); + register_output( + "/chassis/left_back_joint/suspension_mode", left_back_joint_suspension_mode_, false); + register_output( + "/chassis/right_front_joint/suspension_mode", right_front_joint_suspension_mode_, false); + register_output( + "/chassis/right_back_joint/suspension_mode", right_back_joint_suspension_mode_, false); + + register_output( + "/chassis/left_front_joint/suspension_torque", left_front_joint_suspension_torque_, + nan_); + register_output( + "/chassis/left_back_joint/suspension_torque", left_back_joint_suspension_torque_, nan_); + register_output( + "/chassis/right_back_joint/suspension_torque", right_back_joint_suspension_torque_, + nan_); + register_output( + "/chassis/right_front_joint/suspension_torque", right_front_joint_suspension_torque_, + nan_); + *mode_ = rmcs_msgs::ChassisMode::AUTO; chassis_control_velocity_->vector << nan_, nan_, nan_; @@ -381,6 +402,18 @@ class DeformableChassis return wrap_deg(joint_offset) - wrap_deg(*joint_encoder_angle) + legacy_fixed_compensation; } + void clear_suspension_output_interfaces_() { + *left_front_joint_suspension_mode_ = false; + *left_back_joint_suspension_mode_ = false; + *right_back_joint_suspension_mode_ = false; + *right_front_joint_suspension_mode_ = false; + + *left_front_joint_suspension_torque_ = 0.0; + *left_back_joint_suspension_torque_ = 0.0; + *right_back_joint_suspension_torque_ = 0.0; + *right_front_joint_suspension_torque_ = 0.0; + } + void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { @@ -580,6 +613,7 @@ class DeformableChassis } void update_active_suspension_(const JointFeedbackFrame&) { + clear_suspension_output_interfaces_(); if (!suspension_requested_by_input_()) { reset_attitude_correction_state_(); return; @@ -672,6 +706,8 @@ class DeformableChassis *right_front_joint_target_physical_acceleration_ = nan_; *processed_encoder_angle_ = nan_; + + clear_suspension_output_interfaces_(); } void update_velocity_control() { @@ -814,6 +850,12 @@ class DeformableChassis const bool prone_override = refresh_requested_joint_targets_from_deploy_state_(); scope_motor_control(prone_override); update_active_suspension_(joint_feedback); + + *left_front_joint_suspension_mode_ = joint_suspension_active_[kLeftFront]; + *left_back_joint_suspension_mode_ = joint_suspension_active_[kLeftBack]; + *right_front_joint_suspension_mode_ = joint_suspension_active_[kRightFront]; + *right_back_joint_suspension_mode_ = joint_suspension_active_[kRightBack]; + update_joint_target_trajectory(); publish_joint_target_angles(joint_feedback.physical_angles); } @@ -1014,6 +1056,7 @@ class DeformableChassis *rb_angle_error_ = nan_; *rf_angle_error_ = nan_; + clear_suspension_output_interfaces_(); } private: @@ -1097,6 +1140,15 @@ class DeformableChassis OutputInterface processed_encoder_angle_; + OutputInterface left_front_joint_suspension_mode_; + OutputInterface left_back_joint_suspension_mode_; + OutputInterface right_front_joint_suspension_mode_; + OutputInterface right_back_joint_suspension_mode_; + OutputInterface left_front_joint_suspension_torque_; + OutputInterface left_back_joint_suspension_torque_; + OutputInterface right_front_joint_suspension_torque_; + OutputInterface right_back_joint_suspension_torque_; + double min_angle_; double max_angle_; double left_front_joint_offset_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp new file mode 100644 index 00000000..f07b1de3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -0,0 +1,777 @@ +#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/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class DeformableInfantryOmniB + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableInfantryOmniB() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , deformable_infantry_command_( + create_partner_component( + get_component_name() + "_command", *this)) { + using namespace rmcs_description; + + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + + tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); + }); + + rmcs_board_lite = std::make_unique( + *this, *deformable_infantry_command_, + get_parameter("serial_filter_rmcs_board").as_string()); + top_board_ = std::make_unique( + *this, *deformable_infantry_command_, + get_parameter("serial_filter_top_board").as_string()); + } + + ~DeformableInfantryOmniB() override = default; + + void before_updating() override { + top_board_->request_hard_sync_read(); + next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); + } + + void update() override { + rmcs_board_lite->update(); + top_board_->update(); + } + + void command_update() { + const bool even = ((cmd_tick_++ & 1u) == 0u); + rmcs_board_lite->command_update(even); + top_board_->command_update(); + } + +private: + class DeformableInfantryOmniBCommand; + class BottomBoard; + class TopBoard; + + class DeformableInfantryOmniBCommand : public rmcs_executor::Component { + public: + explicit DeformableInfantryOmniBCommand(DeformableInfantryOmniB& deformableInfantry) + : deformableInfantry(deformableInfantry) {} + + void update() override { deformableInfantry.command_update(); } + + DeformableInfantryOmniB& deformableInfantry; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class DeformableInfantryOmniB; + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + explicit BottomBoard( + DeformableInfantryOmniB& deformableInfantry, + DeformableInfantryOmniBCommand& deformableInfantry_command, + const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , deformable_infantry_{deformableInfantry} + , command_{deformableInfantry_command} + , tf_{deformableInfantry.tf_} { + + deformableInfantry.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast( + deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(13.0) + .enable_multi_turn_angle() + .set_reversed()); + + // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point + for (auto& motor : chassis_joint_motors_) + motor.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} + .set_reversed() + .enable_multi_turn_angle()); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + deformableInfantry.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); + deformableInfantry.register_output( + "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); + deformableInfantry.register_output( + "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, + nan_); + deformableInfantry.register_output( + "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, + nan_); + deformableInfantry.register_output( + "/chassis/right_front_joint/physical_velocity", + right_front_joint_physical_velocity_, nan_); + deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); + deformableInfantry.register_output( + "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); + deformableInfantry.register_output("/chassis/radius", radius_, nan_); + + deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + deformableInfantry.get_parameter_or( + "debug_log_wheel_motor", debug_log_wheel_motor_, false); + deformableInfantry.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); + } + + ~BottomBoard() override = default; + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_joint_motors_) + motor.update_status(); + + update_joint_physical_feedback_( + 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); + update_joint_physical_feedback_( + 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); + update_joint_physical_feedback_( + 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); + update_joint_physical_feedback_( + 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + + update_geometry_feedback_(); + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); + + dr16_.update_status(); + gimbal_yaw_motor_.update_status(); + if (supercap_status_received_.load(std::memory_order_relaxed)) + supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); + gimbal_bullet_feeder_.update_status(); + + tf_->set_state( + gimbal_yaw_motor_.angle()); + } + + void command_update(bool even) { + auto builder = start_transmit(); + if (even) { + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[2].generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[3].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x142, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + }); + } else { + builder.can0_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), + }); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), + }); + builder.can3_transmit({ + .can_id = 0x141, + .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), + }); + } + } + + private: + DeformableInfantryOmniB& deformable_infantry_; + rmcs_executor::Component& command_; + + static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; + + static double to_physical_angle_(double motor_angle) { + return joint_zero_physical_angle_rad_ - motor_angle; + } + + static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } + + void update_joint_physical_feedback_( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output) { + if (!joint_status_received_[index].load(std::memory_order_relaxed)) { + *angle_output = nan_; + *velocity_output = nan_; + return; + } + + *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); + *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); + } + + void update_geometry_feedback_() { + const Eigen::Vector4d alpha_rad{ + *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, + *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + const Eigen::Vector4d alpha_dot_rad{ + *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, + *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + + if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { + *encoder_alpha_ = nan_; + *encoder_alpha_dot_ = nan_; + *radius_ = nan_; + return; + } + + *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_dot_ = alpha_dot_rad.mean(); + *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); + } + + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%c %c %c %c]", + chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), + chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), + wheel_rx(1), wheel_rx(2), wheel_rx(3)); + } + + if (debug_log_deformable_joint_motor_) { + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%c %c %c %c]", + *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, + *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, + *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, + *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, + joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', + supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : nan_, + supercap_rx ? supercap_.chassis_voltage() : nan_, + supercap_rx ? supercap_.chassis_power() : nan_, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + wheel_status_received_[0].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[0].store_status(data.can_data); + joint_status_received_[0].store(true, std::memory_order_relaxed); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[1].store_status(data.can_data); + wheel_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[1].store_status(data.can_data); + joint_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); + supercap_.store_status(data.can_data); + supercap_status_received_.store(true, std::memory_order_relaxed); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[2].store_status(data.can_data); + wheel_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[2].store_status(data.can_data); + joint_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x142) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + if (data.can_id == 0x201) { + chassis_wheel_motors_[3].store_status(data.can_data); + wheel_status_received_[3].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { + chassis_joint_motors_[3].store_status(data.can_data); + joint_status_received_[3].store(true, std::memory_order_relaxed); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{ + deformable_infantry_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{deformable_infantry_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{ + deformable_infantry_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{ + deformable_infantry_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{ + deformable_infantry_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{ + deformable_infantry_, command_, "/chassis/right_front_wheel"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{ + deformable_infantry_, command_, "/chassis/left_front_joint"}, + device::LkMotor{ + deformable_infantry_, command_, "/chassis/left_back_joint"}, + device::LkMotor{ + deformable_infantry_, command_, "/chassis/right_back_joint"}, + device::LkMotor{ + deformable_infantry_, command_, "/chassis/right_front_joint"}, + }; + + std::atomic wheel_status_received_[4] = {false, false, false, false}; + std::atomic joint_status_received_[4] = {false, false, false, false}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_{ + Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{ + Clock::now() + std::chrono::seconds(1)}; + device::Supercap supercap_{deformable_infantry_, command_}; + std::atomic latest_supercap_status_{ + device::CanPacket8{uint64_t{0}}}; + std::atomic supercap_status_received_{false}; + device::DjiMotor gimbal_bullet_feeder_{ + deformable_infantry_, command_, "/gimbal/bullet_feeder"}; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; + OutputInterface left_front_joint_physical_angle_; + OutputInterface left_back_joint_physical_angle_; + OutputInterface right_back_joint_physical_angle_; + OutputInterface right_front_joint_physical_angle_; + OutputInterface left_front_joint_physical_velocity_; + OutputInterface left_back_joint_physical_velocity_; + OutputInterface right_back_joint_physical_velocity_; + OutputInterface right_front_joint_physical_velocity_; + OutputInterface encoder_alpha_; + OutputInterface encoder_alpha_dot_; + OutputInterface radius_; + }; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class DeformableInfantryOmniB; + + explicit TopBoard( + DeformableInfantryOmniB& deformableInfantry, + DeformableInfantryOmniBCommand& deformableInfantry_command, + std::string serial_filter = {}) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , hard_sync_pending_(deformableInfantry.hard_sync_pending_) + , tf_(deformableInfantry.tf_) + , bmi088_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") + , gimbal_left_friction_( + deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") + , gimbal_right_friction_( + deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") + , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { + + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} + .set_reversed() + .set_encoder_zero_point( + static_cast( + deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + + scope_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + deformableInfantry.register_output( + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + + bmi088_.set_coordinate_mapping([](double x, double y, double z) { + // Top board BMI088 maps to gimbal frame as (-x, -y, z). + return std::make_tuple(y, -x, z); + }); + } + + ~TopBoard() override = default; + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + + void update() { + bmi088_.update_status(); + + gimbal_pitch_motor_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + scope_motor_.update_status(); + + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> + // OdomImu, so use the encoder pitch from the TF tree to move the + // yaw-link pose back into PitchLink. + tf_->set_transform( + pitch_link_to_odom_imu); + + tf_->set_state( + pitch_encoder_angle); + } + + void command_update() { + auto builder = start_transmit(); + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} + + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x201) + gimbal_left_friction_.store_status(data.can_data); + else if (data.can_id == 0x202) + gimbal_right_friction_.store_status(data.can_data); + else if (data.can_id == 0x203) + scope_motor_.store_status(data.can_data); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + std::atomic& hard_sync_pending_; + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_encoder_; + + device::Bmi088 bmi088_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor scope_motor_; + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + text("- left front: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); + text("- left back: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); + text("- right back: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); + text("- right front: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); + + response->message = feedback_message.str(); + } + + OutputInterface tf_; + InputInterface timestamp_; + std::atomic hard_sync_pending_{false}; + size_t hard_sync_snapshot_count_ = 0; + Clock::time_point next_hard_sync_log_time_{}; + + std::shared_ptr deformable_infantry_command_; + std::unique_ptr rmcs_board_lite; + std::unique_ptr top_board_; + + std::shared_ptr> status_service_; + uint32_t cmd_tick_ = 0; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryOmniB, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 3b656dd6..6a80eff4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -468,7 +468,8 @@ class DeformableInfantryOmni status_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', + supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : kNaN, supercap_rx ? supercap_.chassis_voltage() : kNaN, supercap_rx ? supercap_.chassis_power() : kNaN, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index 6514c58b..5a833272 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -52,19 +53,12 @@ class DeformableInfantryV2 tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - joints_calibrate_subscription_ = create_subscription( - "/joints/calibrate", rclcpp::QoS(1), [this](std_msgs::msg::Int32::UniquePtr msg) { - joints_calibrate_subscription_callback(std::move(msg)); - }); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); + // For command: remote-status + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( + "/rmcs/service/robot_status", + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { + status_service_callback(response); }); rmcs_board_lite = std::make_unique( @@ -98,54 +92,6 @@ class DeformableInfantryV2 class BottomBoard; class TopBoard; - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %d", - rmcs_board_lite->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %d", - rmcs_board_lite->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %d", - rmcs_board_lite->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %d", - rmcs_board_lite->chassis_steer_motors_[3].calibrate_zero_point()); - } - - void joints_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite) - return; - - RCLCPP_INFO( - get_logger(), "New left front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New left back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right back offset: %ld", - rmcs_board_lite->chassis_joint_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "New right front offset: %ld", - rmcs_board_lite->chassis_joint_motors_[3].calibrate_zero_point()); - } - - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_lite || !top_board_) - return; - - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - rmcs_board_lite->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - class DeformableInfantryV2Command : public rmcs_executor::Component { public: explicit DeformableInfantryV2Command(DeformableInfantryV2& deformableInfantry) @@ -164,23 +110,14 @@ class DeformableInfantryV2 explicit BottomBoard( DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::RmcsBoardLite( + DeformableInfantryV2Command& deformableInfantry_command, + const std::string& serial_filter = {}) + : RmcsBoardLite{ serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , deformable_infantry_(deformableInfantry) - , tf_(deformableInfantry.tf_) - , imu_(1000, 0.2, 0.0) - , gimbal_yaw_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/yaw") - , dr16_(deformableInfantry) - , chassis_wheel_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_wheel"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_wheel"},} - , chassis_steer_motors_{device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_steering"}, device::DjiMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_steering"}} - , chassis_joint_motors_{device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_front_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/left_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_back_joint"}, device::LkMotor{deformableInfantry, deformableInfantry_command, "/chassis/right_front_joint"}} - , next_chassis_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , next_supercap_feedback_log_time_(Clock::now() + std::chrono::seconds(1)) - , supercap_(deformableInfantry, deformableInfantry_command) - , gimbal_bullet_feeder_( - deformableInfantry, deformableInfantry_command, "/gimbal/bullet_feeder") { + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , deformable_infantry_{deformableInfantry} + , command_{deformableInfantry_command} + , tf_{deformableInfantry.tf_} { deformableInfantry.register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -568,7 +505,8 @@ class DeformableInfantryV2 deformable_infantry_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', + supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : nan_, supercap_rx ? supercap_.chassis_voltage() : nan_, supercap_rx ? supercap_.chassis_power() : nan_, @@ -666,25 +604,48 @@ class DeformableInfantryV2 imu_.store_gyroscope_status(data.x, data.y, data.z); } + rmcs_executor::Component& command_; + OutputInterface& tf_; - device::Bmi088 imu_; - device::LkMotor gimbal_yaw_motor_; - device::Dr16 dr16_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::LkMotor chassis_joint_motors_[4]; + device::Bmi088 imu_{1000, 0.2, 0.0}; + device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; + device::Dr16 dr16_{deformable_infantry_}; + + device::DjiMotor chassis_wheel_motors_[4]{ + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, + }; + device::DjiMotor chassis_steer_motors_[4]{ + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_steering"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_steering"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_steering"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_steering"}, + }; + device::LkMotor chassis_joint_motors_[4]{ + device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, + }; + std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; bool debug_log_supercap_ = false; bool debug_log_wheel_motor_ = false; bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_; - Clock::time_point next_supercap_feedback_log_time_; - device::Supercap supercap_; - std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; + Clock::time_point next_chassis_feedback_log_time_{ + Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{ + Clock::now() + std::chrono::seconds(1)}; + device::Supercap supercap_{deformable_infantry_, command_}; + std::atomic latest_supercap_status_{ + device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_; + device::DjiMotor gimbal_bullet_feeder_{ + deformable_infantry_, command_, "/gimbal/bullet_feeder"}; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; @@ -853,6 +814,36 @@ class DeformableInfantryV2 device::DjiMotor scope_motor_; }; + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + text("- left front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[0].last_raw_angle()); + text("- left back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[1].last_raw_angle()); + text("- right back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[2].last_raw_angle()); + text("- right front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[3].last_raw_angle()); + text("- left front steer: {}", rmcs_board_lite->chassis_steer_motors_[0].last_raw_angle()); + text("- left back steer: {}", rmcs_board_lite->chassis_steer_motors_[1].last_raw_angle()); + text("- right back steer: {}", rmcs_board_lite->chassis_steer_motors_[2].last_raw_angle()); + text("- right front steer: {}", rmcs_board_lite->chassis_steer_motors_[3].last_raw_angle()); + text("- left front joint: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); + text("- left back joint: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); + text("- right back joint: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); + text("- right front joint: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); + + response->message = feedback_message.str(); + } + OutputInterface tf_; InputInterface timestamp_; std::atomic hard_sync_pending_{false}; @@ -863,10 +854,7 @@ class DeformableInfantryV2 std::unique_ptr rmcs_board_lite; std::unique_ptr top_board_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; - rclcpp::Subscription::SharedPtr joints_calibrate_subscription_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - + std::shared_ptr> status_service_; uint32_t cmd_tick_ = 0; }; From ddf8478f620a6cc9364a8c3a7c9763a89736557b Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 11 May 2026 06:04:02 +0800 Subject: [PATCH 04/90] feat: add suspension toggle functionality and adjust leg radius parameters --- .../controller/chassis/deformable_chassis.cpp | 19 ++++++++++++++----- .../ui/widget/deformable_chassis_top_view.hpp | 4 ++-- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 96828c2c..7c5ca742 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -349,6 +349,7 @@ class DeformableChassis update_mode_from_inputs_(switch_left, switch_right, keyboard); update_velocity_control(); update_lift_target_toggle(keyboard); + update_suspension_toggle_from_inputs_(switch_left, switch_right); run_joint_intent_pipeline_(); } while (false); @@ -459,14 +460,21 @@ class DeformableChassis bool prone_override_requested_() const { return keyboard_.ready() && keyboard_->ctrl; } - bool suspension_requested_by_switch_() const { - return switch_left_.ready() && switch_right_.ready() - && *switch_left_ == rmcs_msgs::Switch::DOWN && *switch_right_ == rmcs_msgs::Switch::UP; + bool suspension_toggle_requested_by_switch_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { + return switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; + } + + void update_suspension_toggle_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) { + suspension_on = !suspension_on; + } } bool suspension_requested_by_input_() const { - return active_suspension_enable_ - && (prone_override_requested_() || suspension_requested_by_switch_()); + return active_suspension_enable_ && (prone_override_requested_() || suspension_on); } bool symmetric_joint_target_requested_() const { @@ -1175,6 +1183,7 @@ class DeformableChassis double target_physical_velocity_limit_; double target_physical_acceleration_limit_; bool active_suspension_enable_; + bool suspension_on = false; double pitch_kp_; double pitch_ki_; double pitch_kd_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index e5ea754b..72c81891 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -50,9 +50,9 @@ class DeformableChassisLegArcs { static constexpr uint16_t center_y_ = 1080 / 2; static constexpr uint16_t front_leg_radius_near_ = 102; - static constexpr uint16_t rear_leg_radius_near_ = 112; + static constexpr uint16_t rear_leg_radius_near_ = 102; static constexpr uint16_t front_leg_radius_far_ = 132; - static constexpr uint16_t rear_leg_radius_far_ = 142; + static constexpr uint16_t rear_leg_radius_far_ = 132; static constexpr uint16_t prone_leg_width_ = 6; static constexpr uint16_t upright_leg_width_ = 16; From 78c6d39a33fcf6772975768f6ed40186ff605c9c Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 11 May 2026 21:30:40 +0800 Subject: [PATCH 05/90] feat: rename prone override method and update suspension toggle logic --- .../src/controller/chassis/deformable_chassis.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 7c5ca742..f4cf3174 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -458,7 +458,7 @@ class DeformableChassis return joint_feedback; } - bool prone_override_requested_() const { return keyboard_.ready() && keyboard_->ctrl; } + bool prone_override_requested_by_keyboard() const { return keyboard_.ready() && keyboard_->ctrl; } bool suspension_toggle_requested_by_switch_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { @@ -469,12 +469,12 @@ class DeformableChassis void update_suspension_toggle_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) { - suspension_on = !suspension_on; + suspension_on_by_switch = !suspension_on_by_switch; } } bool suspension_requested_by_input_() const { - return active_suspension_enable_ && (prone_override_requested_() || suspension_on); + return active_suspension_enable_ && (prone_override_requested_by_keyboard() || suspension_on_by_switch); } bool symmetric_joint_target_requested_() const { @@ -490,7 +490,7 @@ class DeformableChassis requested_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); requested_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - const bool prone_override = prone_override_requested_(); + const bool prone_override = prone_override_requested_by_keyboard(); if (suspension_requested_by_input_()) { requested_target_physical_angles_rad_.fill(deg_to_rad(min_angle_)); } @@ -687,6 +687,7 @@ class DeformableChassis rb_current_target_angle_ = current_target_angle_; rf_current_target_angle_ = current_target_angle_; joint_target_active_ = false; + suspension_on_by_switch = false; *scope_motor_control_torque = nan_; @@ -1183,7 +1184,7 @@ class DeformableChassis double target_physical_velocity_limit_; double target_physical_acceleration_limit_; bool active_suspension_enable_; - bool suspension_on = false; + bool suspension_on_by_switch = false; double pitch_kp_; double pitch_ki_; double pitch_kd_; From 32bb48bda4a49dc4bb9539b178648bbc70b13440 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 12 May 2026 07:10:48 +0800 Subject: [PATCH 06/90] feat: remove suspension mode and torque parameters from joint controllers and add auto aim for deformable infantry --- .../config/deformable-infantry-omni-b.yaml | 15 ------ .../config/deformable-infantry-omni.yaml | 17 +------ .../config/deformable-infantry-steering.yaml | 15 ------ .../src/rmcs_bringup/launch/rmcs.launch.py | 13 ++++- rmcs_ws/src/rmcs_bringup/package.xml | 1 + .../controller/chassis/deformable_chassis.cpp | 50 ------------------- .../bullet_feeder_controller_17mm.cpp | 9 ++-- 7 files changed, 19 insertions(+), 101 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 18d5bd50..840fd485 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -181,8 +181,6 @@ lf_joint_controller: measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - mode_input: /chassis/left_front_joint/suspension_mode - suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque # Normal ADRC servo mode @@ -219,8 +217,6 @@ lf_joint_controller: # Joint-local feedforward / limit shaping torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -228,8 +224,6 @@ lb_joint_controller: measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - mode_input: /chassis/left_back_joint/suspension_mode - suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -260,8 +254,6 @@ lb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -269,8 +261,6 @@ rb_joint_controller: measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - mode_input: /chassis/right_back_joint/suspension_mode - suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -301,8 +291,6 @@ rb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -310,8 +298,6 @@ rf_joint_controller: measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - mode_input: /chassis/right_front_joint/suspension_mode - suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 @@ -342,4 +328,3 @@ rf_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index dd76a55d..6a871259 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,7 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - # - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimComponent -> auto_aim_component value_broadcaster: ros__parameters: @@ -200,8 +200,6 @@ lf_joint_controller: measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - mode_input: /chassis/left_front_joint/suspension_mode - suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque # Normal ADRC servo mode @@ -238,8 +236,6 @@ lf_joint_controller: # Joint-local feedforward / limit shaping torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -247,8 +243,6 @@ lb_joint_controller: measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - mode_input: /chassis/left_back_joint/suspension_mode - suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -0.60 @@ -279,8 +273,6 @@ lb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -288,8 +280,6 @@ rb_joint_controller: measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - mode_input: /chassis/right_back_joint/suspension_mode - suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -0.60 @@ -320,8 +310,6 @@ rb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -329,8 +317,6 @@ rf_joint_controller: measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - mode_input: /chassis/right_front_joint/suspension_mode - suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -0.60 @@ -361,4 +347,3 @@ rf_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 761d2265..51a8fc1d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -181,8 +181,6 @@ lf_joint_controller: measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - mode_input: /chassis/left_front_joint/suspension_mode - suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque # Normal ADRC servo mode @@ -219,8 +217,6 @@ lf_joint_controller: # Joint-local feedforward / limit shaping torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -228,8 +224,6 @@ lb_joint_controller: measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - mode_input: /chassis/left_back_joint/suspension_mode - suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -260,8 +254,6 @@ lb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -269,8 +261,6 @@ rb_joint_controller: measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - mode_input: /chassis/right_back_joint/suspension_mode - suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -301,8 +291,6 @@ rb_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 - rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller @@ -310,8 +298,6 @@ rf_joint_controller: measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - mode_input: /chassis/right_front_joint/suspension_mode - suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 @@ -342,4 +328,3 @@ rf_joint_controller: suspension_output_min: -35.0 suspension_output_max: 35.0 torque_feedforward_gain: 0.0 - suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 976cb9e7..d810a08d 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -6,7 +6,8 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import LogInfo +from launch.actions import IncludeLaunchDescription,LogInfo +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -52,6 +53,16 @@ def visit( if is_automatic: pass + + entities.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' + ]) + ) + ) + + return entities diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index c64c661f..9c8cc981 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,6 +11,7 @@ ament_cmake joint_state_broadcaster + rmcs_auto_aim_v2 robot_state_publisher rviz2 xacro diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index f4cf3174..10aabcfa 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -255,27 +255,6 @@ class DeformableChassis right_front_joint_target_physical_acceleration_, nan_); register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, nan_); - register_output( - "/chassis/left_front_joint/suspension_mode", left_front_joint_suspension_mode_, false); - register_output( - "/chassis/left_back_joint/suspension_mode", left_back_joint_suspension_mode_, false); - register_output( - "/chassis/right_front_joint/suspension_mode", right_front_joint_suspension_mode_, false); - register_output( - "/chassis/right_back_joint/suspension_mode", right_back_joint_suspension_mode_, false); - - register_output( - "/chassis/left_front_joint/suspension_torque", left_front_joint_suspension_torque_, - nan_); - register_output( - "/chassis/left_back_joint/suspension_torque", left_back_joint_suspension_torque_, nan_); - register_output( - "/chassis/right_back_joint/suspension_torque", right_back_joint_suspension_torque_, - nan_); - register_output( - "/chassis/right_front_joint/suspension_torque", right_front_joint_suspension_torque_, - nan_); - *mode_ = rmcs_msgs::ChassisMode::AUTO; chassis_control_velocity_->vector << nan_, nan_, nan_; @@ -403,18 +382,6 @@ class DeformableChassis return wrap_deg(joint_offset) - wrap_deg(*joint_encoder_angle) + legacy_fixed_compensation; } - void clear_suspension_output_interfaces_() { - *left_front_joint_suspension_mode_ = false; - *left_back_joint_suspension_mode_ = false; - *right_back_joint_suspension_mode_ = false; - *right_front_joint_suspension_mode_ = false; - - *left_front_joint_suspension_torque_ = 0.0; - *left_back_joint_suspension_torque_ = 0.0; - *right_back_joint_suspension_torque_ = 0.0; - *right_front_joint_suspension_torque_ = 0.0; - } - void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { @@ -621,7 +588,6 @@ class DeformableChassis } void update_active_suspension_(const JointFeedbackFrame&) { - clear_suspension_output_interfaces_(); if (!suspension_requested_by_input_()) { reset_attitude_correction_state_(); return; @@ -716,7 +682,6 @@ class DeformableChassis *processed_encoder_angle_ = nan_; - clear_suspension_output_interfaces_(); } void update_velocity_control() { @@ -860,11 +825,6 @@ class DeformableChassis scope_motor_control(prone_override); update_active_suspension_(joint_feedback); - *left_front_joint_suspension_mode_ = joint_suspension_active_[kLeftFront]; - *left_back_joint_suspension_mode_ = joint_suspension_active_[kLeftBack]; - *right_front_joint_suspension_mode_ = joint_suspension_active_[kRightFront]; - *right_back_joint_suspension_mode_ = joint_suspension_active_[kRightBack]; - update_joint_target_trajectory(); publish_joint_target_angles(joint_feedback.physical_angles); } @@ -1065,7 +1025,6 @@ class DeformableChassis *rb_angle_error_ = nan_; *rf_angle_error_ = nan_; - clear_suspension_output_interfaces_(); } private: @@ -1149,15 +1108,6 @@ class DeformableChassis OutputInterface processed_encoder_angle_; - OutputInterface left_front_joint_suspension_mode_; - OutputInterface left_back_joint_suspension_mode_; - OutputInterface right_front_joint_suspension_mode_; - OutputInterface right_back_joint_suspension_mode_; - OutputInterface left_front_joint_suspension_torque_; - OutputInterface left_back_joint_suspension_torque_; - OutputInterface right_front_joint_suspension_torque_; - OutputInterface right_back_joint_suspension_torque_; - double min_angle_; double max_angle_; double left_front_joint_offset_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 0c97c98a..556762af 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -53,7 +53,7 @@ class BulletFeederController17mm register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + register_input("/auto_aim/should_shoot", fire_control_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -102,8 +102,9 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - bool triggered = mouse.left || switch_left == Switch::DOWN - || (switch_right == Switch::UP && *fire_control_); + const bool auto_aim_enabled = mouse.right || switch_right == Switch::UP; + bool triggered = (mouse.left && !mouse.right) || switch_left == Switch::DOWN + || (mouse.left && auto_aim_enabled && *fire_control_); bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { @@ -232,4 +233,4 @@ class BulletFeederController17mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) From 0103ef7a0fdb3403d6741e987acb769fe48f3c85 Mon Sep 17 00:00:00 2001 From: eye-on Date: Tue, 12 May 2026 01:09:00 +0800 Subject: [PATCH 07/90] feat: fix the correct supercap boost mode --- .../src/controller/chassis/chassis_power_controller.cpp | 8 ++++++-- rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp | 5 ++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2d084f8b..2d9fa58b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -34,6 +34,7 @@ class ChassisPowerController register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_output("/chassis/supercap/control_enable", supercap_control_enabled_, false); register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); @@ -64,6 +65,7 @@ class ChassisPowerController update_virtual_buffer_energy(); boost_mode_ = keyboard.shift || rotary_knob < -0.9; + *supercap_control_enabled_ = boost_mode_; update_control_power_limit(); } @@ -93,6 +95,7 @@ class ChassisPowerController void reset_power_control() { virtual_buffer_energy_ = virtual_buffer_energy_limit_; boost_mode_ = false; + *supercap_control_enabled_ = false; *chassis_control_power_limit_ = 0.0; } @@ -107,7 +110,7 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - if (boost_mode_ && *supercap_enabled_) + if (*supercap_control_enabled_ && *supercap_enabled_) power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP ? inf_ : *chassis_power_limit_referee_ + 80.0; @@ -161,6 +164,7 @@ class ChassisPowerController InputInterface chassis_buffer_energy_referee_; bool boost_mode_ = false; + OutputInterface supercap_control_enabled_; OutputInterface supercap_charge_power_limit_; double chassis_power_limit_expected_; OutputInterface chassis_control_power_limit_; @@ -179,4 +183,4 @@ class ChassisPowerController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ChassisPowerController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::chassis::ChassisPowerController, rmcs_executor::Component) 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 8ac884f0..e88fc018 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp @@ -28,6 +28,8 @@ class Supercap { status_component.register_output("/chassis/supercap/voltage", supercap_voltage_, 0.0); status_component.register_output("/chassis/supercap/enabled", supercap_enabled_, false); + command_component.register_input( + "/chassis/supercap/control_enable", supercap_control_enabled_); command_component.register_input("/referee/chassis/output_status", chassis_output_status_); command_component.register_input( "/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); @@ -52,7 +54,7 @@ class Supercap { CanPacket8::Quarter generate_command() const { SupercapCommand command; - command.enabled = *chassis_output_status_; + command.enabled = *chassis_output_status_ && *supercap_control_enabled_; const double power_limit = *supercap_charge_power_limit_; if (std::isnan(power_limit)) @@ -111,6 +113,7 @@ class Supercap { Component::OutputInterface supercap_voltage_; Component::OutputInterface supercap_enabled_; + Component::InputInterface supercap_control_enabled_; Component::InputInterface chassis_output_status_; Component::InputInterface supercap_charge_power_limit_; }; From b660087f8e73eab4353c9e9dcb3d00b234921f1a Mon Sep 17 00:00:00 2001 From: eye-on Date: Tue, 12 May 2026 01:40:56 +0800 Subject: [PATCH 08/90] feat:supercup fixed --- .codex | 0 rmcs_ws/build | 1 + rmcs_ws/install | 1 + rmcs_ws/log | 1 + .../config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py | 14 +++++++------- rmcs_ws/src/serial | 1 + 7 files changed, 12 insertions(+), 8 deletions(-) create mode 100644 .codex create mode 120000 rmcs_ws/build create mode 120000 rmcs_ws/install create mode 120000 rmcs_ws/log create mode 160000 rmcs_ws/src/serial diff --git a/.codex b/.codex new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/build b/rmcs_ws/build new file mode 120000 index 00000000..2ec6ad5a --- /dev/null +++ b/rmcs_ws/build @@ -0,0 +1 @@ +build-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/install b/rmcs_ws/install new file mode 120000 index 00000000..4ed00624 --- /dev/null +++ b/rmcs_ws/install @@ -0,0 +1 @@ +install-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/log b/rmcs_ws/log new file mode 120000 index 00000000..4c7d622b --- /dev/null +++ b/rmcs_ws/log @@ -0,0 +1 @@ +log-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 840fd485..b968f194 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -46,7 +46,7 @@ deformable_infantry: left_back_zero_point: 5167 right_back_zero_point: 3098 right_front_zero_point: 6485 - yaw_motor_zero_point: 39442 + yaw_motor_zero_point: 25406 pitch_motor_zero_point: 56556 debug_log_supercap: false debug_log_wheel_motor: false diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index d810a08d..58abbbcc 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -54,13 +54,13 @@ def visit( if is_automatic: pass - entities.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' - ]) - ) - ) + # entities.append( + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ + # FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' + # ]) + # ) + # ) diff --git a/rmcs_ws/src/serial b/rmcs_ws/src/serial new file mode 160000 index 00000000..d50c4018 --- /dev/null +++ b/rmcs_ws/src/serial @@ -0,0 +1 @@ +Subproject commit d50c401809f17cc5862bdbe37cc6e15737ba7c49 From eeb2099ea52bf084a8dde64c723b0a2b3b95c341 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 12 May 2026 09:08:25 +0800 Subject: [PATCH 09/90] Refactor deformable chassis and joint controller configurations - Removed unused parameters and inputs related to joint velocity and torque feedforward from the deformable infantry steering configuration. - Simplified the DeformableChassis class by eliminating legacy joint feedback structures and inputs, focusing on physical angles only. - Updated the DeformableJointController to streamline configuration loading and mode handling, consolidating mode configurations into a single structure. - Enhanced the control logic to directly use the new configuration structure, removing unnecessary complexity and improving clarity. - Ensured that output limits are consistently applied across the control logic, enhancing robustness. --- .devcontainer/state/codex/.tmp/plugins | 1 + rmcs_ws/src/hikcamera | 1 + rmcs_ws/src/rmcs_auto_aim_v2 | 1 + .../config/deformable-infantry-omni-b.yaml | 65 ---- .../config/deformable-infantry-omni.yaml | 81 +---- .../config/deformable-infantry-steering.yaml | 65 ---- .../controller/chassis/deformable_chassis.cpp | 293 ++---------------- .../chassis/deformable_joint_controller.cpp | 258 +++------------ 8 files changed, 70 insertions(+), 695 deletions(-) create mode 160000 .devcontainer/state/codex/.tmp/plugins create mode 160000 rmcs_ws/src/hikcamera create mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 diff --git a/.devcontainer/state/codex/.tmp/plugins b/.devcontainer/state/codex/.tmp/plugins new file mode 160000 index 00000000..3c463363 --- /dev/null +++ b/.devcontainer/state/codex/.tmp/plugins @@ -0,0 +1 @@ +Subproject commit 3c46336323290cda7c90d78e7a0134f47cc73713 diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera new file mode 160000 index 00000000..f0077f03 --- /dev/null +++ b/rmcs_ws/src/hikcamera @@ -0,0 +1 @@ +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 00000000..8f25103a --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit 8f25103a42fd601bd8259a3da51ad8ef870a5b6b diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index b968f194..489ee300 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -178,12 +178,9 @@ lf_joint_controller: ros__parameters: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle - measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle - setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - # Normal ADRC servo mode dt: 0.001 b0: -1.0 kt: 1.0 @@ -200,30 +197,11 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - - # Suspension ADRC servo mode - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - - # Joint-local feedforward / limit shaping - torque_feedforward_gain: 0.0 lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle - measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle - setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -241,26 +219,11 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle - measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle - setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -278,26 +241,11 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle - measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle - setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 @@ -315,16 +263,3 @@ rf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 6a871259..75d015ef 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -61,21 +61,7 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 - # Suspension geometry / support model. - active_suspension_mass: 22.5 - active_suspension_rod_length: 0.150 - active_suspension_Kz: 150.0 - active_suspension_D_leg: 10.0 - active_suspension_torque_limit: 80.0 - active_suspension_gravity_comp_gain: 1.0 - active_suspension_control_acceleration_limit: 6.0 - active_suspension_preload_angle_deg: 8.0 - active_suspension_entry_offset_deg: 1.5 - active_suspension_ride_height_offset_deg: 3.0 - active_suspension_hold_travel_deg: 5.0 - active_suspension_activation_velocity_threshold_deg: 15.0 - - # IMU attitude correction as suspension force bias. + # IMU attitude correction at min-angle stance. active_suspension_pitch_kp: 8.0 active_suspension_pitch_ki: 0.35 active_suspension_pitch_kd: 0.28 @@ -197,12 +183,9 @@ lf_joint_controller: ros__parameters: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle - measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle - setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - # Normal ADRC servo mode dt: 0.001 b0: -0.60 kt: 1.0 @@ -219,30 +202,11 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - - # Suspension ADRC servo mode - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - - # Joint-local feedforward / limit shaping - torque_feedforward_gain: 0.0 lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle - measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle - setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -0.60 @@ -260,26 +224,11 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle - measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle - setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -0.60 @@ -297,26 +246,11 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle - measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle - setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -0.60 @@ -334,16 +268,3 @@ rf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 51a8fc1d..794c623b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -178,12 +178,9 @@ lf_joint_controller: ros__parameters: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle - measurement_velocity: /chassis/left_front_joint/physical_velocity setpoint_angle: /chassis/left_front_joint/target_physical_angle - setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - # Normal ADRC servo mode dt: 0.001 b0: -1.0 kt: 1.0 @@ -200,30 +197,11 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - - # Suspension ADRC servo mode - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - - # Joint-local feedforward / limit shaping - torque_feedforward_gain: 0.0 lb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle - measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle - setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -241,26 +219,11 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rb_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle - measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle - setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -278,26 +241,11 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 rf_joint_controller: ros__parameters: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle - measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle - setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 @@ -315,16 +263,3 @@ rf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 - suspension_td_h: 0.001 - suspension_td_r: 12.0 - suspension_eso_w0: 80.0 - suspension_k1: 6.0 - suspension_k2: 3.0 - suspension_alpha1: 0.75 - suspension_alpha2: 0.7 - suspension_delta: 0.02 - suspension_u_min: -35.0 - suspension_u_max: 35.0 - suspension_output_min: -35.0 - suspension_output_max: 35.0 - torque_feedforward_gain: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 10aabcfa..be49b501 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -26,7 +26,6 @@ class DeformableChassis : public rmcs_executor::Component , public rclcpp::Node { public: - enum class JointFeedbackSource : uint8_t { kLegacyEncoderAngle, kMotorAngle }; enum JointIndex : size_t { kLeftFront = 0, kLeftBack = 1, @@ -35,15 +34,6 @@ class DeformableChassis kJointCount = 4, }; - struct JointFeedbackFrame { - std::array motor_angles{}; - std::array physical_angles{}; - std::array physical_velocities{}; - std::array joint_torques{}; - std::array eso_z2{}; - std::array eso_z3{}; - }; - struct AttitudePidAxis { double kp = 20.0; double ki = 0.0; @@ -75,10 +65,6 @@ class DeformableChassis , min_angle_(get_parameter_or("min_angle", 15.0)) , max_angle_(get_parameter_or("max_angle", 55.0)) - , left_front_joint_offset_(get_parameter_or("left_front_joint_offset", 0.0)) - , left_back_joint_offset_(get_parameter_or("left_back_joint_offset", 0.0)) - , right_front_joint_offset_(get_parameter_or("right_front_joint_offset", 0.0)) - , right_back_joint_offset_(get_parameter_or("right_back_joint_offset", 0.0)) , target_physical_velocity_limit_( std::max( deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), @@ -152,11 +138,6 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/left_front_joint/angle", left_front_joint_angle_, false); - register_input("/chassis/left_back_joint/angle", left_back_joint_angle_, false); - register_input("/chassis/right_front_joint/angle", right_front_joint_angle_, false); - register_input("/chassis/right_back_joint/angle", right_back_joint_angle_, false); - register_input( "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); register_input( @@ -165,31 +146,6 @@ class DeformableChassis "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); register_input( "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - false); - register_input( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - false); - register_input( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - false); - register_input( - "/chassis/right_front_joint/physical_velocity", right_front_joint_physical_velocity_, - false); - register_input("/chassis/left_front_joint/torque", left_front_joint_torque_, false); - register_input("/chassis/left_back_joint/torque", left_back_joint_torque_, false); - register_input("/chassis/right_back_joint/torque", right_back_joint_torque_, false); - register_input("/chassis/right_front_joint/torque", right_front_joint_torque_, false); - - register_input( - "/chassis/left_front_joint/encoder_angle", left_front_joint_encoder_angle_, false); - register_input( - "/chassis/left_back_joint/encoder_angle", left_back_joint_encoder_angle_, false); - register_input( - "/chassis/right_front_joint/encoder_angle", right_front_joint_encoder_angle_, false); - register_input( - "/chassis/right_back_joint/encoder_angle", right_back_joint_encoder_angle_, false); register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); register_input("/chassis/imu/roll", chassis_imu_roll_, false); register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); @@ -208,15 +164,6 @@ class DeformableChassis register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); - register_output( - "/chassis/left_front_joint/target_angle", left_front_joint_target_angle_, nan_); - register_output( - "/chassis/left_back_joint/target_angle", left_back_joint_target_angle_, nan_); - register_output( - "/chassis/right_back_joint/target_angle", right_back_joint_target_angle_, nan_); - register_output( - "/chassis/right_front_joint/target_angle", right_front_joint_target_angle_, nan_); - register_output( "/chassis/left_front_joint/target_physical_angle", left_front_joint_target_physical_angle_, nan_); @@ -253,7 +200,6 @@ class DeformableChassis register_output( "/chassis/right_front_joint/target_physical_acceleration", right_front_joint_target_physical_acceleration_, nan_); - register_output("/chassis/processed_encoder/angle", processed_encoder_angle_, nan_); *mode_ = rmcs_msgs::ChassisMode::AUTO; chassis_control_velocity_->vector << nan_, nan_, nan_; @@ -263,23 +209,6 @@ class DeformableChassis lb_current_target_angle_ = max_angle_; rf_current_target_angle_ = max_angle_; rb_current_target_angle_ = max_angle_; - - const bool left_front_joint_offset = has_parameter("left_front_joint_offset"); - const bool left_back_joint_offset = has_parameter("left_back_joint_offset"); - const bool right_front_joint_offset = has_parameter("right_front_joint_offset"); - const bool right_back_joint_offset = has_parameter("right_back_joint_offset"); - - const bool has_any_joint_offset = left_front_joint_offset || left_back_joint_offset - || right_front_joint_offset || right_back_joint_offset; - const bool has_all_joint_offsets = left_front_joint_offset && left_back_joint_offset - && right_front_joint_offset && right_back_joint_offset; - if (has_any_joint_offset && !has_all_joint_offsets) - throw std::runtime_error( - "deformable chassis joint offsets must be configured for all four joints or " - "removed entirely"); - - joint_feedback_source_ = has_all_joint_offsets ? JointFeedbackSource::kLegacyEncoderAngle - : JointFeedbackSource::kMotorAngle; } void before_updating() override { @@ -292,14 +221,6 @@ class DeformableChassis RCLCPP_WARN( get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } - if (!left_front_joint_torque_.ready()) - left_front_joint_torque_.make_and_bind_directly(0.0); - if (!left_back_joint_torque_.ready()) - left_back_joint_torque_.make_and_bind_directly(0.0); - if (!right_back_joint_torque_.ready()) - right_back_joint_torque_.make_and_bind_directly(0.0); - if (!right_front_joint_torque_.ready()) - right_front_joint_torque_.make_and_bind_directly(0.0); if (!chassis_imu_pitch_.ready()) chassis_imu_pitch_.make_and_bind_directly(0.0); if (!chassis_imu_roll_.ready()) @@ -338,48 +259,18 @@ class DeformableChassis } private: - static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; - static double wrap_deg(double deg) { - deg = std::fmod(deg, 360.0); - if (deg >= 180.0) - deg -= 360.0; - if (deg < -180.0) - deg += 360.0; - return deg; - } - void validate_joint_feedback_inputs() const { - const bool ready = - joint_feedback_source_ == JointFeedbackSource::kMotorAngle - ? left_front_joint_angle_.ready() && left_back_joint_angle_.ready() - && right_front_joint_angle_.ready() && right_back_joint_angle_.ready() - : left_front_joint_encoder_angle_.ready() && left_back_joint_encoder_angle_.ready() - && right_front_joint_encoder_angle_.ready() - && right_back_joint_encoder_angle_.ready(); - - if (ready) + if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() + && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) return; throw std::runtime_error( - joint_feedback_source_ == JointFeedbackSource::kMotorAngle - ? "missing V2 joint feedback interfaces: expected /chassis/*_joint/angle" - : "missing legacy joint feedback interfaces: expected /chassis/*_joint/" - "encoder_angle"); - } - - double joint_angle_deg( - const InputInterface& joint_angle, - const InputInterface& joint_encoder_angle, double joint_offset, - double legacy_fixed_compensation) const { - if (joint_feedback_source_ == JointFeedbackSource::kMotorAngle) - return wrap_deg(*joint_angle * rad_to_deg_); - - return wrap_deg(joint_offset) - wrap_deg(*joint_encoder_angle) + legacy_fixed_compensation; + "missing deformable chassis feedback interfaces: expected /chassis/*_joint/physical_angle"); } void update_mode_from_inputs_( @@ -416,13 +307,20 @@ class DeformableChassis *mode_ = mode; } - // JointFeedbackAdapter: normalize motor / physical / legacy encoder feedback into one frame. - JointFeedbackFrame read_joint_feedback_frame_() const { - JointFeedbackFrame joint_feedback; - update_current_joint_feedback( - joint_feedback.motor_angles, joint_feedback.physical_angles, - joint_feedback.physical_velocities, joint_feedback.joint_torques); - return joint_feedback; + std::array read_current_joint_physical_angles_() const { + const std::array*, kJointCount> physical_angle_inputs{ + &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, + &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; + + std::array current_physical_angles{}; + current_physical_angles.fill(nan_); + for (size_t i = 0; i < kJointCount; ++i) { + if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { + current_physical_angles[i] = *(*physical_angle_inputs[i]); + } + } + + return current_physical_angles; } bool prone_override_requested_by_keyboard() const { return keyboard_.ready() && keyboard_->ctrl; } @@ -527,57 +425,13 @@ class DeformableChassis chassis_imu_calibration_sample_count_); } - void update_current_joint_feedback( - std::array& current_motor_angles, - std::array& current_physical_angles, - std::array& current_physical_velocities, - std::array& current_joint_torques) const { - const std::array*, kJointCount> motor_angle_inputs{ - &left_front_joint_angle_, &left_back_joint_angle_, &right_back_joint_angle_, - &right_front_joint_angle_}; - const std::array*, kJointCount> physical_angle_inputs{ - &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, - &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; - const std::array*, kJointCount> physical_velocity_inputs{ - &left_front_joint_physical_velocity_, &left_back_joint_physical_velocity_, - &right_back_joint_physical_velocity_, &right_front_joint_physical_velocity_}; - const std::array*, kJointCount> torque_inputs{ - &left_front_joint_torque_, &left_back_joint_torque_, &right_back_joint_torque_, - &right_front_joint_torque_}; - current_motor_angles.fill(nan_); - current_physical_angles.fill(nan_); - current_physical_velocities.fill(nan_); - current_joint_torques.fill(nan_); - - for (size_t i = 0; i < kJointCount; ++i) { - if (motor_angle_inputs[i]->ready() && std::isfinite(*(*motor_angle_inputs[i]))) { - current_motor_angles[i] = *(*motor_angle_inputs[i]); - current_physical_angles[i] = motor_to_physical_angle(current_motor_angles[i]); - } - - if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { - current_physical_angles[i] = *(*physical_angle_inputs[i]); - } - if (physical_velocity_inputs[i]->ready() - && std::isfinite(*(*physical_velocity_inputs[i]))) { - current_physical_velocities[i] = *(*physical_velocity_inputs[i]); - } - if (torque_inputs[i]->ready() && std::isfinite(*(*torque_inputs[i]))) { - current_joint_torques[i] = *(*torque_inputs[i]); - } - } - } - bool initialize_joint_target_states_from_feedback( - const std::array& current_motor_angles, const std::array& current_physical_angles) { for (size_t i = 0; i < kJointCount; ++i) { - if (!std::isfinite(current_motor_angles[i]) - || !std::isfinite(current_physical_angles[i])) + if (!std::isfinite(current_physical_angles[i])) return false; } - joint_target_angle_state_rad_ = current_motor_angles; joint_target_physical_angle_state_rad_ = current_physical_angles; joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; @@ -587,7 +441,7 @@ class DeformableChassis return true; } - void update_active_suspension_(const JointFeedbackFrame&) { + void update_active_suspension_() { if (!suspension_requested_by_input_()) { reset_attitude_correction_state_(); return; @@ -662,11 +516,6 @@ class DeformableChassis *rf_angle_error_ = nan_; *rb_angle_error_ = nan_; - *left_front_joint_target_angle_ = nan_; - *left_back_joint_target_angle_ = nan_; - *right_back_joint_target_angle_ = nan_; - *right_front_joint_target_angle_ = nan_; - *left_front_joint_target_physical_angle_ = nan_; *left_back_joint_target_physical_angle_ = nan_; *right_back_joint_target_physical_angle_ = nan_; @@ -680,8 +529,6 @@ class DeformableChassis *right_back_joint_target_physical_acceleration_ = nan_; *right_front_joint_target_physical_acceleration_ = nan_; - *processed_encoder_angle_ = nan_; - } void update_velocity_control() { @@ -811,11 +658,10 @@ class DeformableChassis // Chassis owns the high-level joint intent pipeline: read feedback, generate deploy targets, // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. void run_joint_intent_pipeline_() { - const auto joint_feedback = read_joint_feedback_frame_(); + const auto current_physical_angles = read_current_joint_physical_angles_(); if (!joint_target_active_ - && !initialize_joint_target_states_from_feedback( - joint_feedback.motor_angles, joint_feedback.physical_angles)) { + && !initialize_joint_target_states_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); return; } @@ -823,22 +669,14 @@ class DeformableChassis update_chassis_imu_calibration_(); const bool prone_override = refresh_requested_joint_targets_from_deploy_state_(); scope_motor_control(prone_override); - update_active_suspension_(joint_feedback); + update_active_suspension_(); update_joint_target_trajectory(); - publish_joint_target_angles(joint_feedback.physical_angles); + publish_joint_target_angles(current_physical_angles); } static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - static double physical_to_motor_angle(double physical_angle_rad) { - return joint_zero_physical_angle_rad_ - physical_angle_rad; - } - - static double motor_to_physical_angle(double motor_angle_rad) { - return joint_zero_physical_angle_rad_ - motor_angle_rad; - } - void scope_motor_control(bool prone_override = false) { const bool prone_target_active = prone_override; if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { @@ -854,31 +692,6 @@ class DeformableChassis } } - bool publish_current_joint_target_angles() { - const std::array*, kJointCount> motor_angle_inputs{ - &left_front_joint_angle_, &left_back_joint_angle_, &right_back_joint_angle_, - &right_front_joint_angle_}; - - std::array current_motor_angles{}; - std::array current_physical_angles{}; - for (size_t i = 0; i < kJointCount; ++i) { - if (!motor_angle_inputs[i]->ready() || !std::isfinite(*(*motor_angle_inputs[i]))) { - return false; - } - current_motor_angles[i] = *(*motor_angle_inputs[i]); - current_physical_angles[i] = motor_to_physical_angle(current_motor_angles[i]); - } - - joint_target_angle_state_rad_ = current_motor_angles; - joint_target_physical_angle_state_rad_ = current_physical_angles; - joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - requested_target_physical_angles_rad_ = current_physical_angles; - current_target_physical_angles_rad_ = current_physical_angles; - joint_target_active_ = true; - return true; - } - void update_joint_target_trajectory() { const double dt = update_dt(); for (size_t i = 0; i < kJointCount; ++i) { @@ -922,8 +735,6 @@ class DeformableChassis velocity_state = 0.0; acceleration_state = 0.0; } - - joint_target_angle_state_rad_[i] = physical_to_motor_angle(angle_state); } } @@ -934,11 +745,6 @@ class DeformableChassis return; } - *left_front_joint_target_angle_ = joint_target_angle_state_rad_[kLeftFront]; - *left_back_joint_target_angle_ = joint_target_angle_state_rad_[kLeftBack]; - *right_back_joint_target_angle_ = joint_target_angle_state_rad_[kRightBack]; - *right_front_joint_target_angle_ = joint_target_angle_state_rad_[kRightFront]; - *left_front_joint_target_physical_angle_ = joint_target_physical_angle_state_rad_[kLeftFront]; *left_back_joint_target_physical_angle_ = joint_target_physical_angle_state_rad_[kLeftBack]; @@ -981,30 +787,11 @@ class DeformableChassis ? current_physical_angles[kRightFront] - joint_target_physical_angle_state_rad_[kRightFront] : nan_; - - bool all_joint_angles_finite = true; - double physical_angle_sum = 0.0; - for (double current_physical_angle : current_physical_angles) { - if (!std::isfinite(current_physical_angle)) { - all_joint_angles_finite = false; - break; - } - physical_angle_sum += current_physical_angle; - } - - *processed_encoder_angle_ = all_joint_angles_finite ? rad_to_deg_ * physical_angle_sum - / static_cast(kJointCount) - : nan_; } void publish_nan_joint_targets() { reset_attitude_correction_state_(); - *left_front_joint_target_angle_ = nan_; - *left_back_joint_target_angle_ = nan_; - *right_back_joint_target_angle_ = nan_; - *right_front_joint_target_angle_ = nan_; - *left_front_joint_target_physical_angle_ = nan_; *left_back_joint_target_physical_angle_ = nan_; *right_back_joint_target_physical_angle_ = nan_; @@ -1054,28 +841,10 @@ class DeformableChassis pid::PidCalculator following_velocity_controller_; const double spin_ratio_; - InputInterface left_front_joint_angle_; - InputInterface left_back_joint_angle_; - InputInterface right_front_joint_angle_; - InputInterface right_back_joint_angle_; - InputInterface left_front_joint_physical_angle_; InputInterface left_back_joint_physical_angle_; InputInterface right_front_joint_physical_angle_; InputInterface right_back_joint_physical_angle_; - InputInterface left_front_joint_physical_velocity_; - InputInterface left_back_joint_physical_velocity_; - InputInterface right_front_joint_physical_velocity_; - InputInterface right_back_joint_physical_velocity_; - InputInterface left_front_joint_torque_; - InputInterface left_back_joint_torque_; - InputInterface right_front_joint_torque_; - InputInterface right_back_joint_torque_; - - InputInterface left_front_joint_encoder_angle_; - InputInterface left_back_joint_encoder_angle_; - InputInterface right_front_joint_encoder_angle_; - InputInterface right_back_joint_encoder_angle_; InputInterface chassis_imu_pitch_; InputInterface chassis_imu_roll_; InputInterface chassis_imu_pitch_rate_; @@ -1088,11 +857,6 @@ class DeformableChassis OutputInterface rf_angle_error_; OutputInterface rb_angle_error_; - OutputInterface left_front_joint_target_angle_; - OutputInterface left_back_joint_target_angle_; - OutputInterface right_back_joint_target_angle_; - OutputInterface right_front_joint_target_angle_; - OutputInterface left_front_joint_target_physical_angle_; OutputInterface left_back_joint_target_physical_angle_; OutputInterface right_back_joint_target_physical_angle_; @@ -1106,15 +870,8 @@ class DeformableChassis OutputInterface right_back_joint_target_physical_acceleration_; OutputInterface right_front_joint_target_physical_acceleration_; - OutputInterface processed_encoder_angle_; - double min_angle_; double max_angle_; - double left_front_joint_offset_; - double left_back_joint_offset_; - double right_front_joint_offset_; - double right_back_joint_offset_; - JointFeedbackSource joint_feedback_source_ = JointFeedbackSource::kLegacyEncoderAngle; double current_target_angle_; double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, @@ -1124,7 +881,6 @@ class DeformableChassis std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; bool joint_target_active_ = false; - std::array joint_target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array joint_target_physical_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array joint_target_physical_velocity_state_rad_ = { 0.0, 0.0, 0.0, 0.0}; @@ -1159,9 +915,6 @@ class DeformableChassis double chassis_imu_roll_sum_ = 0.0; bool chassis_imu_calibration_completed_for_window_ = false; static constexpr double default_dt_ = 1e-3; - static constexpr double joint_zero_physical_angle_rad_ = 1.090830782496456; - - static constexpr double pi_ = std::numbers::pi; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index a629449c..14623175 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -33,22 +32,19 @@ class DeformableJointController : public rmcs_executor::Component , public rclcpp::Node { public: - // Joint controller owns only joint-local servo execution. Higher-level deploy/suspension - // intent is generated upstream by DeformableChassis via setpoint/mode/feedforward inputs. - struct ModeConfig { + // Joint controller owns only the local angle-servo execution. Chassis publishes the + // higher-level target angle trajectory; this controller turns that target into motor torque. + struct ControllerConfig { rmcs_core::controller::adrc::TD::Config td; rmcs_core::controller::adrc::ESO::Config eso; rmcs_core::controller::adrc::NLESF::Config nlesf; double output_min = -std::numeric_limits::infinity(); double output_max = std::numeric_limits::infinity(); - double torque_feedforward_gain = 0.0; }; struct InputSnapshot { double measurement_angle = std::numeric_limits::quiet_NaN(); double setpoint_angle = std::numeric_limits::quiet_NaN(); - double joint_torque_feedforward = 0.0; - bool suspension_mode = false; }; DeformableJointController() @@ -56,8 +52,8 @@ class DeformableJointController get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { register_interfaces_(); - load_mode_configs_(); - apply_mode_config_(normal_mode_config_); + load_config_(); + apply_config_(); } void update() override { @@ -67,208 +63,74 @@ class DeformableJointController return; } - update_mode_selection_(inputs.suspension_mode); - const auto& mode_config = active_mode_config_(inputs.suspension_mode); - const auto [output_min, output_max] = effective_output_limits_(mode_config); - if (std::isnan(output_min) || std::isnan(output_max) || output_min > output_max) { - disable_output_(); - return; - } - - if (!ensure_feedforward_ready_(mode_config, inputs)) { - disable_output_(); - return; - } - initialize_if_needed_(inputs); - rmcs_core::controller::adrc::ESO::Output eso_out; double control_torque = nan_; - if (!run_joint_servo_( - inputs, mode_config, output_min, output_max, eso_out, control_torque)) { + if (!run_joint_servo_(inputs, control_torque)) { disable_output_(); return; } - publish_control_output_(control_torque, eso_out); + publish_control_output_(control_torque); } private: void register_interfaces_() { register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); - register_input(get_parameter("measurement_velocity").as_string(), measurement_velocity_); register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); - - if (has_parameter("setpoint_velocity")) { - register_input( - get_parameter("setpoint_velocity").as_string(), setpoint_velocity_input_); - } else { - setpoint_velocity_input_.make_and_bind_directly(0.0); - } - - if (has_parameter("mode_input")) { - register_input(get_parameter("mode_input").as_string(), suspension_mode_input_); - } else { - suspension_mode_input_.make_and_bind_directly(false); - } - - if (has_parameter("suspension_torque")) { - register_input( - get_parameter("suspension_torque").as_string(), joint_torque_feedforward_input_); - } else { - joint_torque_feedforward_input_.make_and_bind_directly(0.0); - } - if (has_parameter("limit")) { - register_input(get_parameter("limit").as_string(), output_limit_); - use_dynamic_limit_ = true; - } - register_output(get_parameter("control").as_string(), control_torque_, nan_); - if (has_parameter("eso_z2_output")) { - register_output(get_parameter("eso_z2_output").as_string(), eso_z2_output_, nan_); - } - if (has_parameter("eso_z3_output")) { - register_output(get_parameter("eso_z3_output").as_string(), eso_z3_output_, nan_); - } } - void load_mode_configs_() { + void load_config_() { dt_ = load_parameter_or(*this, "dt", 0.001); b0_ = load_parameter_or(*this, "b0", 1.0); kt_ = load_parameter_or(*this, "kt", 1.0); - normal_mode_config_.td.h = load_parameter_or(*this, "td_h", dt_); - normal_mode_config_.td.r = load_parameter_or(*this, "td_r", 300.0); - normal_mode_config_.td.max_vel = + config_.td.h = load_parameter_or(*this, "td_h", dt_); + config_.td.r = load_parameter_or(*this, "td_r", 300.0); + config_.td.max_vel = load_parameter_or(*this, "td_max_vel", std::numeric_limits::infinity()); - normal_mode_config_.td.max_acc = + config_.td.max_acc = load_parameter_or(*this, "td_max_acc", std::numeric_limits::infinity()); - normal_mode_config_.eso.h = dt_; - normal_mode_config_.eso.b0 = b0_; - normal_mode_config_.eso.w0 = load_parameter_or(*this, "eso_w0", 80.0); - normal_mode_config_.eso.auto_beta = load_parameter_or(*this, "eso_auto_beta", true); - normal_mode_config_.eso.beta1 = - load_parameter_or(*this, "eso_beta1", 3.0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.beta2 = load_parameter_or( - *this, "eso_beta2", 3.0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.beta3 = load_parameter_or( - *this, "eso_beta3", - normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0 * normal_mode_config_.eso.w0); - normal_mode_config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); - - normal_mode_config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); - normal_mode_config_.nlesf.k2 = load_parameter_or(*this, "k2", 5.0); - normal_mode_config_.nlesf.alpha1 = load_parameter_or(*this, "alpha1", 0.75); - normal_mode_config_.nlesf.alpha2 = load_parameter_or(*this, "alpha2", 1.25); - normal_mode_config_.nlesf.delta = load_parameter_or(*this, "delta", 0.01); - normal_mode_config_.nlesf.u_min = + config_.eso.h = dt_; + config_.eso.b0 = b0_; + config_.eso.w0 = load_parameter_or(*this, "eso_w0", 80.0); + config_.eso.auto_beta = load_parameter_or(*this, "eso_auto_beta", true); + config_.eso.beta1 = load_parameter_or(*this, "eso_beta1", 3.0 * config_.eso.w0); + config_.eso.beta2 = + load_parameter_or(*this, "eso_beta2", 3.0 * config_.eso.w0 * config_.eso.w0); + config_.eso.beta3 = load_parameter_or( + *this, "eso_beta3", config_.eso.w0 * config_.eso.w0 * config_.eso.w0); + config_.eso.z3_limit = load_parameter_or(*this, "eso_z3_limit", 1e9); + + config_.nlesf.k1 = load_parameter_or(*this, "k1", 50.0); + config_.nlesf.k2 = load_parameter_or(*this, "k2", 5.0); + config_.nlesf.alpha1 = load_parameter_or(*this, "alpha1", 0.75); + config_.nlesf.alpha2 = load_parameter_or(*this, "alpha2", 1.25); + config_.nlesf.delta = load_parameter_or(*this, "delta", 0.01); + config_.nlesf.u_min = load_parameter_or(*this, "u_min", -std::numeric_limits::infinity()); - normal_mode_config_.nlesf.u_max = + config_.nlesf.u_max = load_parameter_or(*this, "u_max", std::numeric_limits::infinity()); - normal_mode_config_.output_min = + config_.output_min = load_parameter_or(*this, "output_min", -std::numeric_limits::infinity()); - normal_mode_config_.output_max = + config_.output_max = load_parameter_or(*this, "output_max", std::numeric_limits::infinity()); - if (normal_mode_config_.output_min > normal_mode_config_.output_max) { - std::swap(normal_mode_config_.output_min, normal_mode_config_.output_max); + if (config_.output_min > config_.output_max) { + std::swap(config_.output_min, config_.output_max); } - normal_mode_config_.torque_feedforward_gain = - load_parameter_or(*this, "torque_feedforward_gain", 0.0); - - suspension_mode_config_ = normal_mode_config_; - suspension_mode_config_.td.h = - load_parameter_or(*this, "suspension_td_h", suspension_mode_config_.td.h); - suspension_mode_config_.td.r = - load_parameter_or(*this, "suspension_td_r", suspension_mode_config_.td.r); - suspension_mode_config_.td.max_vel = - load_parameter_or(*this, "suspension_td_max_vel", suspension_mode_config_.td.max_vel); - suspension_mode_config_.td.max_acc = - load_parameter_or(*this, "suspension_td_max_acc", suspension_mode_config_.td.max_acc); - suspension_mode_config_.eso.w0 = - load_parameter_or(*this, "suspension_eso_w0", suspension_mode_config_.eso.w0); - suspension_mode_config_.eso.auto_beta = load_parameter_or( - *this, "suspension_eso_auto_beta", suspension_mode_config_.eso.auto_beta); - suspension_mode_config_.eso.beta1 = - load_parameter_or(*this, "suspension_eso_beta1", suspension_mode_config_.eso.beta1); - suspension_mode_config_.eso.beta2 = - load_parameter_or(*this, "suspension_eso_beta2", suspension_mode_config_.eso.beta2); - suspension_mode_config_.eso.beta3 = - load_parameter_or(*this, "suspension_eso_beta3", suspension_mode_config_.eso.beta3); - suspension_mode_config_.eso.z3_limit = load_parameter_or( - *this, "suspension_eso_z3_limit", suspension_mode_config_.eso.z3_limit); - suspension_mode_config_.nlesf.k1 = - load_parameter_or(*this, "suspension_k1", suspension_mode_config_.nlesf.k1); - suspension_mode_config_.nlesf.k2 = - load_parameter_or(*this, "suspension_k2", suspension_mode_config_.nlesf.k2); - suspension_mode_config_.nlesf.alpha1 = - load_parameter_or(*this, "suspension_alpha1", suspension_mode_config_.nlesf.alpha1); - suspension_mode_config_.nlesf.alpha2 = - load_parameter_or(*this, "suspension_alpha2", suspension_mode_config_.nlesf.alpha2); - suspension_mode_config_.nlesf.delta = - load_parameter_or(*this, "suspension_delta", suspension_mode_config_.nlesf.delta); - suspension_mode_config_.nlesf.u_min = - load_parameter_or(*this, "suspension_u_min", suspension_mode_config_.nlesf.u_min); - suspension_mode_config_.nlesf.u_max = - load_parameter_or(*this, "suspension_u_max", suspension_mode_config_.nlesf.u_max); - suspension_mode_config_.output_min = - load_parameter_or(*this, "suspension_output_min", normal_mode_config_.output_min); - suspension_mode_config_.output_max = - load_parameter_or(*this, "suspension_output_max", normal_mode_config_.output_max); - if (suspension_mode_config_.output_min > suspension_mode_config_.output_max) { - std::swap(suspension_mode_config_.output_min, suspension_mode_config_.output_max); - } - suspension_mode_config_.torque_feedforward_gain = - load_parameter_or(*this, "suspension_torque_feedforward_gain", 1.0); } bool read_inputs_(InputSnapshot& inputs) const { inputs.measurement_angle = *measurement_angle_; inputs.setpoint_angle = *setpoint_angle_; - inputs.joint_torque_feedforward = *joint_torque_feedforward_input_; - inputs.suspension_mode = *suspension_mode_input_; return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); } - void update_mode_selection_(bool suspension_mode) { - if (suspension_mode != last_suspension_mode_) { - apply_mode_config_(active_mode_config_(suspension_mode)); - last_suspension_mode_ = suspension_mode; - } - } - - const ModeConfig& active_mode_config_(bool suspension_mode) const { - return suspension_mode ? suspension_mode_config_ : normal_mode_config_; - } - - void apply_mode_config_(const ModeConfig& mode_config) { - td_.set_config(mode_config.td); - eso_.set_config(mode_config.eso); - nlesf_.set_config(mode_config.nlesf); - } - - std::pair effective_output_limits_(const ModeConfig& mode_config) const { - double output_min = mode_config.output_min; - double output_max = mode_config.output_max; - if (use_dynamic_limit_) { - if (!output_limit_.ready()) { - return {nan_, nan_}; - } - const double limit = *output_limit_; - if (!std::isfinite(limit)) { - return {nan_, nan_}; - } - - const double effective_limit = std::max(0.0, limit); - output_min = std::max(output_min, -effective_limit); - output_max = std::min(output_max, effective_limit); - } - return {output_min, output_max}; - } - - bool ensure_feedforward_ready_( - const ModeConfig& mode_config, const InputSnapshot& inputs) const { - return mode_config.torque_feedforward_gain == 0.0 - || std::isfinite(inputs.joint_torque_feedforward); + void apply_config_() { + td_.set_config(config_.td); + eso_.set_config(config_.eso); + nlesf_.set_config(config_.nlesf); } void initialize_if_needed_(const InputSnapshot& inputs) { @@ -279,21 +141,15 @@ class DeformableJointController initialized_ = true; } - bool run_joint_servo_( - const InputSnapshot& inputs, const ModeConfig& mode_config, double output_min, - double output_max, rmcs_core::controller::adrc::ESO::Output& eso_out, - double& control_torque) { + bool run_joint_servo_(const InputSnapshot& inputs, double& control_torque) { const auto td_out = td_.update(inputs.setpoint_angle); - eso_out = eso_.update(inputs.measurement_angle, last_u_); + const auto eso_out = eso_.update(inputs.measurement_angle, last_u_); const double e1 = td_out.x1 - eso_out.z1; const double e2 = td_out.x2 - eso_out.z2; control_torque = kt_ * nlesf_.compute(e1, e2, eso_out.z3, b0_).u; - if (mode_config.torque_feedforward_gain != 0.0) { - control_torque += mode_config.torque_feedforward_gain * inputs.joint_torque_feedforward; - } - control_torque = std::clamp(control_torque, output_min, output_max); + control_torque = std::clamp(control_torque, config_.output_min, config_.output_max); return std::isfinite(control_torque); } @@ -303,63 +159,35 @@ class DeformableJointController last_u_ = 0.0; } - void publish_control_output_( - double control_torque, const rmcs_core::controller::adrc::ESO::Output& eso_out) { + void publish_control_output_(double control_torque) { *control_torque_ = control_torque; last_u_ = control_torque; - publish_eso_state_(eso_out); - } - - void publish_eso_state_(const rmcs_core::controller::adrc::ESO::Output& eso_out) { - if (eso_z2_output_.active()) { - *eso_z2_output_ = eso_out.z2; - } - if (eso_z3_output_.active()) { - *eso_z3_output_ = eso_out.z3; - } } void disable_output_() { initialized_ = false; last_u_ = 0.0; *control_torque_ = nan_; - if (eso_z2_output_.active()) { - *eso_z2_output_ = nan_; - } - if (eso_z3_output_.active()) { - *eso_z3_output_ = nan_; - } } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); InputInterface measurement_angle_; - InputInterface measurement_velocity_; InputInterface setpoint_angle_; - // Kept for graph compatibility; TD derives the actual servo-rate target locally. - InputInterface setpoint_velocity_input_; - InputInterface suspension_mode_input_; - InputInterface joint_torque_feedforward_input_; - InputInterface output_limit_; OutputInterface control_torque_; - OutputInterface eso_z2_output_; - OutputInterface eso_z3_output_; rmcs_core::controller::adrc::TD td_; rmcs_core::controller::adrc::ESO eso_; rmcs_core::controller::adrc::NLESF nlesf_; - ModeConfig normal_mode_config_; - ModeConfig suspension_mode_config_; + ControllerConfig config_; double dt_ = 0.001; double b0_ = 1.0; double kt_ = 1.0; double last_u_ = 0.0; - bool use_dynamic_limit_ = false; bool initialized_ = false; - bool last_suspension_mode_ = false; }; } // namespace rmcs_core::controller::chassis From fdd0ae0ad00756dbc2c95b74d19d0cc8521c3e5c Mon Sep 17 00:00:00 2001 From: eye-on Date: Tue, 12 May 2026 02:39:21 +0800 Subject: [PATCH 10/90] feat:supercup fixed v2 --- .../src/controller/chassis/chassis_power_controller.cpp | 5 +++-- .../src/rmcs_core/src/hardware/deformable-infantry-omni.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2d9fa58b..a014d717 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -109,8 +109,9 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; + const bool supercap_active = *supercap_control_enabled_ && *supercap_enabled_; - if (*supercap_control_enabled_ && *supercap_enabled_) + if (supercap_active) power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP ? inf_ : *chassis_power_limit_referee_ + 80.0; @@ -129,7 +130,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - constexpr double excess_power_limit = 15; + const double excess_power_limit = supercap_active ? 15.0 : 0.0; power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 6a80eff4..561c22c0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -123,7 +123,7 @@ class DeformableInfantryOmni for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.0) + .set_reduction_ratio(13.0) .enable_multi_turn_angle() .set_reversed()); From 2faf36ea9a83c0116cd6362b51fa54d2a95555c5 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 12 May 2026 12:36:11 +0800 Subject: [PATCH 11/90] feat: update build scripts and configurations for cross-compilation support --- .gitignore | 11 +++++++---- .script/build-rmcs-cross | 9 +++++++++ .script/clean-rmcs | 8 +++++++- .../config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/package.xml | 4 ++-- 5 files changed, 26 insertions(+), 8 deletions(-) diff --git a/.gitignore b/.gitignore index 05b12839..de98c474 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,10 @@ -devel/ -log/ -build/ -install/ +/devel +/log +/build +/install +/rmcs_ws/log +/rmcs_ws/build +/rmcs_ws/install log-cross-*/ build-cross-*/ install-cross-*/ diff --git a/.script/build-rmcs-cross b/.script/build-rmcs-cross index 488054da..e9282b28 100755 --- a/.script/build-rmcs-cross +++ b/.script/build-rmcs-cross @@ -13,12 +13,16 @@ Examples: build-rmcs-cross --target-arch arm64 build-rmcs-cross --target-arch arm64 --link-default build-rmcs-cross --target-arch amd64 --packages-up-to rmcs_executor + +Notes: + arm64 cross builds automatically skip 'hikcamera' and 'rmcs_auto_aim_v2'. EOF } target_arch="" link_default=0 colcon_args=() +auto_skip_packages=() while (($# > 0)); do case "$1" in @@ -59,6 +63,7 @@ fi case "${target_arch}" in arm64) target_triplet="aarch64-linux-gnu" + auto_skip_packages=(hikcamera rmcs_auto_aim_v2) ;; amd64) target_triplet="x86_64-linux-gnu" @@ -152,6 +157,10 @@ export PKG_CONFIG_LIBDIR="${RMCS_SYSROOT}/usr/local/lib/${RMCS_TARGET_TRIPLET}/p cd "${workspace}" +if ((${#auto_skip_packages[@]} > 0)); then + colcon_args+=(--packages-skip "${auto_skip_packages[@]}") +fi + build_base="build-cross-${RMCS_TARGET_ARCH}" install_base="install-cross-${RMCS_TARGET_ARCH}" log_base="log-cross-${RMCS_TARGET_ARCH}" diff --git a/.script/clean-rmcs b/.script/clean-rmcs index 4c1aa6a6..f690e9e6 100755 --- a/.script/clean-rmcs +++ b/.script/clean-rmcs @@ -10,4 +10,10 @@ fi rm -rf -- \ "${RMCS_PATH}/rmcs_ws/build" \ "${RMCS_PATH}/rmcs_ws/install" \ - "${RMCS_PATH}/rmcs_ws/log" + "${RMCS_PATH}/rmcs_ws/log" \ + "${RMCS_PATH}/rmcs_ws/build-cross-arm64" \ + "${RMCS_PATH}/rmcs_ws/install-cross-arm64" \ + "${RMCS_PATH}/rmcs_ws/log-cross-arm64" \ + "${RMCS_PATH}/rmcs_ws/build-cross-amd64" \ + "${RMCS_PATH}/rmcs_ws/install-cross-amd64" \ + "${RMCS_PATH}/rmcs_ws/log-cross-amd64" diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 489ee300..c49fdce2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -40,7 +40,7 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_rmcs_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" left_front_zero_point: 7173 left_back_zero_point: 5167 diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index 9c8cc981..726e7b26 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,7 +11,7 @@ ament_cmake joint_state_broadcaster - rmcs_auto_aim_v2 + rmcs_auto_aim_v2 robot_state_publisher rviz2 xacro @@ -22,4 +22,4 @@ ament_cmake - \ No newline at end of file + From a8e6014a738fd06579faaa17eb20341638217696 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 14 May 2026 05:25:32 +0800 Subject: [PATCH 12/90] feat: update configuration and controller parameters for deformable infantry Co-authored-by: Copilot --- .../config/deformable-infantry-omni-b.yaml | 38 ++++--- .../config/deformable-infantry-omni.yaml | 6 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 14 +-- .../chassis/chassis_power_controller.cpp | 2 +- .../hardware/deformable-infantry-omni-b.cpp | 99 +++++++++++++++---- .../referee/app/ui/deformable_infantry_ui.cpp | 14 +-- 6 files changed, 121 insertions(+), 52 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index c49fdce2..0b28cf37 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -29,7 +29,8 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs::AutoAimComponent -> auto_aim_component value_broadcaster: ros__parameters: @@ -40,14 +41,15 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 right_front_zero_point: 6485 yaw_motor_zero_point: 25406 - pitch_motor_zero_point: 56556 + pitch_motor_zero_point: 56354 debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false @@ -55,8 +57,8 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 20.0 - max_angle: 50.0 + min_angle: 7.0 + max_angle: 58.0 active_suspension_enable: true spin_ratio: 1.0 @@ -84,31 +86,35 @@ chassis_controller: gimbal_controller: ros__parameters: - inertia: 1.0 # kg·m² - friction: 1.65 # Nm/(rad/s) - - upper_limit: -0.61 # -35 deg + upper_limit: -0.80 # -35 deg lower_limit: 0.05 # 6 deg - use_encoder_pitch: true - yaw_angle_kp: 10.0 + yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 8.0 + yaw_velocity_kp: 15.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - pitch_angle_kp: 40.0 + yaw_vel_ff_gain: 0.47 + yaw_acc_ff_gain: 0.00 + + pitch_angle_kp: 25.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 - pitch_velocity_kp: 3.0 + pitch_velocity_kp: 2.2 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 + pitch_acc_ff_gain: 0.10 + pitch_torque_control: true + pitch_fusion_enabled: true + pitch_fusion_alpha: 0.98 + friction_wheel_controller: ros__parameters: friction_wheels: @@ -158,7 +164,7 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.4 + kp: 1.5 ki: 0.0 kd: 0.0 @@ -169,7 +175,7 @@ deformable_chassis_controller: chassis_radius: 0.2341741 rod_length: 0.150 wheel_radius: 0.055 - friction_coefficient: 0.6 + friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 75d015ef..84bfd1e3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -48,7 +48,7 @@ deformable_infantry: right_back_zero_point: 7817 right_front_zero_point: 7136 yaw_motor_zero_point: 50642 - pitch_motor_zero_point: 6245 + pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false debug_log_deformable_joint_motor: false @@ -85,7 +85,7 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.61 # -35 deg + upper_limit: -0.80 # -35 deg lower_limit: 0.05 # 6 deg yaw_angle_kp: 30.0 @@ -174,7 +174,7 @@ deformable_chassis_controller: chassis_radius: 0.2341741 rod_length: 0.150 wheel_radius: 0.055 - friction_coefficient: 66.6 + friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 58abbbcc..d810a08d 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -54,13 +54,13 @@ def visit( if is_automatic: pass - # entities.append( - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource([ - # FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' - # ]) - # ) - # ) + entities.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' + ]) + ) + ) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index a014d717..1725dfa9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -64,7 +64,7 @@ class ChassisPowerController update_virtual_buffer_energy(); - boost_mode_ = keyboard.shift || rotary_knob < -0.9; + boost_mode_ = keyboard.shift || keyboard.ctrl || rotary_knob < -0.9; *supercap_control_enabled_ = boost_mode_; update_control_power_limit(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index f07b1de3..9be7189a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -61,12 +61,17 @@ class DeformableInfantryOmniB status_service_callback(response); }); + std::string serial_filter_imu; + get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); + rmcs_board_lite = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string()); + get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); + if (!serial_filter_imu.empty()) + imu_board_ = std::make_unique(*this, serial_filter_imu); } ~DeformableInfantryOmniB() override = default; @@ -79,6 +84,8 @@ class DeformableInfantryOmniB void update() override { rmcs_board_lite->update(); top_board_->update(); + if (imu_board_) + imu_board_->update(); } void command_update() { @@ -90,6 +97,7 @@ class DeformableInfantryOmniB private: class DeformableInfantryOmniBCommand; class BottomBoard; + class ImuBoard; class TopBoard; class DeformableInfantryOmniBCommand : public rmcs_executor::Component { @@ -586,6 +594,54 @@ class DeformableInfantryOmniB OutputInterface radius_; }; + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryOmniB; + + public: + explicit ImuBoard( + DeformableInfantryOmniB& deformableInfantry, const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , tf_{deformableInfantry.tf_} + , bmi088_{1000, 0.2, 0.0} { + + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + } + + ~ImuBoard() override = default; + + void update() { + bmi088_.update_status(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + private: + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; + + device::Bmi088 bmi088_; + }; + class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class DeformableInfantryOmniB; @@ -593,11 +649,12 @@ class DeformableInfantryOmniB explicit TopBoard( DeformableInfantryOmniB& deformableInfantry, DeformableInfantryOmniBCommand& deformableInfantry_command, - std::string serial_filter = {}) + std::string serial_filter = {}, bool has_external_imu_board = false) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , hard_sync_pending_(deformableInfantry.hard_sync_pending_) + , has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") @@ -626,8 +683,9 @@ class DeformableInfantryOmniB deformableInfantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + if (!has_external_imu_board_) + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); bmi088_.set_coordinate_mapping([](double x, double y, double z) { // Top board BMI088 maps to gimbal frame as (-x, -y, z). @@ -651,22 +709,25 @@ class DeformableInfantryOmniB scope_motor_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); + if (!has_external_imu_board_) { + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> + // OdomImu, so use the encoder pitch from the TF tree to move the + // yaw-link pose back into PitchLink. + tf_->set_transform( + pitch_link_to_odom_imu); + } tf_->set_state( pitch_encoder_angle); @@ -723,6 +784,7 @@ class DeformableInfantryOmniB } std::atomic& hard_sync_pending_; + bool has_external_imu_board_ = false; OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; @@ -765,6 +827,7 @@ class DeformableInfantryOmniB std::shared_ptr deformable_infantry_command_; std::unique_ptr rmcs_board_lite; + std::unique_ptr imu_board_; std::unique_ptr top_board_; std::shared_ptr> status_service_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 8e7a9393..5320d1df 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -97,13 +97,13 @@ class DeformableInfantry status_ring_.update_battery_power(*chassis_voltage_); status_ring_.update_auto_aim_enable(mouse_->right == 1); - pitch_hud_.update( - (gimbal_pitch_angle_.ready() && std::isfinite(*gimbal_pitch_angle_)) - ? *gimbal_pitch_angle_ - : std::numeric_limits::quiet_NaN(), - (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) - ? *chassis_pitch_ - : std::numeric_limits::quiet_NaN()); + // pitch_hud_.update( + // (gimbal_pitch_angle_.ready() && std::isfinite(*gimbal_pitch_angle_)) + // ? *gimbal_pitch_angle_ + // : std::numeric_limits::quiet_NaN(), + // (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) + // ? *chassis_pitch_ + // : std::numeric_limits::quiet_NaN()); } private: From 068ac3366ce8f9f3f9df4089b1b7f33f433d016a Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 15 May 2026 07:04:59 +0800 Subject: [PATCH 13/90] feat: remove unused build and install scripts, update CMakeLists and enhance UI components for deformable infantry --- rmcs_ws/build | 1 - rmcs_ws/install | 1 - rmcs_ws/log | 1 - rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- .../chassis/chassis_power_controller.cpp | 4 +- .../controller/chassis/deformable_chassis.cpp | 99 ++++--- .../referee/app/ui/deformable_infantry_ui.cpp | 49 +++- .../referee/app/ui/widget/animated_toggle.hpp | 74 ++++++ .../app/ui/widget/crosshair_circle.hpp | 3 + .../src/referee/app/ui/widget/pitch_hud.hpp | 243 ++++++------------ 10 files changed, 265 insertions(+), 214 deletions(-) delete mode 120000 rmcs_ws/build delete mode 120000 rmcs_ws/install delete mode 120000 rmcs_ws/log create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp diff --git a/rmcs_ws/build b/rmcs_ws/build deleted file mode 120000 index 2ec6ad5a..00000000 --- a/rmcs_ws/build +++ /dev/null @@ -1 +0,0 @@ -build-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/install b/rmcs_ws/install deleted file mode 120000 index 4ed00624..00000000 --- a/rmcs_ws/install +++ /dev/null @@ -1 +0,0 @@ -install-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/log b/rmcs_ws/log deleted file mode 120000 index 4c7d622b..00000000 --- a/rmcs_ws/log +++ /dev/null @@ -1 +0,0 @@ -log-cross-arm64 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 36e7e570..d275c6db 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0/librmcs-sdk-src-3.1.0.zip - URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0rc0/librmcs-sdk-src-3.2.0-rc.0.zip + URL_HASH SHA256=accefeb3a46d7da32a215765d0907de36c25353dd7951e342aaddfe98d2971e5 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 1725dfa9..ec120e7c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -130,7 +130,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - const double excess_power_limit = supercap_active ? 15.0 : 0.0; + const double excess_power_limit = supercap_active ? 15.0 : 15.0; power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; @@ -155,7 +155,7 @@ class ChassisPowerController InputInterface rotary_knob_; InputInterface chassis_power_; - static constexpr double virtual_buffer_energy_limit_ = 30.0; + static constexpr double virtual_buffer_energy_limit_ = 40.0; double virtual_buffer_energy_; InputInterface supercap_voltage_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index be49b501..8e4aee81 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include "controller/pid/pid_calculator.hpp" @@ -126,11 +125,8 @@ class DeformableChassis roll_pid_.output_limit = roll_diff_limit_; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); @@ -277,11 +273,22 @@ class DeformableChassis rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { auto mode = *mode_; - if (switch_left == rmcs_msgs::Switch::DOWN) + if (switch_left == rmcs_msgs::Switch::DOWN) { + deactivate_complex_spin_(); return; + } - if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE + if (!last_keyboard_.e && keyboard.e) { + if (complex_spin_active_) { + deactivate_complex_spin_(); + if (mode == rmcs_msgs::ChassisMode::SPIN) + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + activate_complex_spin_(mode); + } + } else if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { + deactivate_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { @@ -289,6 +296,7 @@ class DeformableChassis spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { + deactivate_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { @@ -296,17 +304,37 @@ class DeformableChassis spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { + deactivate_complex_spin_(); mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::LAUNCH_RAMP; } else if (!last_keyboard_.z && keyboard.z) { + deactivate_complex_spin_(); mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; } + if (complex_spin_active_) + mode = rmcs_msgs::ChassisMode::SPIN; + *mode_ = mode; } + void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { + complex_spin_active_ = true; + complex_spin_elapsed_ = 0.0; + apply_symmetric_target = true; + if (mode != rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } + + void deactivate_complex_spin_() { + complex_spin_active_ = false; + complex_spin_elapsed_ = 0.0; + } + std::array read_current_joint_physical_angles_() const { const std::array*, kJointCount> physical_angle_inputs{ &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, @@ -349,21 +377,6 @@ class DeformableChassis && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; } - bool refresh_requested_joint_targets_from_deploy_state_() { - requested_target_physical_angles_rad_[kLeftFront] = deg_to_rad(lf_current_target_angle_); - requested_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); - requested_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); - requested_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - - const bool prone_override = prone_override_requested_by_keyboard(); - if (suspension_requested_by_input_()) { - requested_target_physical_angles_rad_.fill(deg_to_rad(min_angle_)); - } - - current_target_physical_angles_rad_ = requested_target_physical_angles_rad_; - return prone_override; - } - void reset_attitude_correction_state_() { pitch_pid_.reset(); roll_pid_.reset(); @@ -435,7 +448,6 @@ class DeformableChassis joint_target_physical_angle_state_rad_ = current_physical_angles; joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - requested_target_physical_angles_rad_ = current_physical_angles; current_target_physical_angles_rad_ = current_physical_angles; joint_target_active_ = true; return true; @@ -508,6 +520,7 @@ class DeformableChassis rf_current_target_angle_ = current_target_angle_; joint_target_active_ = false; suspension_on_by_switch = false; + deactivate_complex_spin_(); *scope_motor_control_torque = nan_; @@ -619,6 +632,7 @@ class DeformableChassis void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { constexpr double rotary_knob_edge_threshold = 0.7; + constexpr double complex_spin_toggle_period = 0.5; const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; const bool rotary_knob_toggle_condition = @@ -626,6 +640,17 @@ class DeformableChassis && *rotary_knob_ >= rotary_knob_edge_threshold; const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + bool complex_spin_toggle_condition = false; + + if (complex_spin_active_) { + complex_spin_elapsed_ += update_dt(); + size_t complex_spin_toggle_count = 0; + while (complex_spin_elapsed_ >= complex_spin_toggle_period) { + complex_spin_elapsed_ -= complex_spin_toggle_period; + ++complex_spin_toggle_count; + } + complex_spin_toggle_condition = (complex_spin_toggle_count % 2) == 1; + } if (apply_symmetric_target) { lf_current_target_angle_ = current_target_angle_; @@ -634,7 +659,7 @@ class DeformableChassis rf_current_target_angle_ = current_target_angle_; } - if (rotary_knob_toggle_condition || keyboard_toggle_condition) { + if (rotary_knob_toggle_condition || keyboard_toggle_condition || complex_spin_toggle_condition) { current_target_angle_ = (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; apply_symmetric_target = true; @@ -659,6 +684,7 @@ class DeformableChassis // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. void run_joint_intent_pipeline_() { const auto current_physical_angles = read_current_joint_physical_angles_(); + const bool suspension_requested = suspension_requested_by_input_(); if (!joint_target_active_ && !initialize_joint_target_states_from_feedback(current_physical_angles)) { @@ -666,9 +692,16 @@ class DeformableChassis return; } + current_target_physical_angles_rad_[kLeftFront] = deg_to_rad(lf_current_target_angle_); + current_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); + current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); + current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); + if (suspension_requested) { + current_target_physical_angles_rad_.fill(deg_to_rad(min_angle_)); + } + update_chassis_imu_calibration_(); - const bool prone_override = refresh_requested_joint_targets_from_deploy_state_(); - scope_motor_control(prone_override); + scope_motor_control(suspension_requested); update_active_suspension_(); update_joint_target_trajectory(); @@ -677,18 +710,12 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - void scope_motor_control(bool prone_override = false) { - const bool prone_target_active = prone_override; + void scope_motor_control(bool suspension_requested = false) { + const bool prone_target_active = suspension_requested; if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { *scope_motor_control_torque = -0.3; - // if (*scope_motor_velocity <= std::abs(0.1)){ - // *scope_motor_control_torque = 0.18 * 1.0 / 36.0; - // } } else { *scope_motor_control_torque = 0.3; - // if (*scope_motor_velocity <= std::abs(0.1)){ - // *scope_motor_control_torque = -0.18 * 1.0 / 36.0; - // } } } @@ -816,11 +843,8 @@ class DeformableChassis private: InputInterface joystick_right_; - InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; InputInterface keyboard_; InputInterface rotary_knob_; InputInterface update_rate_; @@ -838,6 +862,8 @@ class DeformableChassis bool spinning_forward_ = true; bool apply_symmetric_target = true; + bool complex_spin_active_ = false; + double complex_spin_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; @@ -877,7 +903,6 @@ class DeformableChassis double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, rf_current_target_angle_; - std::array requested_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; bool joint_target_active_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 5320d1df..5bb326e2 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -8,9 +9,11 @@ #include #include #include +#include #include #include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/animated_toggle.hpp" #include "referee/app/ui/widget/crosshair_circle.hpp" #include "referee/app/ui/widget/deformable_chassis_top_view.hpp" #include "referee/app/ui/widget/pitch_hud.hpp" @@ -25,7 +28,7 @@ class DeformableInfantry public: DeformableInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , crosshair_circle_(Shape::Color::WHITE, x_center - 14, y_center - 42, 8, 2) , status_ring_(26.5, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, @@ -34,7 +37,14 @@ class DeformableInfantry {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) - , pitch_hud_(PitchHud::Config{1540, y_center, 180, 30.0, 5.0}) + , pitch_hud_(PitchHud::Config{ + .center_x = x_center, + .center_y = y_center, + .radius_px = 95, + .width_px = 12, + .half_span_deg = 30.0, + .warning_pitch_deg = 5.0, + }) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { double deformable_leg_min_angle_deg = 8.0; @@ -48,6 +58,7 @@ class DeformableInfantry deformable_chassis_leg_arcs_.set_angle_range( deformable_leg_min_angle_deg, deformable_leg_max_angle_deg); + register_input("/predefined/timestamp", timestamp_); register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); @@ -79,15 +90,20 @@ class DeformableInfantry register_input("/gimbal/right_friction/velocity", right_friction_velocity_); register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_, false); register_input("/referee/game/stage", game_stage_); + + ctrl_transition_.reset(false); + pitch_hud_.set_visible(false); } void update() override { update_chassis_direction_indicator(); update_deformable_chassis_leg_arcs(); + update_ctrl_ui(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); status_ring_.update_friction_wheel_speed( @@ -97,16 +113,23 @@ class DeformableInfantry status_ring_.update_battery_power(*chassis_voltage_); status_ring_.update_auto_aim_enable(mouse_->right == 1); - // pitch_hud_.update( - // (gimbal_pitch_angle_.ready() && std::isfinite(*gimbal_pitch_angle_)) - // ? *gimbal_pitch_angle_ - // : std::numeric_limits::quiet_NaN(), - // (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) - // ? *chassis_pitch_ - // : std::numeric_limits::quiet_NaN()); } private: + void update_ctrl_ui() { + const bool ctrl_active = keyboard_.ready() && keyboard_->ctrl; + const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); + + crosshair_circle_.set_x(static_cast(std::lround( + static_cast(crosshair_base_x_) + 20.0 * reveal))); + + // const double display_pitch = + // (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) + // ? -*chassis_pitch_ + // : std::numeric_limits::quiet_NaN(); + // pitch_hud_.update(display_pitch, reveal); + } + void update_time_reminder() { if (!game_stage_.ready()) return; @@ -127,8 +150,8 @@ class DeformableInfantry switch (mode) { case rmcs_msgs::ChassisMode::SPIN: return Shape::Color::GREEN; case rmcs_msgs::ChassisMode::AUTO: return Shape::Color::CYAN; - case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::WHITE; - default: return Shape::Color::PINK; + case rmcs_msgs::ChassisMode::STEP_DOWN: return Shape::Color::PINK; + default: return Shape::Color::WHITE; } } @@ -151,7 +174,9 @@ class DeformableInfantry static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr uint16_t crosshair_base_x_ = x_center - 2; + InputInterface timestamp_; InputInterface chassis_mode_; InputInterface chassis_angle_; @@ -172,6 +197,7 @@ class DeformableInfantry InputInterface right_friction_velocity_; InputInterface mouse_; + InputInterface keyboard_; InputInterface gimbal_pitch_angle_; InputInterface chassis_pitch_; @@ -188,6 +214,7 @@ class DeformableInfantry DeformableChassisLegArcs deformable_chassis_leg_arcs_; PitchHud pitch_hud_; + AnimatedToggle ctrl_transition_{}; Integer time_reminder_; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp new file mode 100644 index 00000000..8409612d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/animated_toggle.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::referee::app::ui { + +class AnimatedToggle { +public: + using Clock = std::chrono::steady_clock; + + explicit AnimatedToggle(std::chrono::duration duration = std::chrono::duration{0.5}) + : duration_(duration) {} + + void set_duration(std::chrono::duration duration) { + duration_ = std::max(duration, std::chrono::duration{0.0}); + } + + void reset(bool active) { + initialized_ = false; + value_ = active ? 1.0 : 0.0; + target_ = active; + } + + double update(Clock::time_point now, bool active) { + if (!initialized_) { + initialized_ = true; + start_time_ = now; + start_value_ = active ? 1.0 : 0.0; + end_value_ = start_value_; + value_ = start_value_; + target_ = active; + return value_; + } + + if (active != target_) { + target_ = active; + start_value_ = value_; + end_value_ = active ? 1.0 : 0.0; + start_time_ = now; + } + + if (duration_.count() <= 0.0) { + value_ = end_value_; + return value_; + } + + const double elapsed = std::chrono::duration{now - start_time_}.count(); + const double t = std::clamp(elapsed / duration_.count(), 0.0, 1.0); + value_ = std::lerp(start_value_, end_value_, ease_in_out_cubic_(t)); + return value_; + } + + double value() const { return value_; } + +private: + static double ease_in_out_cubic_(double t) { + t = std::clamp(t, 0.0, 1.0); + if (t < 0.5) + return 4.0 * t * t * t; + return 1.0 - std::pow(-2.0 * t + 2.0, 3.0) / 2.0; + } + + std::chrono::duration duration_; + Clock::time_point start_time_{}; + double start_value_ = 0.0; + double end_value_ = 0.0; + double value_ = 0.0; + bool initialized_ = false; + bool target_ = false; +}; + +} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp index 21526b9b..b96b7cb4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -15,8 +15,11 @@ class CrossHairCircle { : circle_(color, width, x, y, r, r, visible) {} void set_visible(bool value) { circle_.set_visible(value); } + void set_color(Shape::Color color) { circle_.set_color(color); } void set_r(uint16_t r) { circle_.set_r(r); } void set_width(uint16_t width) { circle_.set_width(width); } + void set_x(uint16_t x) { circle_.set_x(x); } + void set_y(uint16_t y) { circle_.set_y(y); } private: Circle circle_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp index ebeea1e4..37286116 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp @@ -1,10 +1,8 @@ #pragma once #include -#include #include #include -#include #include "referee/app/ui/shape/shape.hpp" @@ -13,11 +11,12 @@ namespace rmcs_core::referee::app::ui { class PitchHud { public: struct Config { - uint16_t center_x = 1540; + uint16_t center_x = x_center; uint16_t center_y = y_center; - uint16_t half_height_px = 180; - double half_span_deg = 30.0; - double tick_step_deg = 5.0; + uint16_t radius_px = 150; + uint16_t width_px = 12; + double half_span_deg = 22.0; + double warning_pitch_deg = 5.0; }; PitchHud() { set_config(Config{}); } @@ -25,183 +24,109 @@ class PitchHud { explicit PitchHud(Config config) { set_config(config); } void set_config(Config config) { - config.tick_step_deg = std::max(config.tick_step_deg, 1.0); - config.half_span_deg = std::max(config.half_span_deg, config.tick_step_deg); - config.half_height_px = std::max(config.half_height_px, 40); + config.radius_px = std::max(config.radius_px, 1); + config.width_px = std::max(config.width_px, 1); + config.half_span_deg = std::max(config.half_span_deg, 1.0); + config.warning_pitch_deg = std::max(config.warning_pitch_deg, 0.0); config_ = config; initialize_(); } - void update(double gimbal_pitch, double chassis_pitch) { - if (std::isfinite(gimbal_pitch)) - update_gimbal_pitch_indicator_(pitch_to_hud_y_(gimbal_pitch)); - else - set_indicator_visible_(gimbal_pitch_indicator_, false); - - if (std::isfinite(chassis_pitch)) - update_chassis_pitch_indicator_(pitch_to_hud_y_(chassis_pitch)); - else - set_indicator_visible_(chassis_pitch_indicator_, false); + void set_visible(bool visible) { + background_arc_.set_visible(visible); + positive_fill_arc_.set_visible(visible && positive_fill_visible_); + negative_fill_arc_.set_visible(visible && negative_fill_visible_); } -private: - static constexpr std::size_t tick_capacity_ = 25; - - void initialize_() { - tick_count_ = std::clamp( - static_cast( - std::floor(2.0 * config_.half_span_deg / config_.tick_step_deg + 1.0e-6)) - + 1, - std::size_t{1}, tick_capacity_); - - axis_.set_color(Shape::Color::YELLOW); - axis_.set_width(2); - axis_.set_x(config_.center_x); - axis_.set_y(axis_top_y_()); - axis_.set_x2(config_.center_x); - axis_.set_y2(axis_bottom_y_()); - axis_.set_visible(true); - - for (std::size_t i = 0; i < ticks_.size(); ++i) { - auto& tick = ticks_[i]; - if (i >= tick_count_) { - tick.set_visible(false); - continue; - } - - const double tick_deg = -config_.half_span_deg + static_cast(i) * config_.tick_step_deg; - const int rounded_tick_deg = static_cast(std::lround(tick_deg)); - if (rounded_tick_deg == 0) { - tick.set_visible(false); - continue; - } - - const bool is_major = (std::abs(rounded_tick_deg) % 10 == 0); - const uint16_t tick_length = is_major ? 18 : 10; - const uint16_t tick_y = pitch_to_hud_y_(deg_to_rad_(tick_deg)); - - tick.set_color(Shape::Color::YELLOW); - tick.set_width(2); - tick.set_x(config_.center_x); - tick.set_y(tick_y); - tick.set_x2(config_.center_x + tick_length); - tick.set_y2(tick_y); - tick.set_visible(true); + void update(double pitch_rad, double reveal = 1.0) { + reveal = std::clamp(reveal, 0.0, 1.0); + if (!std::isfinite(pitch_rad) || reveal <= 1.0e-4) { + positive_fill_visible_ = false; + negative_fill_visible_ = false; + set_visible(false); + return; } - for (auto& line : gimbal_pitch_indicator_) { - line.set_color(Shape::Color::YELLOW); - line.set_width(2); - line.set_visible(false); - } - for (auto& line : chassis_pitch_indicator_) { - line.set_color(Shape::Color::YELLOW); - line.set_width(2); - line.set_visible(false); - } - } + apply_geometry_(reveal); - uint16_t axis_top_y_() const { - return clamp_to_screen_y_( - static_cast(config_.center_y) - static_cast(config_.half_height_px)); - } + const double visible_half_span_deg = config_.half_span_deg * reveal; + const double pitch_deg = + std::clamp(rad_to_deg_(pitch_rad), -visible_half_span_deg, visible_half_span_deg); + const double fill_deg = std::abs(pitch_deg); - uint16_t axis_bottom_y_() const { - return clamp_to_screen_y_( - static_cast(config_.center_y) + static_cast(config_.half_height_px)); - } + const bool warning = pitch_deg > config_.warning_pitch_deg; + const auto fill_color = warning ? Shape::Color::PINK : Shape::Color::YELLOW; + positive_fill_arc_.set_color(fill_color); + negative_fill_arc_.set_color(fill_color); + + positive_fill_visible_ = pitch_deg >= 0.0 && fill_deg > 1.0e-4; + negative_fill_visible_ = pitch_deg < 0.0 && fill_deg > 1.0e-4; + + positive_fill_arc_.set_angle_start(to_referee_angle_(pitch_deg)); + positive_fill_arc_.set_angle_end(mid_angle_()); + + negative_fill_arc_.set_angle_start(mid_angle_()); + negative_fill_arc_.set_angle_end(to_referee_angle_(pitch_deg)); - static constexpr double deg_to_rad_(double degrees) { - return degrees * std::numbers::pi / 180.0; + set_visible(true); } - static uint16_t clamp_to_screen_y_(double y) { - return static_cast(std::clamp( - std::lround(y), 0l, static_cast(screen_height - 1))); +private: + void initialize_() { + background_arc_.set_color(Shape::Color::ORANGE); + positive_fill_arc_.set_color(Shape::Color::YELLOW); + negative_fill_arc_.set_color(Shape::Color::YELLOW); + positive_fill_visible_ = false; + negative_fill_visible_ = false; + set_visible(false); } - uint16_t pitch_to_hud_y_(double pitch_rad) const { - const double clamped_pitch = - std::clamp(pitch_rad, -deg_to_rad_(config_.half_span_deg), - deg_to_rad_(config_.half_span_deg)); - const double normalized = clamped_pitch / deg_to_rad_(config_.half_span_deg); - return clamp_to_screen_y_( - static_cast(config_.center_y) - + normalized * static_cast(config_.half_height_px)); + void apply_geometry_(double reveal) { + const uint16_t width = std::max(1, static_cast(std::lround( + static_cast(config_.width_px) * reveal))); + const double half_span_deg = config_.half_span_deg * reveal; + + background_arc_.set_x(config_.center_x); + background_arc_.set_y(config_.center_y); + background_arc_.set_r(config_.radius_px); + background_arc_.set_width(width); + background_arc_.set_angle_start(to_referee_angle_(-half_span_deg)); + background_arc_.set_angle_end(to_referee_angle_(half_span_deg)); + + positive_fill_arc_.set_x(config_.center_x); + positive_fill_arc_.set_y(config_.center_y); + positive_fill_arc_.set_r(config_.radius_px); + positive_fill_arc_.set_width(width); + + negative_fill_arc_.set_x(config_.center_x); + negative_fill_arc_.set_y(config_.center_y); + negative_fill_arc_.set_r(config_.radius_px); + negative_fill_arc_.set_width(width); } - void update_gimbal_pitch_indicator_(uint16_t y) { - const uint16_t tip_x = config_.center_x - 8; - const uint16_t back_x = config_.center_x - 20; - constexpr uint16_t half_height = 8; - const uint16_t top_y = clamp_to_screen_y_(static_cast(y) - half_height); - const uint16_t bottom_y = clamp_to_screen_y_(static_cast(y) + half_height); - - gimbal_pitch_indicator_[0].set_x(back_x); - gimbal_pitch_indicator_[0].set_y(top_y); - gimbal_pitch_indicator_[0].set_x2(tip_x); - gimbal_pitch_indicator_[0].set_y2(y); - - gimbal_pitch_indicator_[1].set_x(back_x); - gimbal_pitch_indicator_[1].set_y(bottom_y); - gimbal_pitch_indicator_[1].set_x2(tip_x); - gimbal_pitch_indicator_[1].set_y2(y); - - gimbal_pitch_indicator_[2].set_x(back_x); - gimbal_pitch_indicator_[2].set_y(top_y); - gimbal_pitch_indicator_[2].set_x2(back_x); - gimbal_pitch_indicator_[2].set_y2(bottom_y); - - set_indicator_visible_(gimbal_pitch_indicator_, true); + static constexpr double rad_to_deg_(double radians) { + return radians * 180.0 / 3.14159265358979323846; } - void update_chassis_pitch_indicator_(uint16_t y) { - const uint16_t front_x = config_.center_x - 20; - const uint16_t rear_x = config_.center_x - 34; - constexpr uint16_t front_half_height = 12; - constexpr uint16_t rear_half_height = 8; - const uint16_t front_top_y = clamp_to_screen_y_(static_cast(y) - front_half_height); - const uint16_t front_bottom_y = - clamp_to_screen_y_(static_cast(y) + front_half_height); - const uint16_t rear_top_y = clamp_to_screen_y_(static_cast(y) - rear_half_height); - const uint16_t rear_bottom_y = - clamp_to_screen_y_(static_cast(y) + rear_half_height); - - chassis_pitch_indicator_[0].set_x(rear_x); - chassis_pitch_indicator_[0].set_y(rear_top_y); - chassis_pitch_indicator_[0].set_x2(front_x); - chassis_pitch_indicator_[0].set_y2(front_top_y); - - chassis_pitch_indicator_[1].set_x(front_x); - chassis_pitch_indicator_[1].set_y(front_top_y); - chassis_pitch_indicator_[1].set_x2(front_x); - chassis_pitch_indicator_[1].set_y2(front_bottom_y); - - chassis_pitch_indicator_[2].set_x(front_x); - chassis_pitch_indicator_[2].set_y(front_bottom_y); - chassis_pitch_indicator_[2].set_x2(rear_x); - chassis_pitch_indicator_[2].set_y2(rear_bottom_y); - - chassis_pitch_indicator_[3].set_x(rear_x); - chassis_pitch_indicator_[3].set_y(rear_bottom_y); - chassis_pitch_indicator_[3].set_x2(rear_x); - chassis_pitch_indicator_[3].set_y2(rear_top_y); - - set_indicator_visible_(chassis_pitch_indicator_, true); + static uint16_t wrap_angle_(long angle) { + angle %= 360; + if (angle < 0) + angle += 360; + return static_cast(angle); } - template - static void set_indicator_visible_(std::array& indicator, bool visible) { - for (auto& line : indicator) - line.set_visible(visible); + static constexpr uint16_t mid_angle_() { return 180; } + + static uint16_t to_referee_angle_(double display_pitch_deg) { + return wrap_angle_(std::lround(static_cast(mid_angle_()) + display_pitch_deg)); } Config config_{}; - std::size_t tick_count_ = 0; - Line axis_; - std::array ticks_; - std::array gimbal_pitch_indicator_; - std::array chassis_pitch_indicator_; + Arc background_arc_{Shape::Color::WHITE, 1, 0, 0, 0, 0, 1, 1, false}; + Arc positive_fill_arc_{Shape::Color::YELLOW, 1, 0, 0, 0, 0, 1, 1, false}; + Arc negative_fill_arc_{Shape::Color::YELLOW, 1, 0, 0, 0, 0, 1, 1, false}; + bool positive_fill_visible_ = false; + bool negative_fill_visible_ = false; }; } // namespace rmcs_core::referee::app::ui From 5ac682411cef708283ce30703d5a25c3cda8f5e0 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 15 May 2026 07:57:39 +0800 Subject: [PATCH 14/90] feat: update supercap command transmission in deformable infantry hardware --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../controller/chassis/chassis_power_controller.cpp | 2 +- .../src/hardware/deformable-infantry-omni-b.cpp | 11 +++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 11 +++++++++++ 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 8f25103a..968a36c3 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 8f25103a42fd601bd8259a3da51ad8ef870a5b6b +Subproject commit 968a36c359c654e00fb9084ab1e5551810a19233 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index ec120e7c..22be1ef3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -155,7 +155,7 @@ class ChassisPowerController InputInterface rotary_knob_; InputInterface chassis_power_; - static constexpr double virtual_buffer_energy_limit_ = 40.0; + static constexpr double virtual_buffer_energy_limit_ = 30.0; double virtual_buffer_energy_; InputInterface supercap_voltage_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 9be7189a..b2d55992 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -314,6 +314,17 @@ class DeformableInfantryOmniB .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { builder.can0_transmit({ .can_id = 0x141, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 561c22c0..5dfbcc6a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -296,6 +296,17 @@ class DeformableInfantryOmni } .as_bytes(), }); + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); } else { builder.can0_transmit({ .can_id = 0x141, From ac7aaae9c4cf555e4dd1c8c471da9a76ffb342df Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 16 May 2026 13:48:57 +0800 Subject: [PATCH 15/90] Refactor Deformable Infantry UI and Remove Pitch HUD - Removed unnecessary includes and the PitchHud widget from deformable_infantry_ui.cpp. - Adjusted crosshair_circle positioning in the DeformableInfantry constructor. - Updated crosshair_circle's x and y coordinates based on new calculations. - Removed unused input registration for chassis_pitch_. - Added getter methods for x and y coordinates in the CrossHairCircle class. - Introduced a new Handler class for managing USB communication in librmcs. - Implemented a transport layer for USB communication, including buffer management and event handling. - Added error handling and logging for USB transport failures. - Created a PacketBuilder class for building packets to be sent over USB. - Established a reconnect mechanism for USB transport to handle connection issues. --- .../config/deformable-infantry-omni-b.yaml | 6 +- .../config/deformable-infantry-omni.yaml | 4 +- rmcs_ws/src/rmcs_core/CMakeLists.txt | 21 + .../chassis/chassis_power_controller.cpp | 5 +- .../controller/chassis/deformable_chassis.cpp | 214 ++++++++-- .../referee/app/ui/deformable_infantry_ui.cpp | 34 +- .../app/ui/widget/crosshair_circle.hpp | 2 + .../src/referee/app/ui/widget/pitch_hud.hpp | 132 ------ .../host/include/librmcs/protocol/handler.hpp | 61 +++ .../librmcs/host/src/protocol/handler.cpp | 390 +++++++++++++++++ .../librmcs/host/src/transport/transport.hpp | 56 +++ .../librmcs/host/src/transport/usb/usb.cpp | 398 ++++++++++++++++++ 12 files changed, 1126 insertions(+), 197 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp create mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp create mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp create mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp create mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 0b28cf37..a235d389 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -41,14 +41,14 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 right_front_zero_point: 6485 - yaw_motor_zero_point: 25406 + yaw_motor_zero_point: 18211 pitch_motor_zero_point: 56354 debug_log_supercap: false debug_log_wheel_motor: false @@ -57,7 +57,7 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 7.0 + min_angle: 8.0 max_angle: 58.0 active_suspension_enable: true spin_ratio: 1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 84bfd1e3..3885fdd2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -40,14 +40,14 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" + serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 right_front_zero_point: 7136 - yaw_motor_zero_point: 50642 + yaw_motor_zero_point: 31856 pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d275c6db..89fbdb4f 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -23,6 +23,27 @@ FetchContent_Declare( ) FetchContent_MakeAvailable(librmcs) +configure_file( + ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp + ${librmcs_SOURCE_DIR}/host/include/librmcs/protocol/handler.hpp + COPYONLY +) +configure_file( + ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/protocol/handler.cpp + ${librmcs_SOURCE_DIR}/host/src/protocol/handler.cpp + COPYONLY +) +configure_file( + ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/transport/transport.hpp + ${librmcs_SOURCE_DIR}/host/src/transport/transport.hpp + COPYONLY +) +configure_file( + ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp + ${librmcs_SOURCE_DIR}/host/src/transport/usb/usb.cpp + COPYONLY +) + file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp ${PROJECT_SOURCE_DIR}/src/*.c diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 22be1ef3..d4988239 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -109,9 +109,8 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - const bool supercap_active = *supercap_control_enabled_ && *supercap_enabled_; - if (supercap_active) + if (*supercap_control_enabled_ && *supercap_enabled_) power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP ? inf_ : *chassis_power_limit_referee_ + 80.0; @@ -130,7 +129,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - const double excess_power_limit = supercap_active ? 15.0 : 15.0; + constexpr double excess_power_limit = 15; power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 8e4aee81..1473f296 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -5,12 +5,14 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -62,8 +64,8 @@ class DeformableChassis , following_velocity_controller_(10.0, 0.0, 0.0) , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) - , min_angle_(get_parameter_or("min_angle", 15.0)) - , max_angle_(get_parameter_or("max_angle", 55.0)) + , min_angle_(get_parameter_or("min_angle", 7.0)) + , max_angle_(get_parameter_or("max_angle", 58.0)) , target_physical_velocity_limit_( std::max( deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), @@ -130,6 +132,7 @@ class DeformableChassis register_input("/remote/keyboard", keyboard_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); + register_input("/tf", tf_, false); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); @@ -259,6 +262,8 @@ class DeformableChassis static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; + static constexpr double imu_calibration_offset_limit_rad_ = + 1.0 * std::numbers::pi / 180.0; void validate_joint_feedback_inputs() const { if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() @@ -273,12 +278,36 @@ class DeformableChassis rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { auto mode = *mode_; + const bool q_pressed = keyboard.q; + const bool e_pressed = keyboard.e; + const bool last_q_pressed = last_keyboard_.q; + const bool last_e_pressed = last_keyboard_.e; + const bool last_c_pressed = last_keyboard_.c; + const bool qe_combo_pressed = q_pressed && e_pressed; + const bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; + const bool e_rising = !last_e_pressed && e_pressed; + const bool c_rising = !last_c_pressed && keyboard.c; + const bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; if (switch_left == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); return; } - if (!last_keyboard_.e && keyboard.e) { + if (qe_complex_spin_active_) { + if (c_rising) { + deactivate_qe_complex_spin_(); + apply_symmetric_target = true; + lf_current_target_angle_ = current_target_angle_; + lb_current_target_angle_ = current_target_angle_; + rb_current_target_angle_ = current_target_angle_; + rf_current_target_angle_ = current_target_angle_; + mode = rmcs_msgs::ChassisMode::SPIN; + } + } else if (qe_combo_rising) { + deactivate_complex_spin_(); + activate_qe_complex_spin_(mode); + } else if (e_rising && !q_pressed) { if (complex_spin_active_) { deactivate_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) @@ -289,6 +318,7 @@ class DeformableChassis } else if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { @@ -297,6 +327,7 @@ class DeformableChassis } } else if (!last_keyboard_.c && keyboard.c) { deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { @@ -305,16 +336,18 @@ class DeformableChassis } } else if (!last_keyboard_.x && keyboard.x) { deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::LAUNCH_RAMP; } else if (!last_keyboard_.z && keyboard.z) { deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; } - if (complex_spin_active_) + if (complex_spin_active_ || qe_complex_spin_active_) mode = rmcs_msgs::ChassisMode::SPIN; *mode_ = mode; @@ -335,6 +368,109 @@ class DeformableChassis complex_spin_elapsed_ = 0.0; } + void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { + qe_complex_spin_active_ = true; + qe_last_toggle_elapsed_ = 0.0; + qe_front_window_armed_ = true; + qe_rear_window_armed_ = true; + qe_front_high_rear_low_ = true; + apply_front_high_rear_low_target_(); + if (mode != rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } + + void deactivate_qe_complex_spin_() { + qe_complex_spin_active_ = false; + qe_last_toggle_elapsed_ = 0.0; + qe_front_window_armed_ = true; + qe_rear_window_armed_ = true; + } + + void apply_front_high_rear_low_target_() { + lf_current_target_angle_ = max_angle_; + rf_current_target_angle_ = max_angle_; + lb_current_target_angle_ = min_angle_; + rb_current_target_angle_ = min_angle_; + apply_symmetric_target = false; + } + + void apply_front_low_rear_high_target_() { + lf_current_target_angle_ = min_angle_; + rf_current_target_angle_ = min_angle_; + lb_current_target_angle_ = max_angle_; + rb_current_target_angle_ = max_angle_; + apply_symmetric_target = false; + } + + void toggle_qe_complex_spin_target_() { + qe_front_high_rear_low_ = !qe_front_high_rear_low_; + if (qe_front_high_rear_low_) { + apply_front_high_rear_low_target_(); + } else { + apply_front_low_rear_high_target_(); + } + qe_last_toggle_elapsed_ = 0.0; + } + + static double wrap_to_pi_(double angle) { + while (angle > std::numbers::pi) + angle -= 2.0 * std::numbers::pi; + while (angle < -std::numbers::pi) + angle += 2.0 * std::numbers::pi; + return angle; + } + + static double heading_from_direction_(const Eigen::Vector3d& direction) { + return std::atan2(direction.y(), direction.x()); + } + + std::optional> read_qe_spin_headings_() const { + if (!tf_.ready()) + return std::nullopt; + + const auto chassis_direction = fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + const auto gimbal_direction = fast_tf::cast( + rmcs_description::YawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + + return std::pair{ + heading_from_direction_(*chassis_direction), heading_from_direction_(*gimbal_direction)}; + } + + void update_qe_complex_spin_toggle_() { + constexpr double heading_trigger_threshold = 20.0 * std::numbers::pi / 180.0; + constexpr double heading_rearm_threshold = 40.0 * std::numbers::pi / 180.0; + constexpr double force_toggle_timeout = 0.25; + + qe_last_toggle_elapsed_ += update_dt(); + + bool should_toggle = false; + if (const auto headings = read_qe_spin_headings_()) { + const auto& [chassis_heading, gimbal_heading] = *headings; + const double relative_heading = wrap_to_pi_(chassis_heading - gimbal_heading); + const double front_error = std::abs(relative_heading); + const double rear_error = std::abs(std::abs(relative_heading) - std::numbers::pi); + + if (front_error >= heading_rearm_threshold) + qe_front_window_armed_ = true; + if (rear_error >= heading_rearm_threshold) + qe_rear_window_armed_ = true; + + if (front_error <= heading_trigger_threshold && qe_front_window_armed_) { + should_toggle = true; + qe_front_window_armed_ = false; + } else if (rear_error <= heading_trigger_threshold && qe_rear_window_armed_) { + should_toggle = true; + qe_rear_window_armed_ = false; + } + } + + if (should_toggle || qe_last_toggle_elapsed_ >= force_toggle_timeout) + toggle_qe_complex_spin_target_(); + } + std::array read_current_joint_physical_angles_() const { const std::array*, kJointCount> physical_angle_inputs{ &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, @@ -392,6 +528,9 @@ class DeformableChassis } void update_chassis_imu_calibration_() { + if (chassis_imu_calibrated_once_) + return; + if (!symmetric_joint_target_requested_()) { reset_chassis_imu_calibration_window_(); return; @@ -426,10 +565,13 @@ class DeformableChassis return; } - chassis_imu_pitch_offset_ = - chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_); - chassis_imu_roll_offset_ = - chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_); + chassis_imu_pitch_offset_ = std::clamp( + chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_), + -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); + chassis_imu_roll_offset_ = std::clamp( + chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_), + -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); + chassis_imu_calibrated_once_ = true; RCLCPP_INFO( get_logger(), "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " @@ -453,6 +595,10 @@ class DeformableChassis return true; } + double active_suspension_min_angle_rad_() const { + return deg_to_rad(min_angle_ - 5.0); + } + void update_active_suspension_() { if (!suspension_requested_by_input_()) { reset_attitude_correction_state_(); @@ -460,7 +606,7 @@ class DeformableChassis } constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - const double base_target_angle = deg_to_rad(min_angle_); + const double base_target_angle = active_suspension_min_angle_rad_(); const double max_target_angle = deg_to_rad(max_angle_); const double corrected_pitch = std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); @@ -481,24 +627,28 @@ class DeformableChassis } // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left - // pair. Every leg starts from min_angle and only receives additive corrections so at least - // one leg always stays at min_angle. + // pair. Every leg starts from the active-suspension minimum and only receives additive + // corrections so at least one leg always stays at that minimum. const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); const double back_pitch_add = std::max(pitch_angle_diff, 0.0); const double left_roll_add = std::max(roll_angle_diff, 0.0); const double right_roll_add = std::max(-roll_angle_diff, 0.0); current_target_physical_angles_rad_[kLeftFront] = - std::clamp(base_target_angle + front_pitch_add + left_roll_add, base_target_angle, + std::clamp( + base_target_angle + front_pitch_add + left_roll_add, base_target_angle, max_target_angle); current_target_physical_angles_rad_[kLeftBack] = - std::clamp(base_target_angle + back_pitch_add + left_roll_add, base_target_angle, + std::clamp( + base_target_angle + back_pitch_add + left_roll_add, base_target_angle, max_target_angle); current_target_physical_angles_rad_[kRightBack] = - std::clamp(base_target_angle + back_pitch_add + right_roll_add, base_target_angle, + std::clamp( + base_target_angle + back_pitch_add + right_roll_add, base_target_angle, max_target_angle); current_target_physical_angles_rad_[kRightFront] = - std::clamp(base_target_angle + front_pitch_add + right_roll_add, base_target_angle, + std::clamp( + base_target_angle + front_pitch_add + right_roll_add, base_target_angle, max_target_angle); joint_suspension_active_.fill(true); @@ -521,6 +671,7 @@ class DeformableChassis joint_target_active_ = false; suspension_on_by_switch = false; deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); *scope_motor_control_torque = nan_; @@ -632,14 +783,15 @@ class DeformableChassis void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { constexpr double rotary_knob_edge_threshold = 0.7; - constexpr double complex_spin_toggle_period = 0.5; + constexpr double complex_spin_toggle_period = 0.2; - const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; + const bool keyboard_toggle_condition = + !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q && !keyboard.e; const bool rotary_knob_toggle_condition = last_rotary_knob_ < rotary_knob_edge_threshold && *rotary_knob_ >= rotary_knob_edge_threshold; - const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; + const bool front_high_rear_low = !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; bool complex_spin_toggle_condition = false; if (complex_spin_active_) { @@ -652,6 +804,9 @@ class DeformableChassis complex_spin_toggle_condition = (complex_spin_toggle_count % 2) == 1; } + if (qe_complex_spin_active_) + update_qe_complex_spin_toggle_(); + if (apply_symmetric_target) { lf_current_target_angle_ = current_target_angle_; lb_current_target_angle_ = current_target_angle_; @@ -664,17 +819,9 @@ class DeformableChassis (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; apply_symmetric_target = true; } else if (front_high_rear_low) { - lf_current_target_angle_ = max_angle_; - rf_current_target_angle_ = max_angle_; - lb_current_target_angle_ = min_angle_; - rb_current_target_angle_ = min_angle_; - apply_symmetric_target = false; + apply_front_high_rear_low_target_(); } else if (front_low_rear_high) { - lf_current_target_angle_ = min_angle_; - rf_current_target_angle_ = min_angle_; - lb_current_target_angle_ = max_angle_; - rb_current_target_angle_ = max_angle_; - apply_symmetric_target = false; + apply_front_low_rear_high_target_(); } last_rotary_knob_ = *rotary_knob_; @@ -697,7 +844,7 @@ class DeformableChassis current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); if (suspension_requested) { - current_target_physical_angles_rad_.fill(deg_to_rad(min_angle_)); + current_target_physical_angles_rad_.fill(active_suspension_min_angle_rad_()); } update_chassis_imu_calibration_(); @@ -842,6 +989,7 @@ class DeformableChassis } private: + InputInterface tf_; InputInterface joystick_right_; InputInterface switch_right_; InputInterface switch_left_; @@ -864,6 +1012,11 @@ class DeformableChassis bool apply_symmetric_target = true; bool complex_spin_active_ = false; double complex_spin_elapsed_ = 0.0; + bool qe_complex_spin_active_ = false; + bool qe_front_window_armed_ = true; + bool qe_rear_window_armed_ = true; + bool qe_front_high_rear_low_ = true; + double qe_last_toggle_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; @@ -939,6 +1092,7 @@ class DeformableChassis double chassis_imu_pitch_sum_ = 0.0; double chassis_imu_roll_sum_ = 0.0; bool chassis_imu_calibration_completed_for_window_ = false; + bool chassis_imu_calibrated_once_ = false; static constexpr double default_dt_ = 1e-3; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 5bb326e2..398f75d8 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include @@ -16,7 +15,6 @@ #include "referee/app/ui/widget/animated_toggle.hpp" #include "referee/app/ui/widget/crosshair_circle.hpp" #include "referee/app/ui/widget/deformable_chassis_top_view.hpp" -#include "referee/app/ui/widget/pitch_hud.hpp" #include "referee/app/ui/widget/status_ring.hpp" namespace rmcs_core::referee::app::ui { @@ -28,7 +26,7 @@ class DeformableInfantry public: DeformableInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , crosshair_circle_(Shape::Color::WHITE, x_center - 14, y_center - 42, 8, 2) + , crosshair_circle_(Shape::Color::WHITE, x_center - 22, y_center - 42, 8, 2) , status_ring_(26.5, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, @@ -37,14 +35,6 @@ class DeformableInfantry {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) - , pitch_hud_(PitchHud::Config{ - .center_x = x_center, - .center_y = y_center, - .radius_px = 95, - .width_px = 12, - .half_span_deg = 30.0, - .warning_pitch_deg = 5.0, - }) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { double deformable_leg_min_angle_deg = 8.0; @@ -62,8 +52,6 @@ class DeformableInfantry register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); - register_input("/chassis/imu/pitch", chassis_pitch_, false); - register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); @@ -92,12 +80,11 @@ class DeformableInfantry register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_, false); - register_input("/referee/game/stage", game_stage_); + crosshair_base_x_ = crosshair_circle_.x(); + crosshair_base_y_ = crosshair_circle_.y(); ctrl_transition_.reset(false); - pitch_hud_.set_visible(false); } void update() override { @@ -122,12 +109,8 @@ class DeformableInfantry crosshair_circle_.set_x(static_cast(std::lround( static_cast(crosshair_base_x_) + 20.0 * reveal))); - - // const double display_pitch = - // (chassis_pitch_.ready() && std::isfinite(*chassis_pitch_)) - // ? -*chassis_pitch_ - // : std::numeric_limits::quiet_NaN(); - // pitch_hud_.update(display_pitch, reveal); + crosshair_circle_.set_y(static_cast(std::lround( + static_cast(crosshair_base_y_) - 20.0 * reveal))); } void update_time_reminder() { @@ -174,7 +157,6 @@ class DeformableInfantry static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; - static constexpr uint16_t crosshair_base_x_ = x_center - 2; InputInterface timestamp_; InputInterface chassis_mode_; @@ -199,9 +181,6 @@ class DeformableInfantry InputInterface mouse_; InputInterface keyboard_; - InputInterface gimbal_pitch_angle_; - InputInterface chassis_pitch_; - InputInterface game_stage_; CrossHairCircle crosshair_circle_; @@ -213,8 +192,9 @@ class DeformableInfantry Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; - PitchHud pitch_hud_; AnimatedToggle ctrl_transition_{}; + uint16_t crosshair_base_x_ = 0; + uint16_t crosshair_base_y_ = 0; Integer time_reminder_; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp index b96b7cb4..15823c6f 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -20,6 +20,8 @@ class CrossHairCircle { void set_width(uint16_t width) { circle_.set_width(width); } void set_x(uint16_t x) { circle_.set_x(x); } void set_y(uint16_t y) { circle_.set_y(y); } + uint16_t x() const { return circle_.x(); } + uint16_t y() const { return circle_.y(); } private: Circle circle_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp deleted file mode 100644 index 37286116..00000000 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/pitch_hud.hpp +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "referee/app/ui/shape/shape.hpp" - -namespace rmcs_core::referee::app::ui { - -class PitchHud { -public: - struct Config { - uint16_t center_x = x_center; - uint16_t center_y = y_center; - uint16_t radius_px = 150; - uint16_t width_px = 12; - double half_span_deg = 22.0; - double warning_pitch_deg = 5.0; - }; - - PitchHud() { set_config(Config{}); } - - explicit PitchHud(Config config) { set_config(config); } - - void set_config(Config config) { - config.radius_px = std::max(config.radius_px, 1); - config.width_px = std::max(config.width_px, 1); - config.half_span_deg = std::max(config.half_span_deg, 1.0); - config.warning_pitch_deg = std::max(config.warning_pitch_deg, 0.0); - config_ = config; - initialize_(); - } - - void set_visible(bool visible) { - background_arc_.set_visible(visible); - positive_fill_arc_.set_visible(visible && positive_fill_visible_); - negative_fill_arc_.set_visible(visible && negative_fill_visible_); - } - - void update(double pitch_rad, double reveal = 1.0) { - reveal = std::clamp(reveal, 0.0, 1.0); - if (!std::isfinite(pitch_rad) || reveal <= 1.0e-4) { - positive_fill_visible_ = false; - negative_fill_visible_ = false; - set_visible(false); - return; - } - - apply_geometry_(reveal); - - const double visible_half_span_deg = config_.half_span_deg * reveal; - const double pitch_deg = - std::clamp(rad_to_deg_(pitch_rad), -visible_half_span_deg, visible_half_span_deg); - const double fill_deg = std::abs(pitch_deg); - - const bool warning = pitch_deg > config_.warning_pitch_deg; - const auto fill_color = warning ? Shape::Color::PINK : Shape::Color::YELLOW; - positive_fill_arc_.set_color(fill_color); - negative_fill_arc_.set_color(fill_color); - - positive_fill_visible_ = pitch_deg >= 0.0 && fill_deg > 1.0e-4; - negative_fill_visible_ = pitch_deg < 0.0 && fill_deg > 1.0e-4; - - positive_fill_arc_.set_angle_start(to_referee_angle_(pitch_deg)); - positive_fill_arc_.set_angle_end(mid_angle_()); - - negative_fill_arc_.set_angle_start(mid_angle_()); - negative_fill_arc_.set_angle_end(to_referee_angle_(pitch_deg)); - - set_visible(true); - } - -private: - void initialize_() { - background_arc_.set_color(Shape::Color::ORANGE); - positive_fill_arc_.set_color(Shape::Color::YELLOW); - negative_fill_arc_.set_color(Shape::Color::YELLOW); - positive_fill_visible_ = false; - negative_fill_visible_ = false; - set_visible(false); - } - - void apply_geometry_(double reveal) { - const uint16_t width = std::max(1, static_cast(std::lround( - static_cast(config_.width_px) * reveal))); - const double half_span_deg = config_.half_span_deg * reveal; - - background_arc_.set_x(config_.center_x); - background_arc_.set_y(config_.center_y); - background_arc_.set_r(config_.radius_px); - background_arc_.set_width(width); - background_arc_.set_angle_start(to_referee_angle_(-half_span_deg)); - background_arc_.set_angle_end(to_referee_angle_(half_span_deg)); - - positive_fill_arc_.set_x(config_.center_x); - positive_fill_arc_.set_y(config_.center_y); - positive_fill_arc_.set_r(config_.radius_px); - positive_fill_arc_.set_width(width); - - negative_fill_arc_.set_x(config_.center_x); - negative_fill_arc_.set_y(config_.center_y); - negative_fill_arc_.set_r(config_.radius_px); - negative_fill_arc_.set_width(width); - } - - static constexpr double rad_to_deg_(double radians) { - return radians * 180.0 / 3.14159265358979323846; - } - - static uint16_t wrap_angle_(long angle) { - angle %= 360; - if (angle < 0) - angle += 360; - return static_cast(angle); - } - - static constexpr uint16_t mid_angle_() { return 180; } - - static uint16_t to_referee_angle_(double display_pitch_deg) { - return wrap_angle_(std::lround(static_cast(mid_angle_()) + display_pitch_deg)); - } - - Config config_{}; - Arc background_arc_{Shape::Color::WHITE, 1, 0, 0, 0, 0, 1, 1, false}; - Arc positive_fill_arc_{Shape::Color::YELLOW, 1, 0, 0, 0, 0, 1, 1, false}; - Arc negative_fill_arc_{Shape::Color::YELLOW, 1, 0, 0, 0, 0, 1, 1, false}; - bool positive_fill_visible_ = false; - bool negative_fill_visible_ = false; -}; - -} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp new file mode 100644 index 00000000..90c0a5f1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include + +#include +#include +#include + +namespace librmcs::host::protocol { + +class LIBRMCS_API Handler { +public: + class LIBRMCS_API PacketBuilder { + public: + PacketBuilder(const PacketBuilder&) = delete; + PacketBuilder& operator=(const PacketBuilder&) = delete; + PacketBuilder(PacketBuilder&&) = delete; + PacketBuilder& operator=(PacketBuilder&&) = delete; + + ~PacketBuilder() noexcept; + + bool write_can(data::DataId field_id, const data::CanDataView& view) noexcept; + + bool write_uart(data::DataId field_id, const data::UartDataView& view) noexcept; + + bool write_gpio_digital_data( + uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept; + + bool write_gpio_digital_read_config( + uint8_t channel_index, const data::GpioReadConfigView& view) noexcept; + + bool write_gpio_analog_data( + uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept; + + private: + friend class Handler; + + explicit PacketBuilder(void* shared_state) noexcept; + + alignas(std::uintptr_t) std::uint8_t storage_[16 * sizeof(std::uintptr_t)]; + }; + + Handler( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const agent::AdvancedOptions& options, data::DataCallback& callback); + + Handler(const Handler&) = delete; + Handler& operator=(const Handler&) = delete; + Handler(Handler&& other) noexcept; + Handler& operator=(Handler&& other) noexcept; + + ~Handler() noexcept; + + PacketBuilder start_transmit() noexcept; + +private: + void* shared_state_ = nullptr; +}; + +} // namespace librmcs::host::protocol diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp new file mode 100644 index 00000000..a52bdde7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp @@ -0,0 +1,390 @@ +#include "librmcs/protocol/handler.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "core/src/protocol/deserializer.hpp" +#include "core/src/protocol/protocol.hpp" +#include "core/src/protocol/serializer.hpp" +#include "core/src/utility/assert.hpp" +#include "host/src/logging/logging.hpp" +#include "host/src/protocol/stream_buffer.hpp" +#include "host/src/transport/transport.hpp" +#include "librmcs/agent/common.hpp" +#include "librmcs/data/datas.hpp" + +namespace librmcs::host::protocol { + +namespace { + +class NullTransport final : public transport::Transport { +public: + std::unique_ptr acquire_transmit_buffer() noexcept override { + return nullptr; + } + + void transmit(std::unique_ptr buffer, size_t) override { + (void)buffer; + } + + void release_transmit_buffer(std::unique_ptr buffer) override { + (void)buffer; + } + + void receive(std::function)> callback) override { + receive_callback_ = std::move(callback); + } + + bool healthy() const noexcept override { return false; } + +private: + std::function)> receive_callback_; +}; + +} // namespace + +class HandlerImpl : public core::protocol::DeserializeCallback { +public: + explicit HandlerImpl(std::unique_ptr transport, data::DataCallback& callback) + : transport_(std::move(transport)) + , callback_(callback) + , deserializer_(*this) { + transport_->receive([this](std::span buffer) { + // Operating system automatically assembles the packet + deserializer_.feed(buffer); + deserializer_.finish_transfer(); + }); + } + + transport::Transport& transport() noexcept { return *transport_; } + + bool healthy() const noexcept { return transport_->healthy(); } + + bool is_null_transport() const noexcept { + return dynamic_cast(transport_.get()) != nullptr; + } + + void can_deserialized_callback( + core::protocol::FieldId id, const data::CanDataView& data) override { + if (!callback_.can_receive_callback(id, data)) + logging::get_logger().error("Unexpected can field id: ", static_cast(id)); + } + + void uart_deserialized_callback( + core::protocol::FieldId id, const data::UartDataView& data) override { + if (!callback_.uart_receive_callback(id, data)) + logging::get_logger().error("Unexpected uart field id: ", static_cast(id)); + } + + void gpio_digital_data_deserialized_callback( + uint8_t channel_index, const data::GpioDigitalDataView& data) override { + callback_.gpio_digital_read_result_callback(channel_index, data); + } + + void gpio_analog_data_deserialized_callback( + uint8_t channel_index, const data::GpioAnalogDataView& data) override { + callback_.gpio_analog_read_result_callback(channel_index, data); + } + + void gpio_digital_read_config_deserialized_callback( + uint8_t channel_index, const data::GpioReadConfigView& data) override { + (void)channel_index; + (void)data; + logging::get_logger().error("Unexpected gpio digital read config field in uplink"); + } + + void gpio_analog_read_config_deserialized_callback( + uint8_t channel_index, const data::GpioReadConfigView& data) override { + (void)channel_index; + (void)data; + logging::get_logger().error("Unexpected gpio analog read config field in uplink"); + } + + void accelerometer_deserialized_callback(const data::AccelerometerDataView& data) override { + callback_.accelerometer_receive_callback(data); + } + + void gyroscope_deserialized_callback(const data::GyroscopeDataView& data) override { + callback_.gyroscope_receive_callback(data); + } + + void temperature_deserialized_callback(const data::TemperatureDataView& data) override { + callback_.temperature_receive_callback(data); + } + + void error_callback() override { + logging::get_logger().error("Deserializer encountered an error while parsing input"); + } + +private: + std::unique_ptr transport_; + data::DataCallback& callback_; + core::protocol::Deserializer deserializer_; +}; + +class HandlerSharedState { +public: + HandlerSharedState( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const agent::AdvancedOptions& options, data::DataCallback& callback) + : usb_vid_(usb_vid) + , usb_pid_(usb_pid) + , serial_filter_(serial_filter) + , options_(options) + , callback_(callback) + , impl_(std::make_shared(create_transport_(), callback_)) + , reconnect_thread_([this]() { reconnect_loop_(); }) {} + + ~HandlerSharedState() { + stop_reconnect_thread_.store(true, std::memory_order::relaxed); + if (reconnect_thread_.joinable()) + reconnect_thread_.join(); + } + + std::shared_ptr acquire_impl_for_transmit() noexcept { + maybe_restart_transport_(); + + const std::scoped_lock guard{mutex_}; + core::utility::assert_debug(static_cast(impl_)); + return impl_; + } + + void note_transmit_result(core::protocol::Serializer::SerializeResult result) noexcept { + using SerializeResult = core::protocol::Serializer::SerializeResult; + + if (result == SerializeResult::kSuccess) [[likely]] { + consecutive_acquire_failures_.store(0, std::memory_order::relaxed); + return; + } + + if (result == SerializeResult::kBadAlloc) { + logging::get_logger().error("Transmit buffer unavailable (acquire failed)"); + + const uint32_t failures = + consecutive_acquire_failures_.fetch_add(1, std::memory_order::relaxed) + 1; + if (failures >= kRestartFailureThreshold_ + && !transport_restart_requested_.exchange(true, std::memory_order::relaxed)) { + logging::get_logger().warn( + "USB transport restart scheduled after {} consecutive transmit buffer " + "acquire failures", + failures); + } + return; + } + + core::utility::assert_failed_debug(); + } + +private: + std::unique_ptr create_transport_() const { + return transport::usb::create_transport( + usb_vid_, usb_pid_, serial_filter_, transport::usb::ConnectionOptions{ + .dangerously_skip_version_checks = + options_.dangerously_skip_version_checks, + }); + } + + void maybe_restart_transport_() noexcept { + auto now = std::chrono::steady_clock::now(); + + const std::scoped_lock guard{mutex_}; + const bool reconnect_due_to_failure = + transport_restart_requested_.load(std::memory_order::relaxed); + const bool reconnect_due_to_unhealthy_transport = impl_ && !impl_->healthy(); + const bool reconnect_due_to_null_transport = impl_ && impl_->is_null_transport(); + if (!reconnect_due_to_failure && !reconnect_due_to_unhealthy_transport + && !reconnect_due_to_null_transport) + return; + if (now < next_restart_time_) + return; + + const uint32_t failures = + consecutive_acquire_failures_.load(std::memory_order::relaxed); + + std::shared_ptr old_impl; + old_impl.swap(impl_); + old_impl.reset(); + + try { + impl_ = std::make_shared(create_transport_(), callback_); + consecutive_acquire_failures_.store(0, std::memory_order::relaxed); + transport_restart_requested_.store(false, std::memory_order::relaxed); + next_restart_time_ = std::chrono::steady_clock::time_point::min(); + logging::get_logger().warn( + "USB transport restarted after {} consecutive transmit buffer acquire failures", + failures); + } catch (const std::exception& ex) { + impl_ = std::make_shared(std::make_unique(), callback_); + transport_restart_requested_.store(true, std::memory_order::relaxed); + next_restart_time_ = std::chrono::steady_clock::now() + kRestartRetryInterval_; + logging::get_logger().error("USB transport restart failed: {}", ex.what()); + } catch (...) { + impl_ = std::make_shared(std::make_unique(), callback_); + transport_restart_requested_.store(true, std::memory_order::relaxed); + next_restart_time_ = std::chrono::steady_clock::now() + kRestartRetryInterval_; + logging::get_logger().error("USB transport restart failed: unknown error"); + } + } + + void reconnect_loop_() { + while (!stop_reconnect_thread_.load(std::memory_order::relaxed)) { + maybe_restart_transport_(); + std::this_thread::sleep_for(kRestartRetryInterval_); + } + } + + static constexpr uint32_t kRestartFailureThreshold_ = 8; + static constexpr auto kRestartRetryInterval_ = std::chrono::milliseconds{500}; + + std::mutex mutex_; + uint16_t usb_vid_; + int32_t usb_pid_; + std::string serial_filter_; + agent::AdvancedOptions options_; + data::DataCallback& callback_; + std::shared_ptr impl_; + std::atomic consecutive_acquire_failures_{0}; + std::atomic transport_restart_requested_{false}; + std::atomic stop_reconnect_thread_{false}; + std::chrono::steady_clock::time_point next_restart_time_ = + std::chrono::steady_clock::time_point::min(); + std::thread reconnect_thread_; +}; + +namespace { + +struct PacketBuilderImpl { + explicit PacketBuilderImpl(HandlerSharedState& shared_state) noexcept + : impl_(shared_state.acquire_impl_for_transmit()) + , shared_state_(shared_state) + , buffer_(impl_->transport()) + , serializer_(buffer_) {} + + PacketBuilderImpl(PacketBuilderImpl&& other) noexcept + : impl_(std::move(other.impl_)) + , shared_state_(other.shared_state_) + , buffer_(std::move(other.buffer_)) + , serializer_(buffer_) {} + + PacketBuilderImpl& operator=(PacketBuilderImpl&&) = delete; + PacketBuilderImpl(const PacketBuilderImpl&) = delete; + PacketBuilderImpl& operator=(const PacketBuilderImpl&) = delete; + ~PacketBuilderImpl() = default; + + [[nodiscard]] bool write_can(data::DataId field_id, const data::CanDataView& view) noexcept { + return process_result(serializer_.write_can(field_id, view)); + } + + [[nodiscard]] bool write_uart(data::DataId field_id, const data::UartDataView& view) noexcept { + return process_result(serializer_.write_uart(field_id, view)); + } + + [[nodiscard]] bool write_gpio_digital_data( + uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept { + return process_result(serializer_.write_gpio_digital_data(channel_index, view)); + } + + [[nodiscard]] bool write_gpio_digital_read_config( + uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { + return process_result(serializer_.write_gpio_digital_read_config(channel_index, view)); + } + + [[nodiscard]] bool write_gpio_analog_data( + uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept { + return process_result(serializer_.write_gpio_analog_data(channel_index, view)); + } + +private: + bool process_result(core::protocol::Serializer::SerializeResult result) noexcept { + using SerializeResult = core::protocol::Serializer::SerializeResult; + if (result == SerializeResult::kInvalidArgument) + return false; + + shared_state_.note_transmit_result(result); + return true; + } + + std::shared_ptr impl_; + HandlerSharedState& shared_state_; + StreamBuffer buffer_; + core::protocol::Serializer serializer_; +}; + +} // namespace + +Handler::PacketBuilder::PacketBuilder(void* shared_state_ptr) noexcept { + static_assert(sizeof(PacketBuilderImpl) <= sizeof(storage_)); + static_assert(alignof(PacketBuilderImpl) <= alignof(std::uintptr_t)); + + auto& shared_state = *static_cast(shared_state_ptr); + std::construct_at(reinterpret_cast(storage_), shared_state); +} + +Handler::PacketBuilder::~PacketBuilder() noexcept { + std::destroy_at(std::launder(reinterpret_cast(storage_))); +} + +bool Handler::PacketBuilder::write_can( + data::DataId field_id, const data::CanDataView& view) noexcept { + return std::launder(reinterpret_cast(storage_))->write_can(field_id, view); +} + +bool Handler::PacketBuilder::write_uart( + data::DataId field_id, const data::UartDataView& view) noexcept { + return std::launder(reinterpret_cast(storage_)) + ->write_uart(field_id, view); +} + +bool Handler::PacketBuilder::write_gpio_digital_data( + uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept { + return std::launder(reinterpret_cast(storage_)) + ->write_gpio_digital_data(channel_index, view); +} + +bool Handler::PacketBuilder::write_gpio_digital_read_config( + uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { + return std::launder(reinterpret_cast(storage_)) + ->write_gpio_digital_read_config(channel_index, view); +} + +bool Handler::PacketBuilder::write_gpio_analog_data( + uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept { + return std::launder(reinterpret_cast(storage_)) + ->write_gpio_analog_data(channel_index, view); +} + +Handler::Handler( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const agent::AdvancedOptions& options, data::DataCallback& callback) + : shared_state_(new HandlerSharedState(usb_vid, usb_pid, serial_filter, options, callback)) {} + +Handler::Handler(Handler&& other) noexcept + : shared_state_(std::exchange(other.shared_state_, nullptr)) {} + +Handler& Handler::operator=(Handler&& other) noexcept { + if (this == &other) + return *this; + delete static_cast(shared_state_); + shared_state_ = std::exchange(other.shared_state_, nullptr); + return *this; +} + +Handler::~Handler() noexcept { delete static_cast(shared_state_); } + +Handler::PacketBuilder Handler::start_transmit() noexcept { + core::utility::assert_debug(shared_state_); + return PacketBuilder{shared_state_}; +} + +} // namespace librmcs::host::protocol diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp new file mode 100644 index 00000000..c298680e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "core/src/protocol/constant.hpp" + +namespace librmcs::host::transport { + +class TransportBuffer { +public: + using BufferSpanType = std::span; + + TransportBuffer() = default; + TransportBuffer(const TransportBuffer&) = delete; + TransportBuffer& operator=(const TransportBuffer&) = delete; + TransportBuffer(TransportBuffer&&) = delete; + TransportBuffer& operator=(TransportBuffer&&) = delete; + virtual ~TransportBuffer() noexcept = default; + + virtual BufferSpanType data() const noexcept = 0; +}; + +class Transport { +public: + Transport() = default; + virtual ~Transport() noexcept = default; + Transport(const Transport&) = delete; + Transport& operator=(const Transport&) = delete; + Transport(Transport&&) = delete; + Transport& operator=(Transport&&) = delete; + + virtual std::unique_ptr acquire_transmit_buffer() noexcept = 0; + virtual void transmit(std::unique_ptr buffer, size_t payload_size) = 0; + virtual void release_transmit_buffer(std::unique_ptr buffer) = 0; + virtual void receive(std::function)> callback) = 0; + virtual bool healthy() const noexcept { return true; } +}; + +namespace usb { + +struct ConnectionOptions { + bool dangerously_skip_version_checks = false; +}; + +std::unique_ptr create_transport( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const ConnectionOptions& options); + +} // namespace usb + +} // namespace librmcs::host::transport diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp new file mode 100644 index 00000000..738ae70e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp @@ -0,0 +1,398 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "core/src/protocol/constant.hpp" +#include "core/src/utility/assert.hpp" +#include "host/src/logging/logging.hpp" +#include "host/src/transport/transport.hpp" +#include "host/src/transport/usb/device_scanner.hpp" +#include "host/src/transport/usb/helper.hpp" +#include "host/src/utility/final_action.hpp" +#include "host/src/utility/ring_buffer.hpp" + +namespace librmcs::host::transport::usb { + +class Usb : public Transport { +public: + explicit Usb( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const ConnectionOptions& options) + : logger_(logging::get_logger()) + , free_transmit_transfers_(kTransmitTransferCount) { + + usb_init(usb_vid, usb_pid, serial_filter, options); + utility::FinalAction rollback_on_failure{[this]() noexcept { + destroy_free_transmit_transfers(); + libusb_release_interface(libusb_device_handle_, kTargetInterface); + libusb_close(libusb_device_handle_); + libusb_exit(libusb_context_); + }}; + + init_transmit_transfers(); + event_thread_ = std::thread{[this]() { handle_events(); }}; + + rollback_on_failure.disable(); + } + + Usb(const Usb&) = delete; + Usb& operator=(const Usb&) = delete; + Usb(Usb&&) = delete; + Usb& operator=(Usb&&) = delete; + + ~Usb() override { + { + const std::scoped_lock guard{transmit_transfer_push_mutex_}; + stop_handling_events_.store(true, std::memory_order::relaxed); + } + destroy_free_transmit_transfers(); + + libusb_release_interface(libusb_device_handle_, kTargetInterface); + + // libusb_close() reliably cancels all pending transfers and invokes their callbacks, + // avoiding race conditions present in other cancellation methods + libusb_close(libusb_device_handle_); + + if (event_thread_.joinable()) + event_thread_.join(); + + libusb_exit(libusb_context_); + } + + std::unique_ptr acquire_transmit_buffer() noexcept override { + if (!connection_healthy_.load(std::memory_order::relaxed)) + return nullptr; + + TransferWrapper* transfer = nullptr; + { + const std::scoped_lock guard{transmit_transfer_pop_mutex_}; + free_transmit_transfers_.pop_front( + [&transfer](TransferWrapper* value) noexcept { transfer = value; }); + } + if (!transfer) + return nullptr; + + return std::unique_ptr{transfer}; + } + + void transmit(std::unique_ptr buffer, size_t size) override { + core::utility::assert_debug(static_cast(buffer)); + + if (size > core::protocol::kProtocolBufferSize) + throw std::invalid_argument("Transmit size exceeds maximum transfer length"); + + if (!connection_healthy_.load(std::memory_order::relaxed)) { + release_transmit_buffer(std::move(buffer)); + return; + } + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) + auto& transfer = static_cast(buffer.get())->transfer_; + transfer->length = static_cast(size); + + int ret = libusb_submit_transfer(transfer); + if (ret != 0) [[unlikely]] { + mark_connection_lost_( + std::format( + "Failed to submit transmit transfer: {} ({})", ret, + helper::libusb_errname(ret))); + release_transmit_buffer(std::move(buffer)); + return; + } + + // If success: Ownership is transferred to libusb + std::ignore = buffer.release(); + } + + void release_transmit_buffer(std::unique_ptr buffer) override { + core::utility::assert_debug(static_cast(buffer)); + + const std::scoped_lock guard{transmit_transfer_push_mutex_}; + + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) + auto* wrapper = static_cast(buffer.release()); + free_transmit_transfers_.emplace_back(wrapper); + } + + void receive(std::function)> callback) override { + if (!callback) + throw std::invalid_argument{"Callback function cannot be null"}; + if (receive_callback_) + throw std::logic_error{"Receive function can only be called once"}; + + receive_callback_ = std::move(callback); + init_receive_transfers(); + } + + bool healthy() const noexcept override { + return connection_healthy_.load(std::memory_order::relaxed); + } + +private: + class TransferWrapper : public TransportBuffer { + friend class Usb; + + public: + explicit TransferWrapper(Usb& self) + : self_(self) + , transfer_(self_.create_libusb_transfer()) {} + + TransferWrapper(const TransferWrapper&) = delete; + TransferWrapper& operator=(const TransferWrapper&) = delete; + TransferWrapper(TransferWrapper&&) = delete; + TransferWrapper& operator=(TransferWrapper&&) = delete; + + ~TransferWrapper() override { + if (transfer_) { + logging::get_logger().error( + "USB TransferBuffer {} was destroyed externally - this is undefined behavior. " + "Buffers must be returned via transmit() or " + "release_transmit_buffer(). ", + static_cast(this)); + destroy(); + } + } + + BufferSpanType data() const noexcept override { + return BufferSpanType{ + reinterpret_cast(transfer_->buffer), + core::protocol::kProtocolBufferSize}; + } + + void destroy() noexcept { + self_.destroy_libusb_transfer(transfer_); + transfer_ = nullptr; + } + + private: + Usb& self_; + + libusb_transfer* transfer_; + }; + + void usb_init( + uint16_t vendor_id, int32_t product_id, std::string_view serial_filter, + const ConnectionOptions& options) { + if (const int ret = libusb_init(&libusb_context_); ret != 0) [[unlikely]] { + throw std::runtime_error( + std::format( + "Failed to initialize libusb: {} ({})", ret, helper::libusb_errname(ret))); + } + utility::FinalAction exit_libusb{[this]() noexcept { libusb_exit(libusb_context_); }}; + + libusb_device_handle_ = DeviceScanner::select_device( + libusb_context_, vendor_id, product_id, serial_filter, options); + utility::FinalAction close_device_handle{ + [this]() noexcept { libusb_close(libusb_device_handle_); }}; + + if (const int ret = libusb_claim_interface(libusb_device_handle_, kTargetInterface); + ret != 0) [[unlikely]] { + throw std::runtime_error( + std::format( + "Failed to claim interface {}: {} ({})", kTargetInterface, ret, + helper::libusb_errname(ret))); + } + + close_device_handle.disable(); + exit_libusb.disable(); + } + + void init_transmit_transfers() { + TransferWrapper* transmit_transfers[kTransmitTransferCount] = {}; + try { + for (auto& wrapper : transmit_transfers) { + wrapper = new TransferWrapper{*this}; + auto* transfer = wrapper->transfer_; + + libusb_fill_bulk_transfer( + transfer, libusb_device_handle_, kOutEndpoint, + new unsigned char[core::protocol::kProtocolBufferSize], 0, + [](libusb_transfer* transfer) { + auto* wrapper = static_cast(transfer->user_data); + wrapper->self_.usb_transmit_complete_callback(wrapper); + }, + wrapper, 0); + transfer->flags = libusb_transfer_flags::LIBUSB_TRANSFER_FREE_BUFFER; + } + } catch (...) { + for (auto& wrapper : transmit_transfers) { + if (wrapper) { + wrapper->destroy(); + delete wrapper; + } + } + throw; + } + + auto* iter = transmit_transfers; + free_transmit_transfers_.push_back_n( + [&iter]() noexcept { return *iter++; }, kTransmitTransferCount); + } + + void handle_events() { + while (active_transfers_.load(std::memory_order::relaxed)) { + libusb_handle_events(libusb_context_); + } + } + + void init_receive_transfers() { + for (size_t i = 0; i < kReceiveTransferCount; i++) { + auto* transfer = create_libusb_transfer(); + + libusb_fill_bulk_transfer( + transfer, libusb_device_handle_, kInEndpoint, + new unsigned char[core::protocol::kProtocolBufferSize], + static_cast(core::protocol::kProtocolBufferSize), + [](libusb_transfer* transfer) { + static_cast(transfer->user_data)->usb_receive_complete_callback(transfer); + }, + this, 0); + transfer->flags = libusb_transfer_flags::LIBUSB_TRANSFER_FREE_BUFFER; + + int ret = libusb_submit_transfer(transfer); + if (ret != 0) [[unlikely]] { + destroy_libusb_transfer(transfer); + throw std::runtime_error( + std::format( + "Failed to submit receive transfer: {} ({})", ret, + helper::libusb_errname(ret))); + } + } + } + + void usb_transmit_complete_callback(TransferWrapper* wrapper) { + const std::scoped_lock guard{transmit_transfer_push_mutex_}; + + if (stop_handling_events_.load(std::memory_order::relaxed)) [[unlikely]] { + wrapper->destroy(); + delete wrapper; + return; + } + + if (wrapper->transfer_->status != LIBUSB_TRANSFER_COMPLETED) [[unlikely]] { + mark_connection_lost_( + std::format( + "Transmit transfer failed with status {}", + static_cast(wrapper->transfer_->status))); + wrapper->destroy(); + delete wrapper; + return; + } + + free_transmit_transfers_.emplace_back(wrapper); + } + + void usb_receive_complete_callback(libusb_transfer* transfer) { + if (stop_handling_events_.load(std::memory_order::relaxed)) [[unlikely]] { + destroy_libusb_transfer(transfer); + return; + } + + if (transfer->status != LIBUSB_TRANSFER_COMPLETED) [[unlikely]] { + mark_connection_lost_( + std::format( + "Receive transfer failed with status {}", + static_cast(transfer->status))); + destroy_libusb_transfer(transfer); + return; + } + + const auto now = std::chrono::steady_clock::now(); + const bool should_drop = now > last_rx_callback_timepoint_ + std::chrono::seconds{1}; + last_rx_callback_timepoint_ = now; + + if (!should_drop && transfer->actual_length > 0) { + const auto* first = reinterpret_cast(transfer->buffer); + const auto size = static_cast(transfer->actual_length); + receive_callback_({first, size}); + } + + int ret = libusb_submit_transfer(transfer); + if (ret != 0) [[unlikely]] { + mark_connection_lost_( + std::format( + "Failed to re-submit receive transfer: {} ({})", ret, + helper::libusb_errname(ret))); + destroy_libusb_transfer(transfer); + } + } + + void mark_connection_lost_(const std::string& reason) noexcept { + bool expected = true; + if (!connection_healthy_.compare_exchange_strong( + expected, false, std::memory_order::relaxed, std::memory_order::relaxed)) { + return; + } + + logger_.error("USB transport marked unhealthy: {}", reason); + } + + void destroy_free_transmit_transfers() noexcept { + free_transmit_transfers_.pop_front_n([](TransferWrapper* wrapper) noexcept { + wrapper->destroy(); + delete wrapper; + }); + } + + libusb_transfer* create_libusb_transfer() { + auto* transfer = libusb_alloc_transfer(0); + if (!transfer) + throw std::bad_alloc{}; + active_transfers_.fetch_add(1, std::memory_order::relaxed); + return transfer; + } + + void destroy_libusb_transfer(libusb_transfer* transfer) noexcept { + libusb_free_transfer(transfer); + active_transfers_.fetch_sub(1, std::memory_order::relaxed); + } + + static constexpr int kTargetInterface = 0x00; + + static constexpr unsigned char kOutEndpoint = 0x01; + static constexpr unsigned char kInEndpoint = 0x81; + + static constexpr size_t kTransmitTransferCount = 64; + static constexpr size_t kReceiveTransferCount = 4; + + logging::Logger& logger_; + + libusb_context* libusb_context_ = nullptr; + libusb_device_handle* libusb_device_handle_ = nullptr; + + std::thread event_thread_; + + std::atomic active_transfers_ = 0; + std::atomic stop_handling_events_ = false; + std::atomic connection_healthy_ = true; + + utility::RingBuffer free_transmit_transfers_; + std::mutex transmit_transfer_pop_mutex_, transmit_transfer_push_mutex_; + + std::function)> receive_callback_; + std::chrono::steady_clock::time_point last_rx_callback_timepoint_ = + std::chrono::steady_clock::time_point::min(); +}; + +std::unique_ptr create_transport( + uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, + const ConnectionOptions& options) { + return std::make_unique(usb_vid, usb_pid, serial_filter, options); +} + +} // namespace librmcs::host::transport::usb From b654d65cbb0de29c12defc6f48df5c81e772f614 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 16 May 2026 22:17:23 +0800 Subject: [PATCH 16/90] feat: update CAN transmission logic in deformable infantry hardware Co-authored-by: Copilot --- .../src/hardware/deformable-infantry-omni-b.cpp | 2 +- .../src/hardware/deformable-infantry-omni.cpp | 11 ----------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index b2d55992..9a7c29a3 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -283,7 +283,7 @@ class DeformableInfantryOmniB device::CanPacket8{ chassis_wheel_motors_[1].generate_command(), device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, } .as_bytes(), diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 5dfbcc6a..561c22c0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -296,17 +296,6 @@ class DeformableInfantryOmni } .as_bytes(), }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); } else { builder.can0_transmit({ .can_id = 0x141, From d04e252d681db21b9c98a71109448604973e1597 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 18 May 2026 12:06:54 +0800 Subject: [PATCH 17/90] feat: add submodule configurations for hikcamera and auto_aim_v2 --- .gitmodules | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.gitmodules b/.gitmodules index 61e5f08f..aeef93a2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,11 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git + +[submodule "rmcs_ws/src/hikcamera"] + path = rmcs_ws/src/hikcamera + url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git + +[submodule "rmcs_ws/src/auto_aim_v2"] + path = rmcs_ws/src/auto_aim_v2 + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git \ No newline at end of file From 234fe7302fc9df8e718472807f08be1ecdacca17 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 19 May 2026 00:04:10 +0800 Subject: [PATCH 18/90] feat: remove unused inputs and simplify toggle logic in deformable chassis controller --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../chassis/chassis_power_controller.cpp | 3 +- .../controller/chassis/deformable_chassis.cpp | 89 +++++-------------- 3 files changed, 25 insertions(+), 69 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 968a36c3..d835419b 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 968a36c359c654e00fb9084ab1e5551810a19233 +Subproject commit d835419bcc0a0e62eb22af298f9259f2c57a8d75 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index d4988239..2920b48f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -54,7 +54,6 @@ class ChassisPowerController auto switch_right = *switch_right_; auto switch_left = *switch_left_; auto keyboard = *keyboard_; - auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -64,7 +63,7 @@ class ChassisPowerController update_virtual_buffer_energy(); - boost_mode_ = keyboard.shift || keyboard.ctrl || rotary_knob < -0.9; + boost_mode_ = keyboard.shift || keyboard.ctrl; *supercap_control_enabled_ = boost_mode_; update_control_power_limit(); } diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 1473f296..ae51dcbb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -5,14 +5,12 @@ #include #include #include -#include #include #include #include #include -#include #include #include #include @@ -132,7 +130,6 @@ class DeformableChassis register_input("/remote/keyboard", keyboard_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); - register_input("/tf", tf_, false); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); @@ -371,8 +368,6 @@ class DeformableChassis void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { qe_complex_spin_active_ = true; qe_last_toggle_elapsed_ = 0.0; - qe_front_window_armed_ = true; - qe_rear_window_armed_ = true; qe_front_high_rear_low_ = true; apply_front_high_rear_low_target_(); if (mode != rmcs_msgs::ChassisMode::SPIN) { @@ -384,8 +379,6 @@ class DeformableChassis void deactivate_qe_complex_spin_() { qe_complex_spin_active_ = false; qe_last_toggle_elapsed_ = 0.0; - qe_front_window_armed_ = true; - qe_rear_window_armed_ = true; } void apply_front_high_rear_low_target_() { @@ -394,6 +387,7 @@ class DeformableChassis lb_current_target_angle_ = min_angle_; rb_current_target_angle_ = min_angle_; apply_symmetric_target = false; + qe_front_high_rear_low_ = true; } void apply_front_low_rear_high_target_() { @@ -402,72 +396,32 @@ class DeformableChassis lb_current_target_angle_ = max_angle_; rb_current_target_angle_ = max_angle_; apply_symmetric_target = false; + qe_front_high_rear_low_ = false; } - void toggle_qe_complex_spin_target_() { - qe_front_high_rear_low_ = !qe_front_high_rear_low_; + void toggle_bg_target_() { if (qe_front_high_rear_low_) { - apply_front_high_rear_low_target_(); - } else { apply_front_low_rear_high_target_(); + } else { + apply_front_high_rear_low_target_(); } - qe_last_toggle_elapsed_ = 0.0; - } - - static double wrap_to_pi_(double angle) { - while (angle > std::numbers::pi) - angle -= 2.0 * std::numbers::pi; - while (angle < -std::numbers::pi) - angle += 2.0 * std::numbers::pi; - return angle; - } - - static double heading_from_direction_(const Eigen::Vector3d& direction) { - return std::atan2(direction.y(), direction.x()); } - std::optional> read_qe_spin_headings_() const { - if (!tf_.ready()) - return std::nullopt; - - const auto chassis_direction = fast_tf::cast( - rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - const auto gimbal_direction = fast_tf::cast( - rmcs_description::YawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - - return std::pair{ - heading_from_direction_(*chassis_direction), heading_from_direction_(*gimbal_direction)}; + void toggle_qe_complex_spin_target_() { + toggle_bg_target_(); } void update_qe_complex_spin_toggle_() { - constexpr double heading_trigger_threshold = 20.0 * std::numbers::pi / 180.0; - constexpr double heading_rearm_threshold = 40.0 * std::numbers::pi / 180.0; - constexpr double force_toggle_timeout = 0.25; + constexpr double qe_complex_spin_toggle_period = 1.0; qe_last_toggle_elapsed_ += update_dt(); - - bool should_toggle = false; - if (const auto headings = read_qe_spin_headings_()) { - const auto& [chassis_heading, gimbal_heading] = *headings; - const double relative_heading = wrap_to_pi_(chassis_heading - gimbal_heading); - const double front_error = std::abs(relative_heading); - const double rear_error = std::abs(std::abs(relative_heading) - std::numbers::pi); - - if (front_error >= heading_rearm_threshold) - qe_front_window_armed_ = true; - if (rear_error >= heading_rearm_threshold) - qe_rear_window_armed_ = true; - - if (front_error <= heading_trigger_threshold && qe_front_window_armed_) { - should_toggle = true; - qe_front_window_armed_ = false; - } else if (rear_error <= heading_trigger_threshold && qe_rear_window_armed_) { - should_toggle = true; - qe_rear_window_armed_ = false; - } + size_t qe_complex_spin_toggle_count = 0; + while (qe_last_toggle_elapsed_ >= qe_complex_spin_toggle_period) { + qe_last_toggle_elapsed_ -= qe_complex_spin_toggle_period; + ++qe_complex_spin_toggle_count; } - if (should_toggle || qe_last_toggle_elapsed_ >= force_toggle_timeout) + if ((qe_complex_spin_toggle_count % 2) == 1) toggle_qe_complex_spin_target_(); } @@ -782,14 +736,18 @@ class DeformableChassis } void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { - constexpr double rotary_knob_edge_threshold = 0.7; - constexpr double complex_spin_toggle_period = 0.2; + constexpr double rotary_knob_symmetric_edge_threshold = 0.7; + constexpr double rotary_knob_bg_edge_threshold = -0.9; + constexpr double complex_spin_toggle_period = 0.5; const bool keyboard_toggle_condition = !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q && !keyboard.e; const bool rotary_knob_toggle_condition = - last_rotary_knob_ < rotary_knob_edge_threshold - && *rotary_knob_ >= rotary_knob_edge_threshold; + last_rotary_knob_ < rotary_knob_symmetric_edge_threshold + && *rotary_knob_ >= rotary_knob_symmetric_edge_threshold; + const bool rotary_knob_bg_toggle_condition = + !qe_complex_spin_active_ && last_rotary_knob_ > rotary_knob_bg_edge_threshold + && *rotary_knob_ <= rotary_knob_bg_edge_threshold; const bool front_high_rear_low = !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; const bool front_low_rear_high = !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; bool complex_spin_toggle_condition = false; @@ -818,6 +776,8 @@ class DeformableChassis current_target_angle_ = (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; apply_symmetric_target = true; + } else if (rotary_knob_bg_toggle_condition) { + toggle_bg_target_(); } else if (front_high_rear_low) { apply_front_high_rear_low_target_(); } else if (front_low_rear_high) { @@ -989,7 +949,6 @@ class DeformableChassis } private: - InputInterface tf_; InputInterface joystick_right_; InputInterface switch_right_; InputInterface switch_left_; @@ -1013,8 +972,6 @@ class DeformableChassis bool complex_spin_active_ = false; double complex_spin_elapsed_ = 0.0; bool qe_complex_spin_active_ = false; - bool qe_front_window_armed_ = true; - bool qe_rear_window_armed_ = true; bool qe_front_high_rear_low_ = true; double qe_last_toggle_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; From 1827c98820d0248e8883cf7aa0bc57da90dea8dc Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 19 May 2026 02:16:42 +0800 Subject: [PATCH 19/90] feat: enhance auto-aim logic in bullet feeder controller --- .../shooting/bullet_feeder_controller_17mm.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 556762af..49f41690 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -84,13 +84,15 @@ class BulletFeederController17mm if (switch_right != Switch::DOWN) { shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; + const bool auto_aim_requested = mouse.right || switch_right == Switch::UP; single_shot_stop_counter_ = std::max(0, single_shot_stop_counter_ - 1); temporary_single_shot_counter_ = std::max(0, temporary_single_shot_counter_ - 1); if (!last_mouse_.left && mouse.left) single_shot_stop_counter_ = single_shot_max_stop_delay_; - else if (last_switch_left_ != Switch::DOWN && switch_left == Switch::DOWN) { + else if (!auto_aim_requested && last_switch_left_ != Switch::DOWN + && switch_left == Switch::DOWN) { single_shot_stop_counter_ = single_shot_max_stop_delay_; temporary_single_shot_counter_ = 500; } @@ -102,9 +104,12 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - const bool auto_aim_enabled = mouse.right || switch_right == Switch::UP; - bool triggered = (mouse.left && !mouse.right) || switch_left == Switch::DOWN - || (mouse.left && auto_aim_enabled && *fire_control_); + const bool auto_aim_enabled = auto_aim_requested; + bool triggered = + (mouse.left && !mouse.right) || + (switch_left == Switch::DOWN && auto_aim_enabled && *fire_control_) || + (switch_left == Switch::DOWN && !auto_aim_enabled) || + (mouse.left && auto_aim_enabled && *fire_control_); bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { From ce5893aaf14033b88eb763768d521204b7a6d079 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 20 May 2026 02:53:49 +0800 Subject: [PATCH 20/90] feat: update joint target state management and fallback radius handling in deformable infantry --- .../config/deformable-infantry-omni.yaml | 2 +- .../controller/chassis/deformable_chassis.cpp | 125 ++++++++++-------- .../hardware/deformable-infantry-omni-b.cpp | 9 +- .../src/hardware/deformable-infantry-omni.cpp | 10 +- 4 files changed, 85 insertions(+), 61 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 3885fdd2..bb968c23 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -47,7 +47,7 @@ deformable_infantry: left_back_zero_point: 5801 right_back_zero_point: 7817 right_front_zero_point: 7136 - yaw_motor_zero_point: 31856 + yaw_motor_zero_point: 31517 pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index ae51dcbb..8e82b832 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -534,19 +534,29 @@ class DeformableChassis chassis_imu_calibration_sample_count_); } - bool initialize_joint_target_states_from_feedback( + bool ensure_joint_target_states_from_feedback( const std::array& current_physical_angles) { + bool any_active = false; for (size_t i = 0; i < kJointCount; ++i) { - if (!std::isfinite(current_physical_angles[i])) - return false; + if (std::isfinite(current_physical_angles[i]) && !joint_target_active_[i]) { + joint_target_physical_angle_state_rad_[i] = current_physical_angles[i]; + joint_target_physical_velocity_state_rad_[i] = 0.0; + joint_target_physical_acceleration_state_rad_[i] = 0.0; + current_target_physical_angles_rad_[i] = current_physical_angles[i]; + joint_target_active_[i] = true; + } + + any_active = any_active || joint_target_active_[i]; } - joint_target_physical_angle_state_rad_ = current_physical_angles; - joint_target_physical_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - joint_target_physical_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - current_target_physical_angles_rad_ = current_physical_angles; - joint_target_active_ = true; - return true; + return any_active; + } + + bool any_joint_target_active_() const { + return std::any_of( + joint_target_active_.begin(), joint_target_active_.end(), [](bool active) { + return active; + }); } double active_suspension_min_angle_rad_() const { @@ -622,7 +632,11 @@ class DeformableChassis lb_current_target_angle_ = current_target_angle_; rb_current_target_angle_ = current_target_angle_; rf_current_target_angle_ = current_target_angle_; - joint_target_active_ = false; + joint_target_active_.fill(false); + current_target_physical_angles_rad_.fill(nan_); + joint_target_physical_angle_state_rad_.fill(nan_); + joint_target_physical_velocity_state_rad_.fill(0.0); + joint_target_physical_acceleration_state_rad_.fill(0.0); suspension_on_by_switch = false; deactivate_complex_spin_(); deactivate_qe_complex_spin_(); @@ -793,8 +807,7 @@ class DeformableChassis const auto current_physical_angles = read_current_joint_physical_angles_(); const bool suspension_requested = suspension_requested_by_input_(); - if (!joint_target_active_ - && !initialize_joint_target_states_from_feedback(current_physical_angles)) { + if (!ensure_joint_target_states_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); return; } @@ -829,6 +842,9 @@ class DeformableChassis void update_joint_target_trajectory() { const double dt = update_dt(); for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) + continue; + double& angle_state = joint_target_physical_angle_state_rad_[i]; double& velocity_state = joint_target_physical_velocity_state_rad_[i]; double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; @@ -874,57 +890,54 @@ class DeformableChassis void publish_joint_target_angles( const std::array& current_physical_angles) { - if (!joint_target_active_) { + if (!any_joint_target_active_()) { publish_nan_joint_targets(); return; } - *left_front_joint_target_physical_angle_ = - joint_target_physical_angle_state_rad_[kLeftFront]; - *left_back_joint_target_physical_angle_ = joint_target_physical_angle_state_rad_[kLeftBack]; - *right_back_joint_target_physical_angle_ = - joint_target_physical_angle_state_rad_[kRightBack]; - *right_front_joint_target_physical_angle_ = - joint_target_physical_angle_state_rad_[kRightFront]; - - *left_front_joint_target_physical_velocity_ = - joint_target_physical_velocity_state_rad_[kLeftFront]; - *left_back_joint_target_physical_velocity_ = - joint_target_physical_velocity_state_rad_[kLeftBack]; - *right_back_joint_target_physical_velocity_ = - joint_target_physical_velocity_state_rad_[kRightBack]; - *right_front_joint_target_physical_velocity_ = - joint_target_physical_velocity_state_rad_[kRightFront]; - - *left_front_joint_target_physical_acceleration_ = - joint_target_physical_acceleration_state_rad_[kLeftFront]; - *left_back_joint_target_physical_acceleration_ = - joint_target_physical_acceleration_state_rad_[kLeftBack]; - *right_back_joint_target_physical_acceleration_ = - joint_target_physical_acceleration_state_rad_[kRightBack]; - *right_front_joint_target_physical_acceleration_ = - joint_target_physical_acceleration_state_rad_[kRightFront]; - - *lf_angle_error_ = std::isfinite(current_physical_angles[kLeftFront]) - ? current_physical_angles[kLeftFront] - - joint_target_physical_angle_state_rad_[kLeftFront] - : nan_; - *lb_angle_error_ = std::isfinite(current_physical_angles[kLeftBack]) - ? current_physical_angles[kLeftBack] - - joint_target_physical_angle_state_rad_[kLeftBack] - : nan_; - *rb_angle_error_ = std::isfinite(current_physical_angles[kRightBack]) - ? current_physical_angles[kRightBack] - - joint_target_physical_angle_state_rad_[kRightBack] - : nan_; - *rf_angle_error_ = std::isfinite(current_physical_angles[kRightFront]) - ? current_physical_angles[kRightFront] - - joint_target_physical_angle_state_rad_[kRightFront] - : nan_; + const auto publish_joint = [this, ¤t_physical_angles]( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output, + OutputInterface& acceleration_output, + OutputInterface& angle_error_output) { + if (!joint_target_active_[index]) { + *angle_output = nan_; + *velocity_output = nan_; + *acceleration_output = nan_; + *angle_error_output = nan_; + return; + } + + *angle_output = joint_target_physical_angle_state_rad_[index]; + *velocity_output = joint_target_physical_velocity_state_rad_[index]; + *acceleration_output = joint_target_physical_acceleration_state_rad_[index]; + *angle_error_output = std::isfinite(current_physical_angles[index]) + ? current_physical_angles[index] + - joint_target_physical_angle_state_rad_[index] + : nan_; + }; + + publish_joint( + kLeftFront, left_front_joint_target_physical_angle_, + left_front_joint_target_physical_velocity_, + left_front_joint_target_physical_acceleration_, lf_angle_error_); + publish_joint( + kLeftBack, left_back_joint_target_physical_angle_, + left_back_joint_target_physical_velocity_, + left_back_joint_target_physical_acceleration_, lb_angle_error_); + publish_joint( + kRightBack, right_back_joint_target_physical_angle_, + right_back_joint_target_physical_velocity_, + right_back_joint_target_physical_acceleration_, rb_angle_error_); + publish_joint( + kRightFront, right_front_joint_target_physical_angle_, + right_front_joint_target_physical_velocity_, + right_front_joint_target_physical_acceleration_, rf_angle_error_); } void publish_nan_joint_targets() { reset_attitude_correction_state_(); + joint_target_active_.fill(false); *left_front_joint_target_physical_angle_ = nan_; *left_back_joint_target_physical_angle_ = nan_; @@ -1015,7 +1028,7 @@ class DeformableChassis std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; - bool joint_target_active_ = false; + std::array joint_target_active_ = {false, false, false, false}; std::array joint_target_physical_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array joint_target_physical_velocity_state_rad_ = { 0.0, 0.0, 0.0, 0.0}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 9a7c29a3..269083d8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -198,7 +198,7 @@ class DeformableInfantryOmniB deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); deformableInfantry.register_output( "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, nan_); + deformableInfantry.register_output("/chassis/radius", radius_, default_radius_); deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); deformableInfantry.get_parameter_or( @@ -352,6 +352,7 @@ class DeformableInfantryOmniB static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; static constexpr double chassis_radius_base_ = 0.2341741; static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = 0.5 * rod_length_ + chassis_radius_base_; static double to_physical_angle_(double motor_angle) { return joint_zero_physical_angle_rad_ - motor_angle; @@ -383,7 +384,11 @@ class DeformableInfantryOmniB if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = nan_; *encoder_alpha_dot_ = nan_; - *radius_ = nan_; + *radius_ = default_radius_; + RCLCPP_WARN_THROTTLE( + deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + default_radius_); return; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 561c22c0..11b162a2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -170,7 +171,7 @@ class DeformableInfantryOmni right_front_joint_physical_velocity_, kNaN); status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); - status.register_output("/chassis/radius", radius_, kNaN); + status.register_output("/chassis/radius", radius_, default_radius_); status.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); status.get_parameter_or("debug_log_wheel_motor", debug_log_wheel_motor_, false); @@ -320,6 +321,7 @@ class DeformableInfantryOmni static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; static constexpr double chassis_radius_base_ = 0.2341741; static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; DeformableInfantryOmni& status_; Component& command_; @@ -405,7 +407,11 @@ class DeformableInfantryOmni if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = kNaN; + *radius_ = default_radius_; + RCLCPP_WARN_THROTTLE( + status_.get_logger(), *status_.get_clock(), 1000, + "deformable joint feedback invalid, fallback chassis radius to default %.3f m", + default_radius_); return; } From eb21d27679f01eb7ab7fa0265746755983806675 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 02:18:37 +0800 Subject: [PATCH 21/90] feat: update yaw motor zero point in deformable infantry configuration --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index d835419b..eaf4a736 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit d835419bcc0a0e62eb22af298f9259f2c57a8d75 +Subproject commit eaf4a73625996ec3443fac333421e3f9c93e91c0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index a235d389..817e0a62 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -48,7 +48,7 @@ deformable_infantry: left_back_zero_point: 5167 right_back_zero_point: 3098 right_front_zero_point: 6485 - yaw_motor_zero_point: 18211 + yaw_motor_zero_point: 57900 pitch_motor_zero_point: 56354 debug_log_supercap: false debug_log_wheel_motor: false From fa7676c47553c4307b0cb760f1d67cedbb9719a3 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 02:55:11 +0800 Subject: [PATCH 22/90] feat: update serial filter for top board and enhance top board functionality in deformable infantry --- .../config/deformable-infantry-omni.yaml | 2 +- .../src/hardware/deformable-infantry-omni.cpp | 98 ++++++++++++++++--- 2 files changed, 83 insertions(+), 17 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index bb968c23..5eb1c098 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -41,7 +41,7 @@ value_broadcaster: deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" + serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" left_front_zero_point: 374 left_back_zero_point: 5801 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 11b162a2..0338686a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include #include @@ -53,7 +52,7 @@ class DeformableInfantryOmni bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string()); + *this, *command_, get_parameter("serial_filter_top_board").as_string(), true); imu_board_ = std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); @@ -68,6 +67,8 @@ class DeformableInfantryOmni ~DeformableInfantryOmni() override = default; + void before_updating() override { top_board_->request_hard_sync_read(); } + void update() override { bottom_board_->update(); top_board_->update(); @@ -615,15 +616,17 @@ class DeformableInfantryOmni device::Bmi088 bmi088_; }; - class TopBoard final : private librmcs::agent::CBoard { + class TopBoard final : private librmcs::agent::RmcsBoardLite { friend class DeformableInfantryOmni; public: explicit TopBoard( - DeformableInfantryOmni& status, Command& command, const std::string& serial_filter = {}) - : librmcs::agent::CBoard( + DeformableInfantryOmni& status, Command& command, std::string serial_filter = {}, + bool has_external_imu_board = false) + : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , has_external_imu_board_(has_external_imu_board) , tf_(status.tf_) , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(status, command, "/gimbal/pitch") @@ -647,16 +650,26 @@ class DeformableInfantryOmni scope_motor_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + if (!has_external_imu_board_) + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + bmi088_.set_coordinate_mapping([](double x, double y, double z) { + // Top board BMI088 maps to gimbal frame as (-x, -y, z). + return std::make_tuple(y, -x, z); + }); } ~TopBoard() override = default; + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + void update() { bmi088_.update_status(); + gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); @@ -664,7 +677,24 @@ class DeformableInfantryOmni const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + if (!has_external_imu_board_) { + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> + // OdomImu, so use the encoder pitch from the TF tree to move the + // yaw-link pose back into PitchLink. + tf_->set_transform( + pitch_link_to_odom_imu); + } tf_->set_state( pitch_encoder_angle); @@ -672,17 +702,41 @@ class DeformableInfantryOmni void command_update() { auto builder = start_transmit(); - builder.can1_transmit({ + builder.can0_transmit({ .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can2_transmit({ + builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, gimbal_right_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, scope_motor_.generate_command(), device::CanPacket8::PaddingQuarter{}, } @@ -693,21 +747,31 @@ class DeformableInfantryOmni private: void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; if (data.can_id == 0x141) gimbal_pitch_motor_.store_status(data.can_data); } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; if (data.can_id == 0x201) gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x202) gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x203) scope_motor_.store_status(data.can_data); } @@ -720,8 +784,10 @@ class DeformableInfantryOmni bmi088_.store_gyroscope_status(data.x, data.y, data.z); } + bool has_external_imu_board_ = false; OutputInterface& tf_; - OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_encoder_; device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; From ed1ade5829aebde3fd8ef6dae6735d3440fde1db Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Thu, 21 May 2026 02:00:51 +0800 Subject: [PATCH 23/90] vt13 test --- .../rmcs_core/src/hardware/device/dr16.hpp | 322 ++++------ .../src/hardware/device/remote_control.hpp | 94 +++ .../rmcs_core/src/hardware/device/vt13.hpp | 198 ++++++ .../rmcs_core/src/hardware/omni_infantry.cpp | 55 +- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 500 --------------- .../rmcs_core/src/hardware/steering-hero.cpp | 591 ------------------ .../src/hardware/steering-infantry.cpp | 537 ---------------- .../include/rmcs_utility/ring_buffer.hpp | 37 ++ 8 files changed, 478 insertions(+), 1856 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp 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 75152b17..6bd0ac2f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,16 +1,13 @@ #pragma once +#include +#include #include #include #include - -#include -#include +#include #include -#include -#include -#include #include #include #include @@ -19,230 +16,165 @@ namespace rmcs_core::hardware::device { class Dr16 { public: - explicit Dr16(rmcs_executor::Component& component) { - component.register_output( - "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); - component.register_output( - "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); - - component.register_output( - "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); - component.register_output( - "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); - - component.register_output( - "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - - component.register_output("/remote/mouse", mouse_output_); - std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); - component.register_output("/remote/keyboard", keyboard_output_); - std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - - component.register_output("/remote/rotary_knob", rotary_knob_output_); - - // Simulate the rotary knob as a switch, with anti-shake algorithm. - component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); - } + Dr16() = default; - void store_status(const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != 6 + 8 + 4) + void store_status(std::span uart_data) { + if (uart_data.size() != kStatusSize) return; - // Avoid using reinterpret_cast here because it does not account for pointer alignment. - // Dr16DataPart structures are aligned, and using reinterpret_cast on potentially unaligned - // uart_data can cause undefined behavior on architectures that enforce strict alignment - // requirements (e.g., ARM). - // Directly accessing unaligned memory through a casted pointer can lead to crashes, - // inefficiencies, or incorrect data reads. Instead, std::memcpy safely copies the data from - // unaligned memory to properly aligned structures without violating alignment or strict - // aliasing rules. + auto* cursor = uart_data.data(); uint64_t part1{}; - std::memcpy(&part1, uart_data, 6); - uart_data += 6; + std::memcpy(&part1, cursor, kPart1Size); + cursor += kPart1Size; data_part1_.store(part1, std::memory_order::relaxed); uint64_t part2{}; - std::memcpy(&part2, uart_data, 8); - uart_data += 8; + std::memcpy(&part2, cursor, kPart2Size); + cursor += kPart2Size; data_part2_.store(part2, std::memory_order::relaxed); uint32_t part3{}; - std::memcpy(&part3, uart_data, 4); - uart_data += 4; + std::memcpy(&part3, cursor, kPart3Size); data_part3_.store(part3, std::memory_order::relaxed); } void update_status() { - auto part1 alignas(uint64_t) = - std::bit_cast(data_part1_.load(std::memory_order::relaxed)); - - auto channel_to_double = [](int32_t value) { - value -= 1024; - if (-660 <= value && value <= 660) - return value / 660.0; - return 0.0; - }; - joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); - joystick_right_.x = channel_to_double(static_cast(part1.joystick_channel1)); - joystick_left_.y = -channel_to_double(static_cast(part1.joystick_channel2)); - joystick_left_.x = channel_to_double(static_cast(part1.joystick_channel3)); - - switch_right_ = static_cast(part1.switch_right); - switch_left_ = static_cast(part1.switch_left); + const auto raw_part1 = data_part1_.load(std::memory_order::relaxed); + const auto part1 = std::bit_cast(raw_part1); - auto part2 alignas(uint64_t) = - std::bit_cast(data_part2_.load(std::memory_order::relaxed)); + joystick_right_ = { + channel_to_double(static_cast(part1.joystick_channel1)), + -channel_to_double(static_cast(part1.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(part1.joystick_channel3)), + -channel_to_double(static_cast(part1.joystick_channel2)), + }; - mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; - mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; + const auto raw_part2 = data_part2_.load(std::memory_order::relaxed); + const auto part2 = std::bit_cast(raw_part2); - mouse_.left = part2.mouse_left; - mouse_.right = part2.mouse_right; + mouse_velocity_ = { + -static_cast(part2.mouse_velocity_y) / 32768.0, + -static_cast(part2.mouse_velocity_x) / 32768.0, + }; + mouse_wheel_ = -static_cast(part2.mouse_velocity_z) / 32768.0; + mouse_ = { + .left = part2.mouse_left, + .right = part2.mouse_right, + }; - auto part3 alignas(uint32_t) = - std::bit_cast(data_part3_.load(std::memory_order::relaxed)); + const auto raw_part3 = data_part3_.load(std::memory_order::relaxed); + const auto part3 = std::bit_cast(raw_part3); - keyboard_ = part3.keyboard; + keyboard_ = std::bit_cast(part3.keyboard); rotary_knob_ = channel_to_double(part3.rotary_knob); - - *joystick_right_output_ = joystick_right(); - *joystick_left_output_ = joystick_left(); - - *switch_right_output_ = switch_right(); - *switch_left_output_ = switch_left(); - - *mouse_velocity_output_ = mouse_velocity(); - *mouse_wheel_output_ = mouse_wheel(); - - *mouse_output_ = mouse(); - *keyboard_output_ = keyboard(); - - *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } - struct Vector { - constexpr static Vector zero() { return {.x = 0, .y = 0}; } - double x, y; - }; - - enum class Switch : uint8_t { kUnknown = 0, kUp = 1, kDown = 2, kMiddle = 3 }; - - struct [[gnu::packed]] Mouse { - constexpr static Mouse zero() { - constexpr uint8_t zero = 0; - return std::bit_cast(zero); - } + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } - bool left : 1; - bool right : 1; - }; - static_assert(sizeof(Mouse) == 1); + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } - struct [[gnu::packed]] Keyboard { - constexpr static Keyboard zero() { - constexpr uint16_t zero = 0; - return std::bit_cast(zero); - } + [[nodiscard]] rmcs_msgs::Switch switch_right() const noexcept { return switch_right_; } - bool w : 1; - bool s : 1; - bool a : 1; - bool d : 1; - bool shift : 1; - bool ctrl : 1; - bool q : 1; - bool e : 1; - bool r : 1; - bool f : 1; - bool g : 1; - bool z : 1; - bool x : 1; - bool c : 1; - bool v : 1; - bool b : 1; - }; - static_assert(sizeof(Keyboard) == 2); + [[nodiscard]] rmcs_msgs::Switch switch_left() const noexcept { return switch_left_; } - Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } - Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } - rmcs_msgs::Switch switch_right() const { - return std::bit_cast(switch_right_); - } - rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } - Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } - rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } - rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + [[nodiscard]] double rotary_knob() const noexcept { return rotary_knob_; } - double rotary_knob() const { return rotary_knob_; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } - double mouse_wheel() const { return mouse_wheel_; } + [[nodiscard]] rmcs_msgs::Switch rotary_knob_switch() const noexcept { + return rotary_knob_switch_; + } private: - static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } - - void update_rotary_knob_switch() { - constexpr double divider = 0.7, anti_shake_shift = 0.05; - double upper_divider = divider, lower_divider = -divider; - - auto& switch_value = *rotary_knob_switch_output_; - if (switch_value == rmcs_msgs::Switch::UP) - upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::MIDDLE) - upper_divider += anti_shake_shift, lower_divider -= anti_shake_shift; - else if (switch_value == rmcs_msgs::Switch::DOWN) - upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - - const auto knob_value = -*rotary_knob_output_; - if (knob_value > upper_divider) { - switch_value = rmcs_msgs::Switch::UP; - } else if (knob_value < lower_divider) { - switch_value = rmcs_msgs::Switch::DOWN; - } else { - switch_value = rmcs_msgs::Switch::MIDDLE; - } - } + static constexpr std::size_t kPart1Size = 6; + static constexpr std::size_t kPart2Size = 8; + static constexpr std::size_t kPart3Size = 4; + static constexpr std::size_t kStatusSize = kPart1Size + kPart2Size + kPart3Size; struct [[gnu::packed]] Dr16DataPart1 { uint64_t joystick_channel0 : 11; uint64_t joystick_channel1 : 11; uint64_t joystick_channel2 : 11; uint64_t joystick_channel3 : 11; - uint64_t switch_right : 2; - uint64_t switch_left : 2; - + uint64_t switch_left : 2; uint64_t padding : 16; }; static_assert(sizeof(Dr16DataPart1) == 8); - std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ - .joystick_channel0 = 1024, - .joystick_channel1 = 1024, - .joystick_channel2 = 1024, - .joystick_channel3 = 1024, - .switch_right = static_cast(Switch::kUnknown), - .switch_left = static_cast(Switch::kUnknown), - .padding = 0, - })}; - static_assert(decltype(data_part1_)::is_always_lock_free); struct [[gnu::packed]] Dr16DataPart2 { int16_t mouse_velocity_x; int16_t mouse_velocity_y; int16_t mouse_velocity_z; - bool mouse_left; bool mouse_right; }; static_assert(sizeof(Dr16DataPart2) == 8); + + struct [[gnu::packed]] Dr16DataPart3 { + uint16_t keyboard; + uint16_t rotary_knob; + }; + static_assert(sizeof(Dr16DataPart3) == 4); + + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + + void update_rotary_knob_switch() { + constexpr double divider = 0.7; + constexpr double anti_shake_shift = 0.05; + + double upper_divider = divider; + double lower_divider = -divider; + if (rotary_knob_switch_ == rmcs_msgs::Switch::UP) { + upper_divider -= anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::MIDDLE) { + upper_divider += anti_shake_shift; + lower_divider -= anti_shake_shift; + } else if (rotary_knob_switch_ == rmcs_msgs::Switch::DOWN) { + upper_divider += anti_shake_shift; + lower_divider += anti_shake_shift; + } + + const auto knob_value = -rotary_knob_; + if (knob_value > upper_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::UP; + } else if (knob_value < lower_divider) { + rotary_knob_switch_ = rmcs_msgs::Switch::DOWN; + } else { + rotary_knob_switch_ = rmcs_msgs::Switch::MIDDLE; + } + } + + std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ + .joystick_channel0 = 1024, + .joystick_channel1 = 1024, + .joystick_channel2 = 1024, + .joystick_channel3 = 1024, + .switch_right = static_cast(rmcs_msgs::Switch::UNKNOWN), + .switch_left = static_cast(rmcs_msgs::Switch::UNKNOWN), + .padding = 0, + })}; + static_assert(decltype(data_part1_)::is_always_lock_free); + std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ .mouse_velocity_x = 0, .mouse_velocity_y = 0, @@ -252,45 +184,25 @@ class Dr16 { })}; static_assert(decltype(data_part2_)::is_always_lock_free); - struct [[gnu::packed]] Dr16DataPart3 { - Keyboard keyboard; - uint16_t rotary_knob; - }; - static_assert(sizeof(Dr16DataPart3) == 4); - std::atomic data_part3_ = {std::bit_cast(Dr16DataPart3{ - .keyboard = Keyboard::zero(), + std::atomic data_part3_{std::bit_cast(Dr16DataPart3{ + .keyboard = 0, .rotary_knob = 0, })}; static_assert(decltype(data_part3_)::is_always_lock_free); - Vector joystick_right_ = Vector::zero(); - Vector joystick_left_ = Vector::zero(); - - Switch switch_right_ = Switch::kUnknown; - Switch switch_left_ = Switch::kUnknown; + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); - Vector mouse_velocity_ = Vector::zero(); + rmcs_msgs::Switch switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; - Mouse mouse_ = Mouse::zero(); - Keyboard keyboard_ = Keyboard::zero(); + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); double rotary_knob_ = 0.0; double mouse_wheel_ = 0.0; - - rmcs_executor::Component::OutputInterface joystick_right_output_; - rmcs_executor::Component::OutputInterface joystick_left_output_; - - rmcs_executor::Component::OutputInterface switch_right_output_; - rmcs_executor::Component::OutputInterface switch_left_output_; - - rmcs_executor::Component::OutputInterface mouse_velocity_output_; - rmcs_executor::Component::OutputInterface mouse_wheel_output_; - - rmcs_executor::Component::OutputInterface mouse_output_; - rmcs_executor::Component::OutputInterface keyboard_output_; - - rmcs_executor::Component::OutputInterface rotary_knob_output_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp new file mode 100644 index 00000000..99b36c3b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -0,0 +1,94 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "hardware/device/dr16.hpp" +#include "hardware/device/vt13.hpp" + +namespace rmcs_core::hardware::device { + +class RemoteControl { +public: + RemoteControl(rmcs_executor::Component& component, Dr16& dr16, Vt13& vt13) + : dr16_(dr16) + , vt13_(vt13) { + component.register_output( + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); + + component.register_output( + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output("/remote/rotary_knob", rotary_knob_output_, 0.0); + component.register_output( + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + + component.register_output( + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_, 0.0); + + component.register_output("/remote/mouse", mouse_output_, rmcs_msgs::Mouse::zero()); + component.register_output( + "/remote/keyboard", keyboard_output_, rmcs_msgs::Keyboard::zero()); + } + + void update() { + *joystick_right_output_ = dr16_.joystick_right(); + *joystick_left_output_ = dr16_.joystick_left(); + + if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { + *switch_right_output_ = rmcs_msgs::Switch::DOWN; + *switch_left_output_ = rmcs_msgs::Switch::DOWN; + } else { + *switch_right_output_ = dr16_.switch_right(); + *switch_left_output_ = dr16_.switch_left(); + } + + *rotary_knob_output_ = dr16_.rotary_knob(); + *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); + + if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *mouse_velocity_output_ = vt13_.mouse_velocity(); + *mouse_wheel_output_ = vt13_.mouse_wheel(); + + *mouse_output_ = vt13_.mouse(); + *keyboard_output_ = vt13_.keyboard(); + } else { + *mouse_velocity_output_ = dr16_.mouse_velocity(); + *mouse_wheel_output_ = dr16_.mouse_wheel(); + + *mouse_output_ = dr16_.mouse(); + *keyboard_output_ = dr16_.keyboard(); + } + } + +private: + Dr16& dr16_; + Vt13& vt13_; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; + + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; + + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; + + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; + + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp new file mode 100644 index 00000000..6cf34d9e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -0,0 +1,198 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class Vt13 { +public: + enum class ModeSwitch : uint8_t { + kUnknown = 0, + kCine = 1, + kNormal = 2, + kSport = 3, + }; + + Vt13() = default; + + void store_status(std::span uart_data) { + const auto written = data_buffer_.emplace_back_n( + [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { + *storage = *iter++; + }, + uart_data.size()); + if (written != uart_data.size()) { + RCLCPP_WARN( + rclcpp::get_logger("vt13"), "VT13 input buffer overflow: dropped %zu of %zu bytes", + uart_data.size() - written, uart_data.size()); + } + } + + void update_status() { + auto readable = data_buffer_.readable(); + if (!readable) + return; + + while (readable) { + ReadResult result = VerificationFailed{}; + + const std::byte front = *data_buffer_.peek_front(); + if (front == std::byte{0xa9}) + result = read_remote_control_data(readable); + else if (front == std::byte(0xa5)) + result = read_referee_style_data(readable); + + if (std::holds_alternative(result)) { + break; + } + if (std::holds_alternative(result)) { + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; + } + if (std::holds_alternative(result)) { + readable -= std::get(result).read; + continue; + } + } + } + + [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } + + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } + [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } + + [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } + [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + +private: + struct Incomplete {}; + struct VerificationFailed {}; + struct Success { + std::size_t read; + }; + using ReadResult = std::variant; + + struct [[gnu::packed]] RemoteControlData { + static constexpr uint16_t kHeaderMagic = 0x53a9; + + uint16_t header; + + uint16_t joystick_channel0 : 11; + uint16_t joystick_channel1 : 11; + uint16_t joystick_channel2 : 11; + uint16_t joystick_channel3 : 11; + + uint8_t mode_switch : 2; + uint8_t pause_button : 1; + uint8_t left_custom_button : 1; + uint8_t right_custom_button : 1; + uint16_t dial : 11; + uint8_t trigger : 1; + uint8_t padding1 : 3; + + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + uint8_t mouse_left : 2; + uint8_t mouse_right : 2; + uint8_t mouse_middle : 2; + uint8_t padding2 : 2; + + uint16_t keyboard; + + uint16_t crc16; + }; + + struct [[gnu::packed]] RefereeFrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; + }; + + ReadResult read_remote_control_data(const std::size_t readable) { + if (readable < sizeof(RemoteControlData)) + return Incomplete{}; + + RemoteControlData data; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&data)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RemoteControlData)); + + if (data.header != RemoteControlData::kHeaderMagic + || !rmcs_utility::dji_crc::verify_crc16(data)) + return VerificationFailed{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); + + update_remote_control_data(data); + return Success{sizeof(RemoteControlData)}; + } + + void update_remote_control_data(const RemoteControlData& data) { + mode_switch_ = static_cast(data.mode_switch + 1); + + mouse_velocity_ = { + -data.mouse_velocity_y / 32768.0, + -data.mouse_velocity_x / 32768.0, + }; + mouse_wheel_ = -static_cast(data.mouse_velocity_z) / 32768.0; + + mouse_ = { + .left = static_cast(data.mouse_left), + .right = static_cast(data.mouse_right), + }; + keyboard_ = std::bit_cast(data.keyboard); + } + + ReadResult read_referee_style_data(const std::size_t readable) { + if (readable < sizeof(RefereeFrameHeader)) + return Incomplete{}; + + RefereeFrameHeader header; + data_buffer_.peek_front_n( + [dst = reinterpret_cast(&header)](std::byte src) mutable noexcept { + *dst++ = src; + }, + sizeof(RefereeFrameHeader)); + + if (!rmcs_utility::dji_crc::verify_crc8(header)) + return VerificationFailed{}; + + const std::size_t total_frame_size = + sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (readable < total_frame_size) + return Incomplete{}; + + data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + return Success{total_frame_size}; + } + + rmcs_utility::RingBuffer data_buffer_{1024}; + + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); + double mouse_wheel_ = 0; + + rmcs_msgs::Mouse mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 234d5aa7..3c01ac5a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include #include @@ -25,20 +25,22 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { class OmniInfantry : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::CBoard { + , private librmcs::agent::RmcsBoardLite { public: OmniInfantry() : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} + , librmcs::agent::RmcsBoardLite(get_parameter("board_serial").as_string(), {true}) , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -53,7 +55,7 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} + , remote_control_(*this, dr16_, vt13_) , bmi088_(1000, 0.2, 0.0) { for (auto& motor : chassis_wheel_motors_) @@ -126,9 +128,8 @@ class OmniInfantry [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); return size; }; } @@ -144,6 +145,8 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); + vt13_.update_status(); + remote_control_.update(); supercap_.update_status(); } @@ -154,11 +157,11 @@ class OmniInfantry .can_id = 0x1FE, .can_data = device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } .as_bytes(), }); @@ -171,11 +174,11 @@ class OmniInfantry .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } .as_bytes(), }); @@ -188,11 +191,11 @@ class OmniInfantry .can_id = 0x200, .can_data = device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } .as_bytes(), }); } @@ -277,6 +280,10 @@ class OmniInfantry } } + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const auto* uart_data = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( @@ -285,7 +292,7 @@ class OmniInfantry } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { @@ -324,6 +331,8 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; + device::Vt13 vt13_; + device::RemoteControl remote_control_; device::Bmi088 bmi088_; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp deleted file mode 100644 index 9d917a2b..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ /dev/null @@ -1,500 +0,0 @@ -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class Sentry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Sentry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { - - register_input("/predefined/timestamp", timestamp_); - register_output("/tf", tf_); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - gimbal_board_ = - std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.08, 0.0, 0.0}); - tf_->set_transform( - Eigen::Translation3d{0.07128, 0.0, 0.0481}); - } - - auto update() -> void override { - top_board_->update(); - bottom_board_->update(); - gimbal_board_->update(); - tf_->set_transform( - gimbal_board_->imu_pose().conjugate()); - } - -private: - class GimbalBoard final : private librmcs::agent::CBoard { - public: - explicit GimbalBoard(std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) { - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); - } - - GimbalBoard(const GimbalBoard&) = delete; - GimbalBoard& operator=(const GimbalBoard&) = delete; - GimbalBoard(GimbalBoard&&) = delete; - GimbalBoard& operator=(GimbalBoard&&) = delete; - - ~GimbalBoard() override = default; - - auto update() -> void { - bmi088_.update_status(); - imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - } - - auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } - - private: - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 bmi088_{1000, 0.2, 0.0}; - Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class Sentry; - - public: - explicit TopBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}, librmcs::agent::AdvancedOptions options = {}) - : librmcs::agent::RmcsBoardLite(board_serial, options) - , tf_(sentry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") - , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") - , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") - , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") - , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_top_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); - } - - auto update() -> void { - gimbal_top_yaw_motor_.update_status(); - gimbal_pitch_motor_.update_status(); - - const auto pitch_angle = - std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); - - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_bullet_feeder_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_top_yaw_motor_.angle()); - tf_->set_state(pitch_angle); - } - - auto command_update() -> void { - auto builder = start_transmit(); - - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - } - .as_bytes(), - }); - - builder.can3_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), - }); - } - - private: - auto can0_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x202) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_right_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - auto can3_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x141) - gimbal_top_yaw_motor_.store_status(data.can_data); - } - - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::LkMotor gimbal_top_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - friend class Sentry; - - public: - explicit BottomBoard( - Sentry& sentry, rmcs_executor::Component& sentry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(sentry.tf_) - , dr16_(sentry) - , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") - , chassis_wheel_motors_( - {sentry, sentry_command, "/chassis/left_front_wheel"}, - {sentry, sentry_command, "/chassis/left_back_wheel"}, - {sentry, sentry_command, "/chassis/right_back_wheel"}, - {sentry, sentry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {sentry, sentry_command, "/chassis/left_front_steering"}, - {sentry, sentry_command, "/chassis/left_back_steering"}, - {sentry, sentry_command, "/chassis/right_back_steering"}, - {sentry, sentry_command, "/chassis/right_front_steering"}) - , supercap_(sentry, sentry_command) { - sentry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); - gimbal_bottom_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point))); - - for (auto& motor : chassis_wheel_motors_) { - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - } - - constexpr auto kSteerNames = std::array{ - "right_back_zero_point", - "right_front_zero_point", - "left_front_zero_point", - "left_back_zero_point", - }; - for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { - const auto zero_point = sentry.get_parameter(name).as_int(); - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point(static_cast(zero_point)) - .enable_multi_turn_angle()); - } - - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - auto update() -> void { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - auto command_update() -> void { - using namespace device; - - auto builder = start_transmit(); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - - auto cache = CanPacket8{}; - auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { - auto command = [&](T arg) { - if constexpr (std::same_as) { - return arg; - } else { - const auto valid = arg >= 0 && arg < 4; - return valid ? motors[arg].generate_command() - : CanPacket8::PaddingQuarter{}; - } - }; - cache = CanPacket8{command(args)...}; - return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; - }; - - if (can_transmission_mode_) { - builder.can1_transmit(generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) - .can2_transmit(generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); - } else { - builder.can1_transmit(generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) - .can2_transmit(generate( - 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - auto dbus_receive_callback(const librmcs::data::UartDataView& data) -> void override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - auto can1_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x141) - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - - auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x202) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[3].store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); - } - - auto uart1_receive_callback(const librmcs::data::UartDataView& data) -> void override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) - -> void override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) - -> void override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - - device::Dr16 dr16_; - device::LkMotor gimbal_bottom_yaw_motor_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - }; - - struct CommandTransmitter : public rmcs_executor::Component { - std::function fn; - - template - explicit CommandTransmitter(Fn&& fn) - : fn{std::forward(fn)} {} - - auto update() -> void override { fn(); } - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); - text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto kPosition = - std::array{"right back", "right front", "left front", "left back"}; - constexpr auto kMaxLength = - std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { - text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - auto command_update() -> void { - top_board_->command_update(); - bottom_board_->command_update(); - } - std::shared_ptr command_component_{ - create_partner_component( - get_component_name() + "_command", [this] { command_update(); })}; - - InputInterface timestamp_; - OutputInterface tf_; - - std::unique_ptr gimbal_board_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - std::shared_ptr> status_service_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp deleted file mode 100644 index d30c1ce0..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ /dev/null @@ -1,591 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/benewake.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringHero - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHero() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - - register_output("/tf", tf_); - tf_->set_transform( - Eigen::Translation3d{0.16, 0.0, 0.15}); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - temperature_logging_timer_.reset(1000); - } - - SteeringHero(const SteeringHero&) = delete; - SteeringHero& operator=(const SteeringHero&) = delete; - SteeringHero(SteeringHero&&) = delete; - SteeringHero& operator=(SteeringHero&&) = delete; - - ~SteeringHero() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroCommand(SteeringHero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - private: - SteeringHero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit TopBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(hero.tf_) - , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) - , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio( - 1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) { - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-x, -y, z); - }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() final = default; - - void update() { - imu_.update_status(); - const Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - benewake_.update_status(); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_friction_wheels_[0].generate_command(), - gimbal_friction_wheels_[1].generate_command(), - gimbal_friction_wheels_[2].generate_command(), - gimbal_friction_wheels_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x141, - .can_data = gimbal_bullet_feeder_ - .generate_torque_command(gimbal_bullet_feeder_.control_torque()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x143, - .can_data = - gimbal_player_viewer_motor_ - .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - gimbal_friction_wheels_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(data.can_data); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(data.can_data); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - benewake_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::Benewake benewake_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_friction_wheels_[4]; - device::LkMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) - , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) { - - hero.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); - return size; - }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() final = default; - - void update() { - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steering_motors_[0].generate_command(), - chassis_steering_motors_[1].generate_command(), - chassis_steering_motors_[2].generate_command(), - chassis_steering_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x141, - .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(data.can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(data.can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(data.can_data); - } else if (can_id == 0x300) { - supercap_.store_status(data.can_data); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(data.can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(data.can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(data.can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(data.can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_motor_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - device::Bmi088 imu_; - OutputInterface& tf_; - - OutputInterface chassis_yaw_velocity_imu_; - - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::LkMotor gimbal_bottom_yaw_motor_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp deleted file mode 100644 index 2b30039e..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ /dev/null @@ -1,537 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringInfantry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringInfantry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_top_board").as_string()); - - bottom_board_ = std::make_unique( - *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); - - tf_->set_transform( - Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - - SteeringInfantry(const SteeringInfantry&) = delete; - SteeringInfantry& operator=(const SteeringInfantry&) = delete; - SteeringInfantry(SteeringInfantry&&) = delete; - SteeringInfantry& operator=(SteeringInfantry&&) = delete; - - ~SteeringInfantry() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - } - - void command_update() { - - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); - } - class SteeringInfantryCommand : public rmcs_executor::Component { - public: - explicit SteeringInfantryCommand(SteeringInfantry& steering_infantry) - : steering_infantry(steering_infantry) {} - - void update() override { steering_infantry.command_update(); } - - SteeringInfantry& steering_infantry; - }; - - class TopBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - explicit TopBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , tf_(steering_infantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(steering_infantry, steering_infantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - steering_infantry, steering_infantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - steering_infantry, steering_infantry_command, "/gimbal/right_friction") { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - - steering_infantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - steering_infantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - // Eigen::AngleAxisd pitch_link_to_bmi088_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); - } - - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() override = default; - - void update() { - bmi088_.update_status(); - const Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_pitch_motor_.angle()); - } - - void command_update() { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command(), - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_pitch_motor_ - .generate_velocity_command(gimbal_pitch_motor_.control_velocity()) - .as_bytes(), - }); - } - - private: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x203) { - gimbal_left_friction_.store_status(data.can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(data.can_data); - } - } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x142) - gimbal_pitch_motor_.store_status(data.can_data); - } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - class BottomBoard final : private librmcs::agent::CBoard { - public: - friend class SteeringInfantry; - - explicit BottomBoard( - SteeringInfantry& steering_infantry, SteeringInfantryCommand& steering_infantry_command, - std::string_view board_serial = {}) - : librmcs::agent::CBoard(board_serial) - , imu_(1000, 0.2, 0.0) - , tf_(steering_infantry.tf_) - , dr16_(steering_infantry) - , gimbal_yaw_motor_(steering_infantry, steering_infantry_command, "/gimbal/yaw") - , gimbal_bullet_feeder_( - steering_infantry, steering_infantry_command, "/gimbal/bullet_feeder") - , chassis_wheel_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_wheel"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {steering_infantry, steering_infantry_command, "/chassis/left_front_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/left_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_back_steering"}, - {steering_infantry, steering_infantry_command, "/chassis/right_front_steering"}) - , supercap_(steering_infantry, steering_infantry_command) { - - steering_infantry.register_output("/referee/serial", referee_serial_); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit({ - .uart_data = std::span{buffer, size} - }); - return size; - }; - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steering_infantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - steering_infantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gy(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - tf_->set_state( - gimbal_yaw_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - if (can_transmission_mode_) { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - - auto steer_builder = start_transmit(); - steer_builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - chassis_steer_motors_[1].generate_command(), - chassis_steer_motors_[2].generate_command(), - chassis_steer_motors_[3].generate_command(), - } - .as_bytes(), - }); - } else { - auto builder = start_transmit(); - - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[2].generate_command(), - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); - - builder.can1_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_ - .generate_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy()) - .as_bytes(), - }); - } - can_transmission_mode_ = !can_transmission_mode_; - } - - private: - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x201) - chassis_wheel_motors_[0].store_status(data.can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[1].store_status(data.can_data); - else if (can_id == 0x203) - chassis_wheel_motors_[2].store_status(data.can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(data.can_data); - else if (can_id == 0x205) - gimbal_bullet_feeder_.store_status(data.can_data); - else if (can_id == 0x300) - supercap_.store_status(data.can_data); - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - auto can_id = data.can_id; - if (can_id == 0x142) - gimbal_yaw_motor_.store_status(data.can_data); - else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(data.can_data); - else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(data.can_data); - else if (can_id == 0x208) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - const auto* uart_data = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, - data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - bool can_transmission_mode_ = true; - device::Bmi088 imu_; - OutputInterface& tf_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - device::LkMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - }; - - OutputInterface tf_; - - std::shared_ptr command_component_; - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp index 2c829c64..82a93fba 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp @@ -90,6 +90,43 @@ class RingBuffer { return std::launder(reinterpret_cast(storage_[out & mask_].data)); } + /*! + * @brief Visit elements from the head without consuming them + * @tparam F Functor with signature `void(T&)` + * @param callback_functor Invoked for each readable element in FIFO order + * @param count Maximum number of elements to inspect (defaults to all available) + * @return Number of elements actually visited + * @note Consumer-only. Does not advance `out_` or destroy elements. + * Mutations performed through the callback are applied in place to the + * buffered elements. + */ + template + requires requires(F& f, T& t) { + { f(t) } noexcept; + } size_t peek_front_n(F callback_functor, size_t count = std::numeric_limits::max()) { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + + const auto readable = in - out; + count = std::min(count, readable); + if (!count) + return 0; + + const auto offset = out & mask_; + const auto slice = std::min(count, max_size() - offset); + + auto process = [&callback_functor](std::byte* storage) { + auto& element = *std::launder(reinterpret_cast(storage)); + callback_functor(element); + }; + for (size_t i = 0; i < slice; i++) + process(storage_[offset + i].data); + for (size_t i = 0; i < count - slice; i++) + process(storage_[i].data); + + return count; + } + /*! * @brief Peek the last produced element (consumer side) * @return Pointer to the last element, or nullptr if empty From e241d44764194e34ed512f84df700311a2cf4523 Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Thu, 21 May 2026 02:24:32 +0800 Subject: [PATCH 24/90] add joystick --- .../src/hardware/device/remote_control.hpp | 15 ++++++++----- .../rmcs_core/src/hardware/device/vt13.hpp | 22 +++++++++++++++++++ 2 files changed, 31 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index 99b36c3b..34eaeec2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -42,9 +42,6 @@ class RemoteControl { } void update() { - *joystick_right_output_ = dr16_.joystick_right(); - *joystick_left_output_ = dr16_.joystick_left(); - if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { *switch_right_output_ = rmcs_msgs::Switch::DOWN; *switch_left_output_ = rmcs_msgs::Switch::DOWN; @@ -53,22 +50,28 @@ class RemoteControl { *switch_left_output_ = dr16_.switch_left(); } - *rotary_knob_output_ = dr16_.rotary_knob(); - *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); - if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *joystick_right_output_ = vt13_.joystick_right(); + *joystick_left_output_ = vt13_.joystick_left(); + *mouse_velocity_output_ = vt13_.mouse_velocity(); *mouse_wheel_output_ = vt13_.mouse_wheel(); *mouse_output_ = vt13_.mouse(); *keyboard_output_ = vt13_.keyboard(); } else { + *joystick_right_output_ = dr16_.joystick_right(); + *joystick_left_output_ = dr16_.joystick_left(); + *mouse_velocity_output_ = dr16_.mouse_velocity(); *mouse_wheel_output_ = dr16_.mouse_wheel(); *mouse_output_ = dr16_.mouse(); *keyboard_output_ = dr16_.keyboard(); } + + *rotary_knob_output_ = dr16_.rotary_knob(); + *rotary_knob_switch_output_ = dr16_.rotary_knob_switch(); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 6cf34d9e..b8dae5ce 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -72,6 +72,9 @@ class Vt13 { [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } + [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + [[nodiscard]] const Eigen::Vector2d& mouse_velocity() const noexcept { return mouse_velocity_; } [[nodiscard]] double mouse_wheel() const noexcept { return mouse_wheel_; } @@ -148,6 +151,15 @@ class Vt13 { void update_remote_control_data(const RemoteControlData& data) { mode_switch_ = static_cast(data.mode_switch + 1); + joystick_right_ = { + channel_to_double(static_cast(data.joystick_channel1)), + -channel_to_double(static_cast(data.joystick_channel0)), + }; + joystick_left_ = { + channel_to_double(static_cast(data.joystick_channel3)), + -channel_to_double(static_cast(data.joystick_channel2)), + }; + mouse_velocity_ = { -data.mouse_velocity_y / 32768.0, -data.mouse_velocity_x / 32768.0, @@ -184,10 +196,20 @@ class Vt13 { return Success{total_frame_size}; } + static double channel_to_double(int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + } + rmcs_utility::RingBuffer data_buffer_{1024}; ModeSwitch mode_switch_ = ModeSwitch::kUnknown; + Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); + Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); double mouse_wheel_ = 0; From 0a63de81e95ed1fb2daafe739d844ce1808e8960 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 04:01:55 +0800 Subject: [PATCH 25/90] feat: integrate Vt13 and remote control into deformable infantry components --- .../hardware/deformable-infantry-omni-b.cpp | 22 +++++++++++++--- .../src/hardware/deformable-infantry-omni.cpp | 25 +++++++++++++++---- .../hardware/deformable-infantry-steering.cpp | 4 +-- 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 269083d8..865e7896 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -29,7 +29,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -71,7 +73,8 @@ class DeformableInfantryOmniB *this, *deformable_infantry_command_, get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, serial_filter_imu); + imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); + remote_control_ = std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); } ~DeformableInfantryOmniB() override = default; @@ -86,6 +89,8 @@ class DeformableInfantryOmniB top_board_->update(); if (imu_board_) imu_board_->update(); + vt13_.update_status(); + remote_control_->update(); } void command_update() { @@ -469,7 +474,7 @@ class DeformableInfantryOmniB } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -550,7 +555,7 @@ class DeformableInfantryOmniB device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{ deformable_infantry_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{deformable_infantry_}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{ @@ -615,11 +620,13 @@ class DeformableInfantryOmniB public: explicit ImuBoard( - DeformableInfantryOmniB& deformableInfantry, const std::string& serial_filter = {}) + DeformableInfantryOmniB& deformableInfantry, device::Vt13& vt13, + const std::string& serial_filter = {}) : RmcsBoardLite{ serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{deformableInfantry.tf_} + , vt13_{vt13} , bmi088_{1000, 0.2, 0.0} { deformableInfantry.register_output( @@ -643,6 +650,10 @@ class DeformableInfantryOmniB } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { bmi088_.store_accelerometer_status(data.x, data.y, data.z); @@ -654,6 +665,7 @@ class DeformableInfantryOmniB OutputInterface& tf_; OutputInterface gimbal_pitch_velocity_imu_; + device::Vt13& vt13_; device::Bmi088 bmi088_; }; @@ -837,6 +849,7 @@ class DeformableInfantryOmniB OutputInterface tf_; InputInterface timestamp_; + device::Vt13 vt13_; std::atomic hard_sync_pending_{false}; size_t hard_sync_snapshot_count_ = 0; Clock::time_point next_hard_sync_log_time_{}; @@ -845,6 +858,7 @@ class DeformableInfantryOmniB std::unique_ptr rmcs_board_lite; std::unique_ptr imu_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr> status_service_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 0338686a..28870de0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -3,7 +3,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" #include #include @@ -53,8 +55,9 @@ class DeformableInfantryOmni *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_top_board").as_string(), true); - imu_board_ = - std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); + imu_board_ = std::make_unique( + *this, vt13_, get_parameter("serial_filter_imu").as_string()); + remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); // For command: remote-status using Srv = std_srvs::srv::Trigger; @@ -73,6 +76,8 @@ class DeformableInfantryOmni bottom_board_->update(); top_board_->update(); imu_board_->update(); + vt13_.update_status(); + remote_control_->update(); } void command_update() { @@ -329,7 +334,7 @@ class DeformableInfantryOmni device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{status_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{status_}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{status_, command_, "/chassis/left_front_wheel"}, @@ -493,7 +498,7 @@ class DeformableInfantryOmni } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -574,11 +579,14 @@ class DeformableInfantryOmni friend class DeformableInfantryOmni; public: - explicit ImuBoard(DeformableInfantryOmni& status, const std::string& serial_filter = {}) + explicit ImuBoard( + DeformableInfantryOmni& status, device::Vt13& vt13, + const std::string& serial_filter = {}) : RmcsBoardLite{ serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} + , vt13_{vt13} , bmi088_{1000, 0.2, 0.0} { status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); @@ -601,6 +609,10 @@ class DeformableInfantryOmni } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { bmi088_.store_accelerometer_status(data.x, data.y, data.z); @@ -612,6 +624,7 @@ class DeformableInfantryOmni OutputInterface& tf_; OutputInterface gimbal_pitch_velocity_imu_; + device::Vt13& vt13_; device::Bmi088 bmi088_; }; @@ -825,10 +838,12 @@ class DeformableInfantryOmni OutputInterface tf_; InputInterface timestamp_; + device::Vt13 vt13_; std::unique_ptr imu_board_; std::unique_ptr top_board_; std::unique_ptr bottom_board_; + std::unique_ptr remote_control_; std::shared_ptr command_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index 5a833272..c4cace8e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -523,7 +523,7 @@ class DeformableInfantryV2 } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -610,7 +610,7 @@ class DeformableInfantryV2 device::Bmi088 imu_{1000, 0.2, 0.0}; device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_{deformable_infantry_}; + device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, From 6317815c7d21fceb81df071a3f2309534d8be6cc Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 04:11:39 +0800 Subject: [PATCH 26/90] feat: enhance remote control functionality with timeout check in Vt13 --- .../src/rmcs_core/src/hardware/device/remote_control.hpp | 9 +++++++-- rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp | 9 +++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index 34eaeec2..fda5afdc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -42,7 +43,9 @@ class RemoteControl { } void update() { - if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { + const bool vt13_fresh = vt13_.remote_control_fresh(vt13_timeout_); + + if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { *switch_right_output_ = rmcs_msgs::Switch::DOWN; *switch_left_output_ = rmcs_msgs::Switch::DOWN; } else { @@ -50,7 +53,7 @@ class RemoteControl { *switch_left_output_ = dr16_.switch_left(); } - if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { *joystick_right_output_ = vt13_.joystick_right(); *joystick_left_output_ = vt13_.joystick_left(); @@ -75,6 +78,8 @@ class RemoteControl { } private: + static constexpr auto vt13_timeout_ = std::chrono::milliseconds{250}; + Dr16& dr16_; Vt13& vt13_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index b8dae5ce..8a68eccf 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -19,6 +20,8 @@ namespace rmcs_core::hardware::device { class Vt13 { public: + using Clock = std::chrono::steady_clock; + enum class ModeSwitch : uint8_t { kUnknown = 0, kCine = 1, @@ -80,6 +83,10 @@ class Vt13 { [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } + [[nodiscard]] bool remote_control_fresh(Clock::duration timeout) const noexcept { + return last_remote_control_frame_time_ != Clock::time_point::min() + && Clock::now() - last_remote_control_frame_time_ <= timeout; + } private: struct Incomplete {}; @@ -149,6 +156,7 @@ class Vt13 { } void update_remote_control_data(const RemoteControlData& data) { + last_remote_control_frame_time_ = Clock::now(); mode_switch_ = static_cast(data.mode_switch + 1); joystick_right_ = { @@ -204,6 +212,7 @@ class Vt13 { } rmcs_utility::RingBuffer data_buffer_{1024}; + Clock::time_point last_remote_control_frame_time_ = Clock::time_point::min(); ModeSwitch mode_switch_ = ModeSwitch::kUnknown; From 1be6b3b3d90f0692878a35d50e5f37539b53b46e Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 07:31:38 +0800 Subject: [PATCH 27/90] strange --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp | 2 +- rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index eaf4a736..7fbe1bd4 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit eaf4a73625996ec3443fac333421e3f9c93e91c0 +Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 865e7896..55429648 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -717,7 +717,7 @@ class DeformableInfantryOmniB bmi088_.set_coordinate_mapping([](double x, double y, double z) { // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); + return std::make_tuple(-x, -y, z); }); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 28870de0..e9b86f23 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -669,7 +669,7 @@ class DeformableInfantryOmni bmi088_.set_coordinate_mapping([](double x, double y, double z) { // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); + return std::make_tuple(-x, -y, z); }); } From 95c71685bb3afc66537d21b61cb2a31ad179c830 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 21 May 2026 17:19:10 +0800 Subject: [PATCH 28/90] feat: add fault handling and remote control invalidation in VT13 device --- .../rmcs_core/src/hardware/device/vt13.hpp | 33 ++++++++++++++++--- 1 file changed, 29 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 8a68eccf..6a38b7fe 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -41,10 +42,16 @@ class Vt13 { RCLCPP_WARN( rclcpp::get_logger("vt13"), "VT13 input buffer overflow: dropped %zu of %zu bytes", uart_data.size() - written, uart_data.size()); + faulted_.store(true, std::memory_order_relaxed); } } void update_status() { + if (faulted_.exchange(false, std::memory_order_relaxed)) { + invalidate_remote_control(); + return; + } + auto readable = data_buffer_.readable(); if (!readable) return; @@ -62,9 +69,8 @@ class Vt13 { break; } if (std::holds_alternative(result)) { - data_buffer_.pop_front([](std::byte&&) noexcept {}); - readable--; - continue; + invalidate_remote_control(); + break; } if (std::holds_alternative(result)) { readable -= std::get(result).read; @@ -84,6 +90,8 @@ class Vt13 { [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } [[nodiscard]] bool remote_control_fresh(Clock::duration timeout) const noexcept { + if (faulted_.load(std::memory_order_relaxed)) + return false; return last_remote_control_frame_time_ != Clock::time_point::min() && Clock::now() - last_remote_control_frame_time_ <= timeout; } @@ -146,7 +154,8 @@ class Vt13 { sizeof(RemoteControlData)); if (data.header != RemoteControlData::kHeaderMagic - || !rmcs_utility::dji_crc::verify_crc16(data)) + || !rmcs_utility::dji_crc::verify_crc16(data) + || data.mode_switch > 2) return VerificationFailed{}; data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); @@ -197,6 +206,8 @@ class Vt13 { const std::size_t total_frame_size = sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; + if (total_frame_size > data_buffer_.max_size()) + return VerificationFailed{}; if (readable < total_frame_size) return Incomplete{}; @@ -204,6 +215,19 @@ class Vt13 { return Success{total_frame_size}; } + void invalidate_remote_control() { + faulted_.store(false, std::memory_order_relaxed); + data_buffer_.clear(); + last_remote_control_frame_time_ = Clock::time_point::min(); + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_.setZero(); + joystick_right_.setZero(); + mouse_velocity_.setZero(); + mouse_wheel_ = 0.0; + mouse_ = rmcs_msgs::Mouse::zero(); + keyboard_ = rmcs_msgs::Keyboard::zero(); + } + static double channel_to_double(int32_t value) { value -= 1024; if (-660 <= value && value <= 660) @@ -212,6 +236,7 @@ class Vt13 { } rmcs_utility::RingBuffer data_buffer_{1024}; + std::atomic faulted_{false}; Clock::time_point last_remote_control_frame_time_ = Clock::time_point::min(); ModeSwitch mode_switch_ = ModeSwitch::kUnknown; From b46fda54d8c7dd5d4a1a56f9a494f73c09df6fff Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 09:09:47 +0800 Subject: [PATCH 29/90] feat: update .gitmodules to include rmcs_auto_aim_v2 and serial submodules --- .codex | 0 .devcontainer/state/codex/.tmp/plugins | 1 - .gitmodules | 10 +++++++--- 3 files changed, 7 insertions(+), 4 deletions(-) delete mode 100644 .codex delete mode 160000 .devcontainer/state/codex/.tmp/plugins diff --git a/.codex b/.codex deleted file mode 100644 index e69de29b..00000000 diff --git a/.devcontainer/state/codex/.tmp/plugins b/.devcontainer/state/codex/.tmp/plugins deleted file mode 160000 index 3c463363..00000000 --- a/.devcontainer/state/codex/.tmp/plugins +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3c46336323290cda7c90d78e7a0134f47cc73713 diff --git a/.gitmodules b/.gitmodules index aeef93a2..7aa943b0 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,6 +6,10 @@ path = rmcs_ws/src/hikcamera url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git -[submodule "rmcs_ws/src/auto_aim_v2"] - path = rmcs_ws/src/auto_aim_v2 - url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git \ No newline at end of file +[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] + path = rmcs_ws/src/rmcs_auto_aim_v2 + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git + +[submodule "rmcs_ws/src/serial"] + path = rmcs_ws/src/serial + url = git@github.com:Alliance-Algorithm/ros2-serial.git From 9bf1a98a45a4bd47523776959e84eaaf78c1e76c Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 09:30:18 +0800 Subject: [PATCH 30/90] feat: update .gitmodules to remove serial submodules --- .gitmodules | 6 +----- rmcs_ws/src/serial | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) delete mode 160000 rmcs_ws/src/serial diff --git a/.gitmodules b/.gitmodules index 7aa943b0..6fd0f3b2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,8 +8,4 @@ [submodule "rmcs_ws/src/rmcs_auto_aim_v2"] path = rmcs_ws/src/rmcs_auto_aim_v2 - url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git - -[submodule "rmcs_ws/src/serial"] - path = rmcs_ws/src/serial - url = git@github.com:Alliance-Algorithm/ros2-serial.git + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git \ No newline at end of file diff --git a/rmcs_ws/src/serial b/rmcs_ws/src/serial deleted file mode 160000 index d50c4018..00000000 --- a/rmcs_ws/src/serial +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d50c401809f17cc5862bdbe37cc6e15737ba7c49 From c191f106dc0351796861ad16fd0f4af98278f06a Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 12:01:30 +0800 Subject: [PATCH 31/90] feat: update transform parameters and enhance chassis output status handling --- .../chassis/chassis_power_controller.cpp | 7 ++----- .../hardware/deformable-infantry-omni-b.cpp | 2 +- .../src/hardware/deformable-infantry-omni.cpp | 2 +- .../hardware/deformable-infantry-steering.cpp | 2 +- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 19 ++++++++++++++++++- 5 files changed, 23 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2920b48f..543750e6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -63,7 +63,7 @@ class ChassisPowerController update_virtual_buffer_energy(); - boost_mode_ = keyboard.shift || keyboard.ctrl; + boost_mode_ = keyboard.shift; *supercap_control_enabled_ = boost_mode_; update_control_power_limit(); } @@ -110,9 +110,7 @@ class ChassisPowerController double power_limit; if (*supercap_control_enabled_ && *supercap_enabled_) - power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? inf_ - : *chassis_power_limit_referee_ + 80.0; + power_limit = *chassis_power_limit_referee_ + 80.0; else power_limit = *chassis_power_limit_referee_; chassis_power_limit_expected_ = power_limit; @@ -142,7 +140,6 @@ class ChassisPowerController static_cast(std::round(*chassis_control_power_limit_))); } - static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double nan_ = std::numeric_limits::quiet_NaN(); InputInterface mode_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 55429648..0acb879b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -53,7 +53,7 @@ class DeformableInfantryOmniB register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); // For command: remote-status using Srv = std_srvs::srv::Trigger; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index e9b86f23..cfd8b398 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -49,7 +49,7 @@ class DeformableInfantryOmni register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index c4cace8e..301ed10f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -51,7 +51,7 @@ class DeformableInfantryV2 register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); // For command: remote-status using Srv = std_srvs::srv::Trigger; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c080a355..0ad65b0b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -16,6 +17,8 @@ namespace rmcs_core::referee { using namespace status; +using Clock = std::chrono::steady_clock; + class Status : public rmcs_executor::Component , public rclcpp::Node { @@ -97,8 +100,10 @@ class Status } void update() override { - if (!serial_.active()) + if (!serial_.active()) { + update_chassis_output_status_timeout(); return; + } if (cache_size_ >= sizeof(frame_.header)) { auto frame_size = sizeof(frame_.header) + sizeof(frame_.body.command_id) @@ -142,9 +147,17 @@ class Status *robot_chassis_power_ = 0.0; *robot_buffer_energy_ = 60.0; } + + update_chassis_output_status_timeout(); } private: + void update_chassis_output_status_timeout() { + if (Clock::now() - last_robot_status_update_time_ > chassis_output_status_timeout_) { + *chassis_output_status_ = true; + } + } + void process_frame() { auto command_id = frame_.body.command_id; if (command_id == 0x0001) @@ -211,6 +224,8 @@ class Status } void update_robot_status() { + last_robot_status_update_time_ = Clock::now(); + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) robot_status_watchdog_.reset(60'000); else @@ -306,6 +321,7 @@ class Status static constexpr int64_t safe_shooter_heat_limit = 50'000; // Chassis: Health priority with level 1 static constexpr double safe_chassis_power_limit = 45; + static constexpr auto chassis_output_status_timeout_ = std::chrono::milliseconds{500}; rclcpp::Logger logger_; @@ -327,6 +343,7 @@ class Status OutputInterface robot_shooter_cooling_, robot_shooter_heat_limit_; OutputInterface robot_chassis_power_limit_; OutputInterface chassis_output_status_; + Clock::time_point last_robot_status_update_time_ = Clock::now(); rmcs_utility::TickTimer power_heat_data_watchdog_; OutputInterface robot_chassis_power_; From 6f182930d192f9c3ac9d489f5a07dfabefac0f2e Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 12:03:48 +0800 Subject: [PATCH 32/90] feat:add kSport default mode in remote_control --- rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index fda5afdc..9794fb72 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -54,6 +54,8 @@ class RemoteControl { } if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; + *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; *joystick_right_output_ = vt13_.joystick_right(); *joystick_left_output_ = vt13_.joystick_left(); From d74f7d9ad012c93200a9943ca9a3a70a66cb2d6b Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Fri, 22 May 2026 13:21:21 +0800 Subject: [PATCH 33/90] feat: update serial filter for IMU and modify chassis direction indicator angle --- .../config/deformable-infantry-omni.yaml | 4 +- .../rmcs_core/src/hardware/device/dr16.hpp | 16 ++ .../src/hardware/device/remote_control.hpp | 32 ++- .../src/hardware/device/supercap.hpp | 2 +- .../rmcs_core/src/hardware/device/vt13.hpp | 203 +++++++++++++++++- .../referee/app/ui/deformable_infantry_ui.cpp | 2 +- 6 files changed, 247 insertions(+), 12 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 5eb1c098..e3fa2fef 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -42,7 +42,7 @@ deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" - serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" + serial_filter_imu: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 @@ -51,7 +51,7 @@ deformable_infantry: pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false + debug_log_deformable_joint_motor: true chassis_controller: ros__parameters: 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 6bd0ac2f..17711aa7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -22,6 +23,8 @@ class Dr16 { if (uart_data.size() != kStatusSize) return; + last_receive_time_.store(Clock::now(), std::memory_order_relaxed); + auto* cursor = uart_data.data(); uint64_t part1{}; @@ -78,6 +81,12 @@ class Dr16 { [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } + [[nodiscard]] bool valid() const noexcept { + const auto last_receive_time = last_receive_time_.load(std::memory_order_relaxed); + return last_receive_time != TimePoint::min() + && Clock::now() - last_receive_time <= kFreshTimeout; + } + [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } [[nodiscard]] rmcs_msgs::Switch switch_right() const noexcept { return switch_right_; } @@ -99,6 +108,11 @@ class Dr16 { } private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr std::size_t kPart1Size = 6; static constexpr std::size_t kPart2Size = 8; static constexpr std::size_t kPart3Size = 4; @@ -190,6 +204,8 @@ class Dr16 { })}; static_assert(decltype(data_part3_)::is_always_lock_free); + std::atomic last_receive_time_{TimePoint::min()}; + Eigen::Vector2d joystick_right_ = Eigen::Vector2d::Zero(); Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); Eigen::Vector2d mouse_velocity_ = Eigen::Vector2d::Zero(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index 9794fb72..ebaa8809 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -43,16 +43,20 @@ class RemoteControl { } void update() { +<<<<<<< HEAD const bool vt13_fresh = vt13_.remote_control_fresh(vt13_timeout_); if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { *switch_right_output_ = rmcs_msgs::Switch::DOWN; *switch_left_output_ = rmcs_msgs::Switch::DOWN; } else { +======= + if (dr16_.valid() || !vt13_.valid() || vt13_.mode_switch() == Vt13::ModeSwitch::kNormal) { +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) *switch_right_output_ = dr16_.switch_right(); *switch_left_output_ = dr16_.switch_left(); - } +<<<<<<< HEAD if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; @@ -65,6 +69,8 @@ class RemoteControl { *mouse_output_ = vt13_.mouse(); *keyboard_output_ = vt13_.keyboard(); } else { +======= +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) *joystick_right_output_ = dr16_.joystick_right(); *joystick_left_output_ = dr16_.joystick_left(); @@ -73,6 +79,30 @@ class RemoteControl { *mouse_output_ = dr16_.mouse(); *keyboard_output_ = dr16_.keyboard(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { + *switch_right_output_ = rmcs_msgs::Switch::DOWN; + *switch_left_output_ = rmcs_msgs::Switch::DOWN; + + *joystick_right_output_ = Eigen::Vector2d::Zero(); + *joystick_left_output_ = Eigen::Vector2d::Zero(); + + *mouse_velocity_output_ = Eigen::Vector2d::Zero(); + *mouse_wheel_output_ = 0; + + *mouse_output_ = rmcs_msgs::Mouse::zero(); + *keyboard_output_ = rmcs_msgs::Keyboard::zero(); + } else if (vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { + *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; + *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; + + *joystick_right_output_ = vt13_.joystick_right(); + *joystick_left_output_ = vt13_.joystick_left(); + + *mouse_velocity_output_ = vt13_.mouse_velocity(); + *mouse_wheel_output_ = vt13_.mouse_wheel(); + + *mouse_output_ = vt13_.mouse(); + *keyboard_output_ = vt13_.keyboard(); } *rotary_knob_output_ = dr16_.rotary_knob(); 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 e88fc018..087b9f96 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp @@ -54,7 +54,7 @@ class Supercap { CanPacket8::Quarter generate_command() const { SupercapCommand command; - command.enabled = *chassis_output_status_ && *supercap_control_enabled_; + command.enabled = *chassis_output_status_; const double power_limit = *supercap_charge_power_limit_; if (std::isnan(power_limit)) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 6a38b7fe..0e374a19 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -1,8 +1,15 @@ #pragma once +<<<<<<< HEAD #include +======= +#include +#include +#include +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) #include #include +#include #include #include #include @@ -33,53 +40,89 @@ class Vt13 { Vt13() = default; void store_status(std::span uart_data) { + store_calls_.fetch_add(1, std::memory_order_relaxed); + received_bytes_.fetch_add(uart_data.size(), std::memory_order_relaxed); + const auto written = data_buffer_.emplace_back_n( [iter = uart_data.cbegin()](std::byte* storage) mutable noexcept { *storage = *iter++; }, uart_data.size()); if (written != uart_data.size()) { +<<<<<<< HEAD RCLCPP_WARN( rclcpp::get_logger("vt13"), "VT13 input buffer overflow: dropped %zu of %zu bytes", uart_data.size() - written, uart_data.size()); faulted_.store(true, std::memory_order_relaxed); +======= + const auto dropped = uart_data.size() - written; + overflow_count_.fetch_add(1, std::memory_order_relaxed); + overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); + if (should_log_overflow()) { + RCLCPP_WARN( + logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, + uart_data.size()); + } +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) } } void update_status() { +<<<<<<< HEAD if (faulted_.exchange(false, std::memory_order_relaxed)) { invalidate_remote_control(); return; } +======= + const auto now = Clock::now(); +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) auto readable = data_buffer_.readable(); - if (!readable) - return; + peak_readable_ = std::max(peak_readable_, readable); while (readable) { ReadResult result = VerificationFailed{}; const std::byte front = *data_buffer_.peek_front(); if (front == std::byte{0xa9}) - result = read_remote_control_data(readable); + result = read_remote_control_data(readable, now); else if (front == std::byte(0xa5)) - result = read_referee_style_data(readable); + result = read_referee_style_data(readable, now); + else { + unknown_prefix_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 unknown prefix: front=0x%02x readable=%zu", + std::to_integer(front), readable); + } + } if (std::holds_alternative(result)) { break; } if (std::holds_alternative(result)) { +<<<<<<< HEAD invalidate_remote_control(); break; +======= + verification_failures_++; + data_buffer_.pop_front([](std::byte&&) noexcept {}); + readable--; + continue; +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) } if (std::holds_alternative(result)) { readable -= std::get(result).read; continue; } } + + refresh_validity(now); + maybe_log_statistics(now); } [[nodiscard]] ModeSwitch mode_switch() const noexcept { return mode_switch_; } + [[nodiscard]] bool valid() const noexcept { return valid_; } [[nodiscard]] const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } [[nodiscard]] const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } @@ -97,6 +140,15 @@ class Vt13 { } private: + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); + static constexpr auto kVerificationLogInterval = std::chrono::seconds(1); + static constexpr auto kOverflowLogInterval = std::chrono::seconds(1); + static constexpr auto kStatisticsLogInterval = std::chrono::seconds(5); + static constexpr std::size_t kRefereeFrameMaxSize = 256; + struct Incomplete {}; struct VerificationFailed {}; struct Success { @@ -142,7 +194,7 @@ class Vt13 { uint8_t crc8; }; - ReadResult read_remote_control_data(const std::size_t readable) { + ReadResult read_remote_control_data(const std::size_t readable, const TimePoint now) { if (readable < sizeof(RemoteControlData)) return Incomplete{}; @@ -153,14 +205,34 @@ class Vt13 { }, sizeof(RemoteControlData)); +<<<<<<< HEAD if (data.header != RemoteControlData::kHeaderMagic || !rmcs_utility::dji_crc::verify_crc16(data) || data.mode_switch > 2) +======= + if (data.header != RemoteControlData::kHeaderMagic) { + remote_bad_header_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", + data.header, readable); + } +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) + return VerificationFailed{}; + } + if (!rmcs_utility::dji_crc::verify_crc16(data)) { + remote_bad_crc_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 remote control crc16 invalid: readable=%zu", readable); return VerificationFailed{}; + } data_buffer_.pop_front_n([](std::byte&&) noexcept {}, sizeof(RemoteControlData)); update_remote_control_data(data); + valid_ = true; + last_remote_control_received_at_ = now; + remote_success_count_++; return Success{sizeof(RemoteControlData)}; } @@ -190,7 +262,7 @@ class Vt13 { keyboard_ = std::bit_cast(data.keyboard); } - ReadResult read_referee_style_data(const std::size_t readable) { + ReadResult read_referee_style_data(const std::size_t readable, const TimePoint now) { if (readable < sizeof(RefereeFrameHeader)) return Incomplete{}; @@ -201,20 +273,38 @@ class Vt13 { }, sizeof(RefereeFrameHeader)); - if (!rmcs_utility::dji_crc::verify_crc8(header)) + if (!rmcs_utility::dji_crc::verify_crc8(header)) { + referee_bad_crc8_count_++; + if (should_log_verification_failure(now)) + RCLCPP_WARN(logger_, "VT13 referee header crc8 invalid: readable=%zu", readable); return VerificationFailed{}; + } const std::size_t total_frame_size = sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; +<<<<<<< HEAD if (total_frame_size > data_buffer_.max_size()) return VerificationFailed{}; +======= + if (total_frame_size > kRefereeFrameMaxSize) { + referee_oversize_count_++; + if (should_log_verification_failure(now)) { + RCLCPP_WARN( + logger_, "VT13 referee frame oversized: data_length=%u total=%zu readable=%zu", + header.data_length, total_frame_size, readable); + } + return VerificationFailed{}; + } +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) if (readable < total_frame_size) return Incomplete{}; data_buffer_.pop_front_n([](std::byte&&) noexcept {}, total_frame_size); + referee_discarded_count_++; return Success{total_frame_size}; } +<<<<<<< HEAD void invalidate_remote_control() { faulted_.store(false, std::memory_order_relaxed); data_buffer_.clear(); @@ -224,6 +314,83 @@ class Vt13 { joystick_right_.setZero(); mouse_velocity_.setZero(); mouse_wheel_ = 0.0; +======= + bool should_log_verification_failure(const TimePoint now) { + if (last_verification_log_time_ != TimePoint::min() + && now - last_verification_log_time_ < kVerificationLogInterval) + return false; + last_verification_log_time_ = now; + return true; + } + + bool should_log_overflow() { + const auto now = Clock::now(); + if (last_overflow_log_time_ != TimePoint::min() + && now - last_overflow_log_time_ < kOverflowLogInterval) + return false; + + last_overflow_log_time_ = now; + return true; + } + + void refresh_validity(const TimePoint now) { + if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + return; + + reset_remote_control_state(); + valid_ = false; + } + + void maybe_log_statistics(const TimePoint now) { + if (last_statistics_log_time_ == TimePoint::min()) { + last_statistics_log_time_ = now; + return; + } + + const auto elapsed = now - last_statistics_log_time_; + if (elapsed < kStatisticsLogInterval) + return; + + const auto readable = data_buffer_.readable(); + const auto store_calls = store_calls_.exchange(0, std::memory_order_relaxed); + const auto received_bytes = received_bytes_.exchange(0, std::memory_order_relaxed); + const auto overflow_count = overflow_count_.exchange(0, std::memory_order_relaxed); + const auto overflow_dropped_bytes = + overflow_dropped_bytes_.exchange(0, std::memory_order_relaxed); + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + + RCLCPP_INFO( + logger_, + "VT13 stats: rx=%.1f Hz %.1f B/s remote_ok=%zu verify_fail=%zu remote_bad_header=%zu " + "remote_bad_crc=%zu referee_discarded=%zu referee_bad_crc8=%zu referee_oversize=%zu " + "unknown_prefix=%zu overflow=%llu dropped=%llu readable=%zu peak=%zu valid=%s", + static_cast(store_calls) / elapsed_seconds, + static_cast(received_bytes) / elapsed_seconds, remote_success_count_, + verification_failures_, remote_bad_header_count_, remote_bad_crc_count_, + referee_discarded_count_, referee_bad_crc8_count_, referee_oversize_count_, + unknown_prefix_count_, static_cast(overflow_count), + static_cast(overflow_dropped_bytes), readable, peak_readable_, + valid_ ? "true" : "false"); + + remote_success_count_ = 0; + verification_failures_ = 0; + remote_bad_header_count_ = 0; + remote_bad_crc_count_ = 0; + referee_discarded_count_ = 0; + referee_bad_crc8_count_ = 0; + referee_oversize_count_ = 0; + unknown_prefix_count_ = 0; + peak_readable_ = readable; + last_statistics_log_time_ = now; + } + + void reset_remote_control_state() { + mode_switch_ = ModeSwitch::kUnknown; + joystick_left_ = Eigen::Vector2d::Zero(); + joystick_right_ = Eigen::Vector2d::Zero(); + mouse_velocity_ = Eigen::Vector2d::Zero(); + mouse_wheel_ = 0; +>>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) mouse_ = rmcs_msgs::Mouse::zero(); keyboard_ = rmcs_msgs::Keyboard::zero(); } @@ -235,10 +402,32 @@ class Vt13 { return 0.0; } + rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); rmcs_utility::RingBuffer data_buffer_{1024}; std::atomic faulted_{false}; Clock::time_point last_remote_control_frame_time_ = Clock::time_point::min(); + std::atomic store_calls_{0}; + std::atomic received_bytes_{0}; + std::atomic overflow_count_{0}; + std::atomic overflow_dropped_bytes_{0}; + + TimePoint last_remote_control_received_at_ = TimePoint::min(); + TimePoint last_verification_log_time_ = TimePoint::min(); + TimePoint last_overflow_log_time_ = TimePoint::min(); + TimePoint last_statistics_log_time_ = TimePoint::min(); + + bool valid_ = false; + std::size_t peak_readable_ = 0; + std::size_t remote_success_count_ = 0; + std::size_t verification_failures_ = 0; + std::size_t remote_bad_header_count_ = 0; + std::size_t remote_bad_crc_count_ = 0; + std::size_t referee_discarded_count_ = 0; + std::size_t referee_bad_crc8_count_ = 0; + std::size_t referee_oversize_count_ = 0; + std::size_t unknown_prefix_count_ = 0; + ModeSwitch mode_switch_ = ModeSwitch::kUnknown; Eigen::Vector2d joystick_left_ = Eigen::Vector2d::Zero(); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 398f75d8..c6961758 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -126,7 +126,7 @@ class DeformableInfantry std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); }; chassis_direction_indicator_.set_color(chassis_direction_indicator_color(chassis_mode)); - chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + chassis_direction_indicator_.set_angle(to_referee_angle(0.0), 30); } static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { From 2e0034a08f525de6c5a633434dda9b1249fa9a2e Mon Sep 17 00:00:00 2001 From: qzhhhi Date: Fri, 22 May 2026 13:21:21 +0800 Subject: [PATCH 34/90] vt13 fixfixfix --- .../rmcs_core/src/hardware/device/dr16.hpp | 2 +- .../src/hardware/device/remote_control.hpp | 29 +-------- .../rmcs_core/src/hardware/device/vt13.hpp | 60 +------------------ 3 files changed, 3 insertions(+), 88 deletions(-) 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 17711aa7..cd0a8fbc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -221,4 +221,4 @@ class Dr16 { double mouse_wheel_ = 0.0; }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index ebaa8809..236f3821 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -43,34 +42,10 @@ class RemoteControl { } void update() { -<<<<<<< HEAD - const bool vt13_fresh = vt13_.remote_control_fresh(vt13_timeout_); - - if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kCine) { - *switch_right_output_ = rmcs_msgs::Switch::DOWN; - *switch_left_output_ = rmcs_msgs::Switch::DOWN; - } else { -======= if (dr16_.valid() || !vt13_.valid() || vt13_.mode_switch() == Vt13::ModeSwitch::kNormal) { ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) *switch_right_output_ = dr16_.switch_right(); *switch_left_output_ = dr16_.switch_left(); -<<<<<<< HEAD - if (vt13_fresh && vt13_.mode_switch() == Vt13::ModeSwitch::kSport) { - *switch_right_output_ = rmcs_msgs::Switch::MIDDLE; - *switch_left_output_ = rmcs_msgs::Switch::MIDDLE; - *joystick_right_output_ = vt13_.joystick_right(); - *joystick_left_output_ = vt13_.joystick_left(); - - *mouse_velocity_output_ = vt13_.mouse_velocity(); - *mouse_wheel_output_ = vt13_.mouse_wheel(); - - *mouse_output_ = vt13_.mouse(); - *keyboard_output_ = vt13_.keyboard(); - } else { -======= ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) *joystick_right_output_ = dr16_.joystick_right(); *joystick_left_output_ = dr16_.joystick_left(); @@ -110,8 +85,6 @@ class RemoteControl { } private: - static constexpr auto vt13_timeout_ = std::chrono::milliseconds{250}; - Dr16& dr16_; Vt13& vt13_; @@ -131,4 +104,4 @@ class RemoteControl { rmcs_executor::Component::OutputInterface keyboard_output_; }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 0e374a19..eca43528 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -1,12 +1,8 @@ #pragma once -<<<<<<< HEAD -#include -======= #include #include #include ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) #include #include #include @@ -28,8 +24,6 @@ namespace rmcs_core::hardware::device { class Vt13 { public: - using Clock = std::chrono::steady_clock; - enum class ModeSwitch : uint8_t { kUnknown = 0, kCine = 1, @@ -49,12 +43,6 @@ class Vt13 { }, uart_data.size()); if (written != uart_data.size()) { -<<<<<<< HEAD - RCLCPP_WARN( - rclcpp::get_logger("vt13"), "VT13 input buffer overflow: dropped %zu of %zu bytes", - uart_data.size() - written, uart_data.size()); - faulted_.store(true, std::memory_order_relaxed); -======= const auto dropped = uart_data.size() - written; overflow_count_.fetch_add(1, std::memory_order_relaxed); overflow_dropped_bytes_.fetch_add(dropped, std::memory_order_relaxed); @@ -63,20 +51,11 @@ class Vt13 { logger_, "VT13 input buffer overflow: dropped %zu of %zu bytes", dropped, uart_data.size()); } ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) } } void update_status() { -<<<<<<< HEAD - if (faulted_.exchange(false, std::memory_order_relaxed)) { - invalidate_remote_control(); - return; - } - -======= const auto now = Clock::now(); ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) auto readable = data_buffer_.readable(); peak_readable_ = std::max(peak_readable_, readable); @@ -101,15 +80,10 @@ class Vt13 { break; } if (std::holds_alternative(result)) { -<<<<<<< HEAD - invalidate_remote_control(); - break; -======= verification_failures_++; data_buffer_.pop_front([](std::byte&&) noexcept {}); readable--; continue; ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) } if (std::holds_alternative(result)) { readable -= std::get(result).read; @@ -132,12 +106,6 @@ class Vt13 { [[nodiscard]] rmcs_msgs::Mouse mouse() const noexcept { return mouse_; } [[nodiscard]] rmcs_msgs::Keyboard keyboard() const noexcept { return keyboard_; } - [[nodiscard]] bool remote_control_fresh(Clock::duration timeout) const noexcept { - if (faulted_.load(std::memory_order_relaxed)) - return false; - return last_remote_control_frame_time_ != Clock::time_point::min() - && Clock::now() - last_remote_control_frame_time_ <= timeout; - } private: using Clock = std::chrono::steady_clock; @@ -205,11 +173,6 @@ class Vt13 { }, sizeof(RemoteControlData)); -<<<<<<< HEAD - if (data.header != RemoteControlData::kHeaderMagic - || !rmcs_utility::dji_crc::verify_crc16(data) - || data.mode_switch > 2) -======= if (data.header != RemoteControlData::kHeaderMagic) { remote_bad_header_count_++; if (should_log_verification_failure(now)) { @@ -217,7 +180,6 @@ class Vt13 { logger_, "VT13 remote control header invalid: header=0x%04x readable=%zu", data.header, readable); } ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) return VerificationFailed{}; } if (!rmcs_utility::dji_crc::verify_crc16(data)) { @@ -237,7 +199,6 @@ class Vt13 { } void update_remote_control_data(const RemoteControlData& data) { - last_remote_control_frame_time_ = Clock::now(); mode_switch_ = static_cast(data.mode_switch + 1); joystick_right_ = { @@ -282,10 +243,6 @@ class Vt13 { const std::size_t total_frame_size = sizeof(RefereeFrameHeader) + 2 + header.data_length + 2; -<<<<<<< HEAD - if (total_frame_size > data_buffer_.max_size()) - return VerificationFailed{}; -======= if (total_frame_size > kRefereeFrameMaxSize) { referee_oversize_count_++; if (should_log_verification_failure(now)) { @@ -295,7 +252,6 @@ class Vt13 { } return VerificationFailed{}; } ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) if (readable < total_frame_size) return Incomplete{}; @@ -304,17 +260,6 @@ class Vt13 { return Success{total_frame_size}; } -<<<<<<< HEAD - void invalidate_remote_control() { - faulted_.store(false, std::memory_order_relaxed); - data_buffer_.clear(); - last_remote_control_frame_time_ = Clock::time_point::min(); - mode_switch_ = ModeSwitch::kUnknown; - joystick_left_.setZero(); - joystick_right_.setZero(); - mouse_velocity_.setZero(); - mouse_wheel_ = 0.0; -======= bool should_log_verification_failure(const TimePoint now) { if (last_verification_log_time_ != TimePoint::min() && now - last_verification_log_time_ < kVerificationLogInterval) @@ -390,7 +335,6 @@ class Vt13 { joystick_right_ = Eigen::Vector2d::Zero(); mouse_velocity_ = Eigen::Vector2d::Zero(); mouse_wheel_ = 0; ->>>>>>> 7a9bbcf6 (vt13 fixfixfixfix) mouse_ = rmcs_msgs::Mouse::zero(); keyboard_ = rmcs_msgs::Keyboard::zero(); } @@ -404,8 +348,6 @@ class Vt13 { rclcpp::Logger logger_ = rclcpp::get_logger("vt13"); rmcs_utility::RingBuffer data_buffer_{1024}; - std::atomic faulted_{false}; - Clock::time_point last_remote_control_frame_time_ = Clock::time_point::min(); std::atomic store_calls_{0}; std::atomic received_bytes_{0}; @@ -440,4 +382,4 @@ class Vt13 { rmcs_msgs::Keyboard keyboard_ = rmcs_msgs::Keyboard::zero(); }; -} // namespace rmcs_core::hardware::device +} // namespace rmcs_core::hardware::device \ No newline at end of file From 88f2e5193bca22557debfabe800882ce06e01510 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 13:58:47 +0800 Subject: [PATCH 35/90] feat: simplify chassis direction indicator angle calculation --- .../rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index c6961758..79a59e5b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -121,12 +121,8 @@ class DeformableInfantry void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; - auto to_referee_angle = [](double angle) { - return static_cast( - std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); - }; chassis_direction_indicator_.set_color(chassis_direction_indicator_color(chassis_mode)); - chassis_direction_indicator_.set_angle(to_referee_angle(0.0), 30); + chassis_direction_indicator_.set_angle(270, 30); } static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { From d454db35549599d696da789ae52127e2a8defdb2 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 19:22:06 +0800 Subject: [PATCH 36/90] feat: update deformable infantry configuration and UI angle settings --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp | 4 ++-- .../rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index e3fa2fef..aed9a7d7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -51,7 +51,7 @@ deformable_infantry: pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false - debug_log_deformable_joint_motor: true + debug_log_deformable_joint_motor: false chassis_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index eca43528..5d9c4698 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -206,8 +206,8 @@ class Vt13 { -channel_to_double(static_cast(data.joystick_channel0)), }; joystick_left_ = { - channel_to_double(static_cast(data.joystick_channel3)), - -channel_to_double(static_cast(data.joystick_channel2)), + channel_to_double(static_cast(data.joystick_channel2)), + -channel_to_double(static_cast(data.joystick_channel3)), }; mouse_velocity_ = { diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 79a59e5b..6213421b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -122,7 +122,7 @@ class DeformableInfantry auto chassis_mode = *chassis_mode_; chassis_direction_indicator_.set_color(chassis_direction_indicator_color(chassis_mode)); - chassis_direction_indicator_.set_angle(270, 30); + chassis_direction_indicator_.set_angle(0, 30); } static Shape::Color chassis_direction_indicator_color(rmcs_msgs::ChassisMode mode) { From b49a30801ef14106dc8e66b3279e7067b7ac5b36 Mon Sep 17 00:00:00 2001 From: eye-on Date: Fri, 22 May 2026 19:39:50 +0800 Subject: [PATCH 37/90] feat:add heat controller --- .../config/deformable-infantry-omni.yaml | 7 ++- .../controller/chassis/deformable_chassis.cpp | 4 +- .../shooting/friction_wheel_controller.cpp | 53 ++++++++++++++++++- rmcs_ws/src/serial | 1 + 4 files changed, 61 insertions(+), 4 deletions(-) create mode 160000 rmcs_ws/src/serial diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index aed9a7d7..4f9a3df6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -58,6 +58,7 @@ chassis_controller: # Deploy geometry / chassis-owned joint intent min_angle: 8.0 max_angle: 58.0 + launch_ramp_shortcut_enabled: false active_suspension_enable: true spin_ratio: 1.0 @@ -120,8 +121,10 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 575.0 + - 575.0 + friction_velocity_min: 550.0 + friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 8e82b832..cb6b93d4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -61,6 +61,7 @@ class DeformableChassis rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , launch_ramp_shortcut_enabled_(get_parameter_or("launch_ramp_shortcut_enabled", true)) , min_angle_(get_parameter_or("min_angle", 7.0)) , max_angle_(get_parameter_or("max_angle", 58.0)) @@ -331,7 +332,7 @@ class DeformableChassis mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } - } else if (!last_keyboard_.x && keyboard.x) { + } else if (launch_ramp_shortcut_enabled_ && !last_keyboard_.x && keyboard.x) { deactivate_complex_spin_(); deactivate_qe_complex_spin_(); mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP @@ -989,6 +990,7 @@ class DeformableChassis double qe_last_toggle_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; + const bool launch_ramp_shortcut_enabled_; InputInterface left_front_joint_physical_angle_; InputInterface left_back_joint_physical_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a83dc5f4..d3470eb3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,6 +31,7 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/mouse/mouse_wheel", mouse_wheel_); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); auto friction_working_velocities = get_parameter("friction_velocities").as_double_array(); @@ -53,6 +54,36 @@ class FrictionWheelController friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } + const bool has_adjustable_min = has_parameter("friction_velocity_min"); + const bool has_adjustable_max = has_parameter("friction_velocity_max"); + if (has_adjustable_min != has_adjustable_max) + throw std::runtime_error( + "Parameter mismatch: 'friction_velocity_min' and 'friction_velocity_max' must be " + "provided together."); + + friction_velocity_adjustment_enabled_ = has_adjustable_min; + if (friction_velocity_adjustment_enabled_) { + friction_velocity_min_ = get_parameter("friction_velocity_min").as_double(); + friction_velocity_max_ = get_parameter("friction_velocity_max").as_double(); + if (friction_velocity_min_ > friction_velocity_max_) + throw std::runtime_error( + "Invalid friction velocity range: 'friction_velocity_min' must be less than " + "or equal to 'friction_velocity_max'."); + + friction_adjustable_velocity_ = friction_working_velocities_[0]; + for (size_t i = 1; i < friction_count_; i++) { + if (std::abs(friction_working_velocities_[i] - friction_adjustable_velocity_) > 1e-6) + throw std::runtime_error( + "Adjustable friction velocity requires all 'friction_velocities' to be " + "identical."); + } + + if (friction_adjustable_velocity_ < friction_velocity_min_ + || friction_adjustable_velocity_ > friction_velocity_max_) + throw std::runtime_error( + "Initial friction velocity is outside the configured adjustable range."); + } + friction_soft_start_stop_step_ = (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); @@ -65,6 +96,7 @@ class FrictionWheelController const auto switch_right = *switch_right_; const auto switch_left = *switch_left_; const auto keyboard = *keyboard_; + const auto mouse_wheel = *mouse_wheel_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,6 +106,8 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { + update_adjustable_friction_velocity(keyboard, mouse_wheel); + if ((!last_keyboard_.v && keyboard.v) || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { friction_enabled_ = !friction_enabled_; @@ -107,6 +141,18 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } + void update_adjustable_friction_velocity(const rmcs_msgs::Keyboard& keyboard, double mouse_wheel) { + if (!friction_velocity_adjustment_enabled_ || !keyboard.x) + return; + + friction_adjustable_velocity_ += mouse_wheel * 5.0 * 0.001; + friction_adjustable_velocity_ = + std::clamp(friction_adjustable_velocity_, friction_velocity_min_, friction_velocity_max_); + + for (size_t i = 0; i < friction_count_; i++) + friction_working_velocities_[i] = friction_adjustable_velocity_; + } + void update_friction_velocities() { if (std::isnan(friction_soft_start_stop_percentage_)) { friction_soft_start_stop_percentage_ = 0.0; @@ -187,6 +233,7 @@ class FrictionWheelController InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; + InputInterface mouse_wheel_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -199,6 +246,10 @@ class FrictionWheelController std::unique_ptr[]> friction_velocities_; bool friction_enabled_ = false; + bool friction_velocity_adjustment_enabled_ = false; + double friction_adjustable_velocity_ = 0.0; + double friction_velocity_min_ = 0.0; + double friction_velocity_max_ = 0.0; double friction_soft_start_stop_step_; double friction_soft_start_stop_percentage_ = nan_; @@ -219,4 +270,4 @@ class FrictionWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/serial b/rmcs_ws/src/serial new file mode 160000 index 00000000..d50c4018 --- /dev/null +++ b/rmcs_ws/src/serial @@ -0,0 +1 @@ +Subproject commit d50c401809f17cc5862bdbe37cc6e15737ba7c49 From f913c3226d622991502d963632a3d07388b97c62 Mon Sep 17 00:00:00 2001 From: eye-on Date: Fri, 22 May 2026 21:15:47 +0800 Subject: [PATCH 38/90] feat:add info logger --- .codex | 0 rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/controller/shooting/friction_wheel_controller.cpp | 4 ++++ rmcs_ws/src/serial | 1 - 4 files changed, 5 insertions(+), 2 deletions(-) create mode 100644 .codex delete mode 160000 rmcs_ws/src/serial diff --git a/.codex b/.codex new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..50f0c914 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index d3470eb3..1d7e5a9b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -124,6 +124,10 @@ class FrictionWheelController last_switch_left_ = switch_left; last_keyboard_ = keyboard; } + + RCLCPP_INFO( + get_logger(), "current speed: %.2f, %.2f", + *friction_velocities_[0], *friction_velocities_[1]); } private: diff --git a/rmcs_ws/src/serial b/rmcs_ws/src/serial deleted file mode 160000 index d50c4018..00000000 --- a/rmcs_ws/src/serial +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d50c401809f17cc5862bdbe37cc6e15737ba7c49 From e494a6cb4d73613c504db2ad4fb2ff263a3a2981 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 22 May 2026 21:25:02 +0800 Subject: [PATCH 39/90] feat: enhance friction wheel controller and UI with working velocity outputs --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../shooting/friction_wheel_controller.cpp | 12 +++++++ .../controller/shooting/heat_controller.cpp | 2 +- .../referee/app/ui/deformable_infantry_ui.cpp | 32 +++++++++++++++++-- 4 files changed, 43 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 50f0c914..7fbe1bd4 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 +Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 1d7e5a9b..050443e5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -46,10 +46,15 @@ class FrictionWheelController friction_count_ = friction_wheels.size(); friction_working_velocities_ = std::make_unique(friction_count_); friction_velocities_ = std::make_unique[]>(friction_count_); + friction_working_velocity_outputs_ = + std::make_unique[]>(friction_count_); friction_control_velocities_ = std::make_unique[]>(friction_count_); for (size_t i = 0; i < friction_count_; i++) { friction_working_velocities_[i] = friction_working_velocities[i]; register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/working_velocity", friction_working_velocity_outputs_[i], + friction_working_velocities_[i]); register_output( friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } @@ -107,6 +112,7 @@ class FrictionWheelController if (switch_right != Switch::DOWN) { update_adjustable_friction_velocity(keyboard, mouse_wheel); + update_friction_working_velocity_outputs(); if ((!last_keyboard_.v && keyboard.v) || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { @@ -157,6 +163,11 @@ class FrictionWheelController friction_working_velocities_[i] = friction_adjustable_velocity_; } + void update_friction_working_velocity_outputs() { + for (size_t i = 0; i < friction_count_; i++) + *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; + } + void update_friction_velocities() { if (std::isnan(friction_soft_start_stop_percentage_)) { friction_soft_start_stop_percentage_ = 0.0; @@ -248,6 +259,7 @@ class FrictionWheelController std::unique_ptr friction_working_velocities_; std::unique_ptr[]> friction_velocities_; + std::unique_ptr[]> friction_working_velocity_outputs_; bool friction_enabled_ = false; bool friction_velocity_adjustment_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..ce6c85f4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -29,7 +29,7 @@ class HeatController shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + shooter_heat_ += heat_per_shot + *shooter_cooling_; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 6213421b..d3a472ff 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,15 @@ class DeformableInfantry , vertical_center_guidelines_( {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , friction_wheel_speed_indicator_( + Shape::Color::PINK, 20, 2, + static_cast(std::lround( + static_cast(x_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)), + static_cast(std::lround( + static_cast(y_center) + - friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)), + 0) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { @@ -73,6 +83,7 @@ class DeformableInfantry register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + register_input("/gimbal/left_friction/working_velocity", left_friction_working_velocity_); register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); register_input("/gimbal/left_friction/velocity", left_friction_velocity_); register_input("/gimbal/right_friction/velocity", right_friction_velocity_); @@ -93,9 +104,21 @@ class DeformableInfantry update_ctrl_ui(); status_ring_.update_bullet_allowance(*robot_bullet_allowance_); - status_ring_.update_friction_wheel_speed( - std::min(*left_friction_velocity_, *right_friction_velocity_), - *left_friction_control_velocity_ > 0); + const double friction_wheel_speed = + std::min(*left_friction_velocity_, *right_friction_velocity_); + const bool friction_wheel_enabled = *left_friction_control_velocity_ > 0; + const auto friction_wheel_speed_value = + static_cast(std::lround(*left_friction_working_velocity_)); + status_ring_.update_friction_wheel_speed(friction_wheel_speed, friction_wheel_enabled); + if (friction_wheel_speed_indicator_.value() != friction_wheel_speed_value) { + friction_wheel_speed_indicator_.set_value(friction_wheel_speed_value); + friction_wheel_speed_indicator_.set_center_x( + static_cast(std::lround( + static_cast(x_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2))); + } + friction_wheel_speed_indicator_.set_color( + friction_wheel_enabled ? Shape::Color::GREEN : Shape::Color::PINK); status_ring_.update_supercap(*supercap_voltage_, *supercap_enabled_); status_ring_.update_battery_power(*chassis_voltage_); @@ -153,6 +176,7 @@ class DeformableInfantry static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; InputInterface timestamp_; InputInterface chassis_mode_; @@ -170,6 +194,7 @@ class DeformableInfantry InputInterface robot_bullet_allowance_; + InputInterface left_friction_working_velocity_; InputInterface left_friction_control_velocity_; InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; @@ -184,6 +209,7 @@ class DeformableInfantry Line horizontal_center_guidelines_[2]; Line vertical_center_guidelines_[2]; + Integer friction_wheel_speed_indicator_; Arc chassis_direction_indicator_; DeformableChassisLegArcs deformable_chassis_leg_arcs_; From 49890c8cc476af52ea939cc44442fa6cb3ce3a63 Mon Sep 17 00:00:00 2001 From: eye-on Date: Fri, 22 May 2026 21:54:57 +0800 Subject: [PATCH 40/90] feat;heat parameter fixed --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/controller/shooting/friction_wheel_controller.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..50f0c914 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 050443e5..a8125c0d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -131,9 +131,6 @@ class FrictionWheelController last_keyboard_ = keyboard; } - RCLCPP_INFO( - get_logger(), "current speed: %.2f, %.2f", - *friction_velocities_[0], *friction_velocities_[1]); } private: @@ -155,7 +152,7 @@ class FrictionWheelController if (!friction_velocity_adjustment_enabled_ || !keyboard.x) return; - friction_adjustable_velocity_ += mouse_wheel * 5.0 * 0.001; + friction_adjustable_velocity_ += mouse_wheel * 5.0 * 10; friction_adjustable_velocity_ = std::clamp(friction_adjustable_velocity_, friction_velocity_min_, friction_velocity_max_); From 09fd6f2ec134a17c5210e90773e2af90330c4779 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 23 May 2026 00:27:49 +0800 Subject: [PATCH 41/90] feat: update heat controller parameters and adjust friction wheel speed indicator logic --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../config/deformable-infantry-omni-b.yaml | 2 +- .../config/deformable-infantry-omni.yaml | 2 +- .../config/deformable-infantry-steering.yaml | 2 +- .../chassis/chassis_power_controller.cpp | 2 +- .../shooting/friction_wheel_controller.cpp | 2 +- .../controller/shooting/heat_controller.cpp | 2 +- .../referee/app/ui/deformable_infantry_ui.cpp | 31 ++++++++++++------- 8 files changed, 26 insertions(+), 19 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 50f0c914..7fbe1bd4 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 +Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 817e0a62..77aff1ce 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -128,7 +128,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 2000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 4f9a3df6..aea37ad7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -130,7 +130,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 2000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 794c623b..86fc0539 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -122,7 +122,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 0 + reserved_heat: 2000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 543750e6..0b80b0f9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -126,7 +126,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - constexpr double excess_power_limit = 15; + constexpr double excess_power_limit = 5.0; power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a8125c0d..3018b3f8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -152,7 +152,7 @@ class FrictionWheelController if (!friction_velocity_adjustment_enabled_ || !keyboard.x) return; - friction_adjustable_velocity_ += mouse_wheel * 5.0 * 10; + friction_adjustable_velocity_ -= mouse_wheel * 5.0 * 10; friction_adjustable_velocity_ = std::clamp(friction_adjustable_velocity_, friction_velocity_min_, friction_velocity_max_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index ce6c85f4..3de0960a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -29,7 +29,7 @@ class HeatController shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); if (*bullet_fired_) - shooter_heat_ += heat_per_shot + *shooter_cooling_; + shooter_heat_ += heat_per_shot + 10; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index d3a472ff..686ca831 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -36,14 +36,8 @@ class DeformableInfantry {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) , friction_wheel_speed_indicator_( - Shape::Color::PINK, 20, 2, - static_cast(std::lround( - static_cast(x_center) - + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)), - static_cast(std::lround( - static_cast(y_center) - - friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)), - 0) + Shape::Color::PINK, friction_wheel_speed_indicator_font_size_, 2, 0, + friction_wheel_speed_indicator_y(), 0) , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { @@ -93,6 +87,8 @@ class DeformableInfantry register_input("/referee/game/stage", game_stage_); + friction_wheel_speed_indicator_.set_center_x(friction_wheel_speed_indicator_center_x()); + crosshair_base_x_ = crosshair_circle_.x(); crosshair_base_y_ = crosshair_circle_.y(); ctrl_transition_.reset(false); @@ -112,10 +108,7 @@ class DeformableInfantry status_ring_.update_friction_wheel_speed(friction_wheel_speed, friction_wheel_enabled); if (friction_wheel_speed_indicator_.value() != friction_wheel_speed_value) { friction_wheel_speed_indicator_.set_value(friction_wheel_speed_value); - friction_wheel_speed_indicator_.set_center_x( - static_cast(std::lround( - static_cast(x_center) - + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2))); + friction_wheel_speed_indicator_.set_center_x(friction_wheel_speed_indicator_center_x()); } friction_wheel_speed_indicator_.set_color( friction_wheel_enabled ? Shape::Color::GREEN : Shape::Color::PINK); @@ -177,6 +170,20 @@ class DeformableInfantry static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; + static constexpr uint16_t friction_wheel_speed_indicator_font_size_ = 20; + + static uint16_t friction_wheel_speed_indicator_center_x() { + return static_cast(std::lround( + static_cast(x_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2)); + } + + static uint16_t friction_wheel_speed_indicator_y() { + return static_cast(std::lround( + static_cast(y_center) + + friction_wheel_speed_indicator_radius_ / std::numbers::sqrt2 + - static_cast(friction_wheel_speed_indicator_font_size_) / 2.0)); + } InputInterface timestamp_; InputInterface chassis_mode_; From cf29bdc8af9cea87c020946a7f275f303d9588dd Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 23 May 2026 09:22:59 +0800 Subject: [PATCH 42/90] feat: adjust status ring parameters for improved accuracy --- .../rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 2 +- .../src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 686ca831..ccc4ea48 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -28,7 +28,7 @@ class DeformableInfantry DeformableInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_circle_(Shape::Color::WHITE, x_center - 22, y_center - 42, 8, 2) - , status_ring_(26.5, 26.5, 600, 300) + , status_ring_(23.5, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index c00e5f59..1f3ed26e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -246,10 +246,10 @@ class StatusRing { } void update_supercap(double value, bool enable) { - auto angle = 275 + calculate_angle(value, 10.5, supercap_limit_) + 1; + auto angle = 275 + calculate_angle(value, 8.5, supercap_limit_) + 1; supercap_status_.set_angle_end(static_cast(angle)); - if (value > 22.6) { + if (value > 20.0) { supercap_status_.set_color(enable ? Shape::Color::CYAN : Shape::Color::GREEN); } else if (value > 13.5) { supercap_status_.set_color(enable ? Shape::Color::YELLOW : Shape::Color::ORANGE); From c09ea57098c4c885a9d09632ec46250578992f73 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 23 May 2026 10:30:30 +0800 Subject: [PATCH 43/90] feat: increase excess power limit for virtual buffer energy --- .../src/controller/chassis/chassis_power_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 0b80b0f9..06229d18 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -126,7 +126,7 @@ class ChassisPowerController 0.0, 1.0); // Maximum excess power when virtual buffer energy is full. - constexpr double excess_power_limit = 5.0; + constexpr double excess_power_limit = 15.0; power_limit += excess_power_limit; power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; From 78b2cc44d9ca14cacf0c795197ef39ec9b430d1d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 23 May 2026 10:56:11 +0800 Subject: [PATCH 44/90] feat: increase reserved heat parameter for heat controller and adjust status ring parameters --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- .../src/rmcs_bringup/config/deformable-infantry-steering.yaml | 2 +- .../src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 2 +- rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 77aff1ce..87e47ffa 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -128,7 +128,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 2000 + reserved_heat: 5000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index aea37ad7..47248ccd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -130,7 +130,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 2000 + reserved_heat: 5000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 86fc0539..5c07258b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -122,7 +122,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 2000 + reserved_heat: 5000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index ccc4ea48..6f09cb3c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -28,7 +28,7 @@ class DeformableInfantry DeformableInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_circle_(Shape::Color::WHITE, x_center - 22, y_center - 42, 8, 2) - , status_ring_(23.5, 26.5, 600, 300) + , status_ring_(24.0, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 1f3ed26e..48e5499a 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -246,7 +246,7 @@ class StatusRing { } void update_supercap(double value, bool enable) { - auto angle = 275 + calculate_angle(value, 8.5, supercap_limit_) + 1; + auto angle = 275 + calculate_angle(value, 8.0, supercap_limit_) + 1; supercap_status_.set_angle_end(static_cast(angle)); if (value > 20.0) { From 2fb483bc5a225dfb35964f1d8a66b16003cc37c2 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 24 May 2026 00:01:23 +0800 Subject: [PATCH 45/90] feat: adjust gimbal upper limit and friction wheel velocities for improved performance --- .../src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 6 +++--- .../src/rmcs_bringup/config/deformable-infantry-omni.yaml | 6 +++--- .../rmcs_bringup/config/deformable-infantry-steering.yaml | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 87e47ffa..a71c2a98 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -86,7 +86,7 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.80 # -35 deg + upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg yaw_angle_kp: 30.0 @@ -121,8 +121,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 580.0 + - 580.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 47248ccd..883f8ed1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -86,7 +86,7 @@ chassis_controller: gimbal_controller: ros__parameters: - upper_limit: -0.80 # -35 deg + upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg yaw_angle_kp: 30.0 @@ -121,8 +121,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 575.0 - - 575.0 + - 580.0 + - 580.0 friction_velocity_min: 550.0 friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 5c07258b..de50ece8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -87,7 +87,7 @@ gimbal_controller: inertia: 1.0 # kg·m² friction: 1.65 # Nm/(rad/s) - upper_limit: -0.61 # -35 deg + upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg use_encoder_pitch: true @@ -115,8 +115,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 580.0 + - 580.0 friction_soft_start_stop_time: 1.0 heat_controller: From cd036d4ff0e636703c45585ff3509a8d17367e15 Mon Sep 17 00:00:00 2001 From: eye-on Date: Sun, 24 May 2026 00:25:10 +0800 Subject: [PATCH 46/90] feat:fixed supercup --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../referee/app/ui/deformable_infantry_ui.cpp | 7 +++++- .../src/referee/app/ui/widget/status_ring.hpp | 25 ++++++++++++++++++- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..50f0c914 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 6f09cb3c..5babefe5 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -21,6 +21,10 @@ namespace rmcs_core::referee::app::ui { using namespace std::chrono_literals; +namespace { +constexpr double supercap_cutoff_voltage = 8.0; +} + class DeformableInfantry : public rmcs_executor::Component , public rclcpp::Node { @@ -112,7 +116,8 @@ class DeformableInfantry } friction_wheel_speed_indicator_.set_color( friction_wheel_enabled ? Shape::Color::GREEN : Shape::Color::PINK); - status_ring_.update_supercap(*supercap_voltage_, *supercap_enabled_); + status_ring_.update_supercap_energy( + *supercap_voltage_, *supercap_enabled_, supercap_cutoff_voltage); status_ring_.update_battery_power(*chassis_voltage_); status_ring_.update_auto_aim_enable(mouse_->right == 1); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 48e5499a..35882689 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -258,6 +258,19 @@ class StatusRing { } } + void update_supercap_energy(double value, bool enable, double cutoff_voltage) { + auto angle = 275 + calculate_energy_angle(value, cutoff_voltage, supercap_limit_) + 1; + supercap_status_.set_angle_end(static_cast(angle)); + + if (value > 20.0) { + supercap_status_.set_color(enable ? Shape::Color::CYAN : Shape::Color::GREEN); + } else if (value > 13.5) { + supercap_status_.set_color(enable ? Shape::Color::YELLOW : Shape::Color::ORANGE); + } else { + supercap_status_.set_color(enable ? Shape::Color::PURPLE : Shape::Color::PINK); + } + } + void update_battery_power(double value) { auto angle = 265 - calculate_angle(value, 20, 25.7) - 1; battery_status_.set_angle_start(static_cast(angle)); @@ -305,6 +318,16 @@ class StatusRing { return visible_angle * std::clamp(value - min, 0.0, max - min) / (max - min); } + static constexpr double calculate_energy_angle( + double value, double cutoff_voltage, double full_voltage) { + const double clamped_value = std::clamp(value, cutoff_voltage, full_voltage); + const double numerator = + clamped_value * clamped_value - cutoff_voltage * cutoff_voltage; + const double denominator = + full_voltage * full_voltage - cutoff_voltage * cutoff_voltage; + return visible_angle * numerator / denominator; + } + void set_limits( double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { supercap_limit_ = supercap_limit; @@ -378,4 +401,4 @@ class StatusRing { Arc bullet_scales_[4]; }; -} // namespace rmcs_core::referee::app::ui \ No newline at end of file +} // namespace rmcs_core::referee::app::ui From 3f67501f4daa2c97c6567111ad56aa711c0f3f34 Mon Sep 17 00:00:00 2001 From: eye-on Date: Sun, 24 May 2026 01:29:26 +0800 Subject: [PATCH 47/90] feat:friction applied in omni-b --- .../src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index a71c2a98..b9e8f985 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -59,6 +59,7 @@ chassis_controller: # Deploy geometry / chassis-owned joint intent min_angle: 8.0 max_angle: 58.0 + launch_ramp_shortcut_enabled: false active_suspension_enable: true spin_ratio: 1.0 @@ -123,6 +124,8 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 + friction_velocity_min: 550.0 + friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: From 1e08647f5c4119204f53d54b6fbd5fd0619f71cc Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 24 May 2026 03:33:37 +0800 Subject: [PATCH 48/90] feat:update virtual_buffer_energy_limit_ value --- .../src/controller/chassis/chassis_power_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 06229d18..1eee280c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -150,7 +150,7 @@ class ChassisPowerController InputInterface rotary_knob_; InputInterface chassis_power_; - static constexpr double virtual_buffer_energy_limit_ = 30.0; + static constexpr double virtual_buffer_energy_limit_ = 15.0; double virtual_buffer_energy_; InputInterface supercap_voltage_; From 52b8e2db3e538d0ebcb164258f8b431056feff0c Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 24 May 2026 07:55:53 +0800 Subject: [PATCH 49/90] calibrate yaw --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 883f8ed1..9317566a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -47,7 +47,7 @@ deformable_infantry: left_back_zero_point: 5801 right_back_zero_point: 7817 right_front_zero_point: 7136 - yaw_motor_zero_point: 31517 + yaw_motor_zero_point: 26873 pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false From bb7f172fa2c8814f2d431156b3f2b75904c1d968 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 24 May 2026 07:58:40 +0800 Subject: [PATCH 50/90] calibrate yaw --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 9317566a..35b5eedb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -47,7 +47,7 @@ deformable_infantry: left_back_zero_point: 5801 right_back_zero_point: 7817 right_front_zero_point: 7136 - yaw_motor_zero_point: 26873 + yaw_motor_zero_point: 43365 pitch_motor_zero_point: 6853 debug_log_supercap: false debug_log_wheel_motor: false From 83d616330b120f99fa52fc82d1573a2d6864d96a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 24 May 2026 11:05:06 +0800 Subject: [PATCH 51/90] feat: add armor priority --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../bullet_feeder_controller_17mm.cpp | 30 +++++++------------ 2 files changed, 12 insertions(+), 20 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 50f0c914..feff0ed0 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 50f0c914430bb874c4207c253942991dc23d3ba5 +Subproject commit feff0ed06de3cec12df3df600e3df3e2f1066d60 diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 49f41690..1c8f4d50 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -1,7 +1,3 @@ -#include - -#include - #include #include #include @@ -53,7 +49,7 @@ class BulletFeederController17mm register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/auto_aim/should_shoot", fire_control_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -63,8 +59,8 @@ class BulletFeederController17mm } void before_updating() override { - if (!fire_control_.ready()) - fire_control_.bind_directly(false); + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); } void update() override { @@ -80,19 +76,17 @@ class BulletFeederController17mm || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - int64_t bullet_allowance = 0; + std::int64_t bullet_allowance = 0; if (switch_right != Switch::DOWN) { shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; - const bool auto_aim_requested = mouse.right || switch_right == Switch::UP; single_shot_stop_counter_ = std::max(0, single_shot_stop_counter_ - 1); temporary_single_shot_counter_ = std::max(0, temporary_single_shot_counter_ - 1); if (!last_mouse_.left && mouse.left) single_shot_stop_counter_ = single_shot_max_stop_delay_; - else if (!auto_aim_requested && last_switch_left_ != Switch::DOWN - && switch_left == Switch::DOWN) { + else if (last_switch_left_ != Switch::DOWN && switch_left == Switch::DOWN) { single_shot_stop_counter_ = single_shot_max_stop_delay_; temporary_single_shot_counter_ = 500; } @@ -104,16 +98,14 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - const bool auto_aim_enabled = auto_aim_requested; - bool triggered = - (mouse.left && !mouse.right) || - (switch_left == Switch::DOWN && auto_aim_enabled && *fire_control_) || - (switch_left == Switch::DOWN && !auto_aim_enabled) || - (mouse.left && auto_aim_enabled && *fire_control_); + auto aiming_enable = mouse_->right || (switch_right == Switch::UP); + auto attack_intent = mouse_->left || (switch_left == Switch::DOWN); + auto triggered = + aiming_enable ? (*should_shoot_ && attack_intent) : attack_intent; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { - bool triggered = single_shot_stop_counter_ > 0; + auto triggered = single_shot_stop_counter_ > 0; bullet_allowance = triggered && (*control_bullet_allowance_limited_by_heat_ > 0); } @@ -215,7 +207,7 @@ class BulletFeederController17mm InputInterface mouse_; InputInterface keyboard_; - InputInterface fire_control_; + InputInterface should_shoot_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; From 2fdad581d6b6d7690b11cbfd3485d3297bdabaf8 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 24 May 2026 18:58:13 +0800 Subject: [PATCH 52/90] feat: update reserved heat value for heat controller --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- .../src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- .../rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 6 +++--- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index feff0ed0..3da57e44 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit feff0ed06de3cec12df3df600e3df3e2f1066d60 +Subproject commit 3da57e441bd8b620a7a7db7bcb2ed99dab22e2e3 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index b9e8f985..8658473f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -131,7 +131,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 5000 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 35b5eedb..2c1103c4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -130,7 +130,7 @@ friction_wheel_controller: heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 5000 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 5babefe5..69a870a4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -31,7 +31,7 @@ class DeformableInfantry public: DeformableInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , crosshair_circle_(Shape::Color::WHITE, x_center - 22, y_center - 42, 8, 2) + , crosshair_circle_(Shape::Color::WHITE, x_center - 26, y_center - 34, 8, 2) , status_ring_(24.0, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, @@ -129,9 +129,9 @@ class DeformableInfantry const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); crosshair_circle_.set_x(static_cast(std::lround( - static_cast(crosshair_base_x_) + 20.0 * reveal))); + static_cast(crosshair_base_x_) + 45.0 * reveal))); crosshair_circle_.set_y(static_cast(std::lround( - static_cast(crosshair_base_y_) - 20.0 * reveal))); + static_cast(crosshair_base_y_) + 20.0 * reveal))); } void update_time_reminder() { From 70c0688646778ba2e3fb768717580384d8d6b7f0 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 31 May 2026 14:45:50 +0800 Subject: [PATCH 53/90] fix: move supercap_cutoff_voltage constant to class scope --- .codex | 0 .../rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp | 5 +---- 2 files changed, 1 insertion(+), 4 deletions(-) delete mode 100644 .codex diff --git a/.codex b/.codex deleted file mode 100644 index e69de29b..00000000 diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 69a870a4..aea13c51 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -21,10 +21,6 @@ namespace rmcs_core::referee::app::ui { using namespace std::chrono_literals; -namespace { -constexpr double supercap_cutoff_voltage = 8.0; -} - class DeformableInfantry : public rmcs_executor::Component , public rclcpp::Node { @@ -176,6 +172,7 @@ class DeformableInfantry static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; static constexpr uint16_t friction_wheel_speed_indicator_font_size_ = 20; + static constexpr double supercap_cutoff_voltage = 8.0; static uint16_t friction_wheel_speed_indicator_center_x() { return static_cast(std::lround( From 02bc0003af3018f6e99bcf8a758169244efc3c22 Mon Sep 17 00:00:00 2001 From: eye-on Date: Tue, 2 Jun 2026 18:31:49 +0800 Subject: [PATCH 54/90] feat(gimbal): add ctrl pitch recenter --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../deformable_infantry_gimbal_controller.cpp | 6 ++++ .../gimbal/two_axis_gimbal_solver.hpp | 31 +++++++++++++++++++ 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 3da57e44..7fbe1bd4 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 3da57e441bd8b620a7a7db7bcb2ed99dab22e2e3 +Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 6d3fbb72..d479354a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -92,6 +93,7 @@ class DeformableInfantryGimbalController struct Input { explicit Input(rmcs_executor::Component& component) { component.register_input("/remote/joystick/left", joystick_left); + component.register_input("/remote/keyboard", keyboard); component.register_input("/remote/switch/right", switch_right); component.register_input("/remote/switch/left", switch_left); component.register_input("/remote/mouse/velocity", mouse_velocity); @@ -109,6 +111,7 @@ class DeformableInfantryGimbalController } InputInterface joystick_left; + InputInterface keyboard; InputInterface switch_right; InputInterface switch_left; InputInterface mouse_velocity; @@ -157,6 +160,9 @@ class DeformableInfantryGimbalController const auto yaw_shift = joystick_sensitivity_ * input_.joystick_left->y() + mouse_sensitivity_ * input_.mouse_velocity->y(); + if (input_.keyboard->ctrl) + return gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevelYawShift{yaw_shift}); + const auto pitch_shift = -joystick_sensitivity_ * input_.joystick_left->x() + mouse_sensitivity_ * input_.mouse_velocity->x(); return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index c1fc772f..e07392b2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -54,6 +54,37 @@ class TwoAxisGimbalSolver { } }; + class SetToLevelYawShift : public Operation { + public: + explicit SetToLevelYawShift(double yaw_shift) + : yaw_shift_(yaw_shift) {} + + private: + PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { + OdomImu::DirectionVector odom_dir; + if (!super.control_enabled_) { + odom_dir = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *super.tf_); + } else { + odom_dir = super.control_direction_; + } + + if (std::abs(odom_dir->x()) < 1e-6 && std::abs(odom_dir->y()) < 1e-6) + return {}; + + super.control_enabled_ = true; + odom_dir->z() = 0; + odom_dir->normalize(); + auto dir = fast_tf::cast(odom_dir, *super.tf_); + dir->normalize(); + + const auto yaw_transform = Eigen::AngleAxisd{yaw_shift_, Eigen::Vector3d::UnitZ()}; + return PitchLink::DirectionVector{yaw_transform * (*dir)}; + } + + double yaw_shift_; + }; + class SetControlDirection : public Operation { public: explicit SetControlDirection(OdomImu::DirectionVector target) From b8a0efc2575545908b97aec1a64ad2a3862859f6 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 3 Jun 2026 22:28:16 +0800 Subject: [PATCH 55/90] feat: update wheel radius for deformable chassis controller --- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml | 2 +- rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml | 2 +- .../src/rmcs_bringup/config/deformable-infantry-steering.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 8658473f..b93a40e2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -177,7 +177,7 @@ deformable_chassis_controller: moment_of_inertia: 1.0 chassis_radius: 0.2341741 rod_length: 0.150 - wheel_radius: 0.055 + wheel_radius: 0.07 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 2c1103c4..07b3f791 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -176,7 +176,7 @@ deformable_chassis_controller: moment_of_inertia: 1.0 chassis_radius: 0.2341741 rod_length: 0.150 - wheel_radius: 0.055 + wheel_radius: 0.07 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index de50ece8..3b20a9b0 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -168,7 +168,7 @@ deformable_chassis_controller: moment_of_inertia: 1.0 chassis_radius: 0.2341741 rod_length: 0.150 - wheel_radius: 0.055 + wheel_radius: 0.07 friction_coefficient: 0.6 k1: 2.958580e+00 k2: 3.082190e-03 From cd521cbc9809370408ba1513006169d6e0528264 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 6 Jun 2026 21:06:17 +0800 Subject: [PATCH 56/90] feat(gimbal): add feedforward gains for yaw and pitch control --- .../deformable_infantry_gimbal_controller.cpp | 38 +++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index d479354a..a194ffb2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -32,6 +32,9 @@ class DeformableInfantryGimbalController get_parameter("pitch_torque_control", pitch_torque_control_enabled_); get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); + get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); + get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); + get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); } auto update() -> void override { @@ -62,15 +65,32 @@ class DeformableInfantryGimbalController return; } - const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); + const auto feedforward_enabled = auto_aim_active && input_.auto_aim_feedforward_valid.ready() + && *input_.auto_aim_feedforward_valid; + const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() + && std::isfinite(*input_.auto_aim_yaw_rate) + ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate + : 0.0; + const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() + && std::isfinite(*input_.auto_aim_yaw_acc) + ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc + : 0.0; + const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() + && std::isfinite(*input_.auto_aim_pitch_acc) + ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc + : 0.0; + + const auto yaw_velocity_ref = + yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu); + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_acc_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -108,6 +128,11 @@ class DeformableInfantryGimbalController component.register_input("/auto_aim/should_control", auto_aim_should_control, false); component.register_input( "/auto_aim/control_direction", auto_aim_control_direction, false); + component.register_input( + "/auto_aim/feedforward_valid", auto_aim_feedforward_valid, false); + component.register_input("/auto_aim/yaw_rate", auto_aim_yaw_rate, false); + component.register_input("/auto_aim/yaw_acc", auto_aim_yaw_acc, false); + component.register_input("/auto_aim/pitch_acc", auto_aim_pitch_acc, false); } InputInterface joystick_left; @@ -125,6 +150,10 @@ class DeformableInfantryGimbalController InputInterface auto_aim_should_control; InputInterface auto_aim_control_direction; + InputInterface auto_aim_feedforward_valid; + InputInterface auto_aim_yaw_rate; + InputInterface auto_aim_yaw_acc; + InputInterface auto_aim_pitch_acc; } input_{*this}; struct Output { @@ -213,6 +242,9 @@ class DeformableInfantryGimbalController double joystick_sensitivity_ = 0.003; double mouse_sensitivity_ = 0.5; bool pitch_torque_control_enabled_ = false; + double yaw_vel_ff_gain_ = 0.0; + double yaw_acc_ff_gain_ = 0.0; + double pitch_acc_ff_gain_ = 0.0; }; } // namespace rmcs_core::controller::gimbal From 7990df04f5ab72d37e1378785444abf80135b7e3 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 9 Jun 2026 19:58:19 +0800 Subject: [PATCH 57/90] feat: add IMU support and refactor gimbal controller parameters --- .devcontainer/state/codex/.tmp/plugins | 1 + rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../config/deformable-infantry-steering.yaml | 2 +- .../deformable_infantry_gimbal_controller.cpp | 3 +- .../gimbal/two_axis_gimbal_solver.hpp | 33 +----- .../hardware/deformable-infantry-steering.cpp | 112 +++++++++++++++--- 6 files changed, 102 insertions(+), 51 deletions(-) create mode 160000 .devcontainer/state/codex/.tmp/plugins diff --git a/.devcontainer/state/codex/.tmp/plugins b/.devcontainer/state/codex/.tmp/plugins new file mode 160000 index 00000000..3c463363 --- /dev/null +++ b/.devcontainer/state/codex/.tmp/plugins @@ -0,0 +1 @@ +Subproject commit 3c46336323290cda7c90d78e7a0134f47cc73713 diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..feff0ed0 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit feff0ed06de3cec12df3df600e3df3e2f1066d60 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 3b20a9b0..ee4364fa 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -42,6 +42,7 @@ deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 @@ -89,7 +90,6 @@ gimbal_controller: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg - use_encoder_pitch: true yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index a194ffb2..1bc50158 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -215,8 +215,7 @@ class DeformableInfantryGimbalController } TwoAxisGimbalSolver gimbal_solver_{ - *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), - true}; + *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double()}; pid::PidCalculator yaw_angle_pid_{ get_parameter("yaw_angle_kp").as_double(), diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index e07392b2..89ac5a09 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -21,14 +21,10 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver( - rmcs_executor::Component& component, double upper_limit, double lower_limit, - bool use_encoder_pitch = false) + TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) - , use_encoder_pitch_(use_encoder_pitch) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { - component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); component.register_input("/tf", tf_); } @@ -136,9 +132,7 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; - auto [control_direction_yaw_link, pitch] = - use_encoder_pitch_ ? pitch_link_to_yaw_link_from_encoder(control_direction) - : pitch_link_to_yaw_link(control_direction); + auto [control_direction_yaw_link, pitch] = pitch_link_to_yaw_link(control_direction); clamp_control_direction(control_direction_yaw_link); if (!control_enabled_) @@ -175,25 +169,6 @@ class TwoAxisGimbalSolver { return result; } - auto pitch_link_to_yaw_link_from_encoder(const PitchLink::DirectionVector& dir) const - -> std::pair { - - std::pair result; - auto& [dir_yaw_link, pitch_cs] = result; - - const double encoder_pitch = *gimbal_pitch_angle_; - pitch_cs = {std::cos(encoder_pitch), -std::sin(encoder_pitch)}; - - const auto& [x, y, z] = *dir; - dir_yaw_link = { - x * pitch_cs.x() - z * pitch_cs.y(), - y, - x * pitch_cs.y() + z * pitch_cs.x(), - }; - - return result; - } - static PitchLink::DirectionVector yaw_link_to_pitch_link(const YawLink::DirectionVector& dir, const Eigen::Vector2d& pitch) { @@ -235,9 +210,7 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; - bool use_encoder_pitch_ = false; - rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index 301ed10f..c811d97c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -29,7 +29,9 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "hardware/device/remote_control.hpp" #include "hardware/device/supercap.hpp" +#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -64,9 +66,15 @@ class DeformableInfantryV2 rmcs_board_lite = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); + std::string serial_filter_imu; + get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); top_board_ = std::make_unique( *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string()); + get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); + if (!serial_filter_imu.empty()) + imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); + remote_control_ = + std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); } ~DeformableInfantryV2() override = default; @@ -79,6 +87,10 @@ class DeformableInfantryV2 void update() override { rmcs_board_lite->update(); top_board_->update(); + if (imu_board_) + imu_board_->update(); + vt13_.update_status(); + remote_control_->update(); } void command_update() { @@ -90,6 +102,7 @@ class DeformableInfantryV2 private: class DeformableInfantryV2Command; class BottomBoard; + class ImuBoard; class TopBoard; class DeformableInfantryV2Command : public rmcs_executor::Component { @@ -674,11 +687,13 @@ class DeformableInfantryV2 explicit TopBoard( DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) + DeformableInfantryV2Command& deformableInfantry_command, + std::string serial_filter = {}, bool has_external_imu_board = false) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , hard_sync_pending_(deformableInfantry.hard_sync_pending_) + , has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) , bmi088_(1000, 0.2, 0.0) , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") @@ -707,8 +722,9 @@ class DeformableInfantryV2 deformableInfantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + if (!has_external_imu_board_) + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); bmi088_.set_coordinate_mapping([](double x, double y, double z) { // Top board BMI088 maps to gimbal frame as (-x, -y, z). @@ -731,21 +747,24 @@ class DeformableInfantryV2 scope_motor_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); + if (!has_external_imu_board_) { + Eigen::Quaterniond const odom_imu_to_yaw_link{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); + Eigen::Quaterniond pitch_link_to_odom_imu = + Eigen::Quaterniond{ + Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} + * yaw_link_to_odom_imu; + pitch_link_to_odom_imu.normalize(); + + *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); + // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use + // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. + tf_->set_transform( + pitch_link_to_odom_imu); + } tf_->set_state( pitch_encoder_angle); @@ -802,6 +821,7 @@ class DeformableInfantryV2 } std::atomic& hard_sync_pending_; + bool has_external_imu_board_ = false; OutputInterface& tf_; OutputInterface gimbal_yaw_velocity_bmi088_; @@ -814,6 +834,61 @@ class DeformableInfantryV2 device::DjiMotor scope_motor_; }; + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + friend class DeformableInfantryV2; + + public: + explicit ImuBoard( + DeformableInfantryV2& deformableInfantry, device::Vt13& vt13, + const std::string& serial_filter = {}) + : RmcsBoardLite{ + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} + , tf_{deformableInfantry.tf_} + , vt13_{vt13} + , bmi088_{1000, 0.2, 0.0} { + + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + } + + ~ImuBoard() override = default; + + void update() { + bmi088_.update_status(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; + device::Vt13& vt13_; + + device::Bmi088 bmi088_; + }; + auto status_service_callback(const std::shared_ptr& response) -> void { response->success = true; @@ -846,13 +921,16 @@ class DeformableInfantryV2 OutputInterface tf_; InputInterface timestamp_; + device::Vt13 vt13_; std::atomic hard_sync_pending_{false}; size_t hard_sync_snapshot_count_ = 0; Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; std::unique_ptr rmcs_board_lite; + std::unique_ptr imu_board_; std::unique_ptr top_board_; + std::unique_ptr remote_control_; std::shared_ptr> status_service_; uint32_t cmd_tick_ = 0; From 1a90a1de5bdb29f127f89a4784d038d4af30cee1 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 9 Jun 2026 22:07:41 +0800 Subject: [PATCH 58/90] feat(gimbal): add control hold parameters and manual yaw velocity override --- .../config/deformable-infantry-omni-b.yaml | 2 + .../config/deformable-infantry-omni.yaml | 2 + .../config/deformable-infantry-steering.yaml | 2 + .../controller/chassis/deformable_chassis.cpp | 24 +++++- .../deformable_infantry_gimbal_controller.cpp | 86 ++++++++++++++++++- .../hardware/deformable-infantry-omni-b.cpp | 2 +- .../src/hardware/deformable-infantry-omni.cpp | 2 +- .../hardware/deformable-infantry-steering.cpp | 2 +- 8 files changed, 113 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index b93a40e2..8976c1ea 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -89,6 +89,8 @@ gimbal_controller: ros__parameters: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg + ctrl_hold_pitch_target_angle: 0.0 + ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 07b3f791..fde52334 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -88,6 +88,8 @@ gimbal_controller: ros__parameters: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg + ctrl_hold_pitch_target_angle: 0.0 + ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index ee4364fa..9b8022bc 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -90,6 +90,8 @@ gimbal_controller: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg + ctrl_hold_pitch_target_angle: 0.0 + ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index cb6b93d4..9d17ebdf 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -134,6 +134,7 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/manual_yaw_velocity_override", manual_yaw_velocity_override_, false); register_input( "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); @@ -155,6 +156,7 @@ class DeformableChassis register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); + register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); @@ -199,6 +201,7 @@ class DeformableChassis right_front_joint_target_physical_acceleration_, nan_); *mode_ = rmcs_msgs::ChassisMode::AUTO; + *ctrl_hold_active_ = false; chassis_control_velocity_->vector << nan_, nan_, nan_; current_target_angle_ = max_angle_; @@ -218,6 +221,8 @@ class DeformableChassis RCLCPP_WARN( get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } + if (!manual_yaw_velocity_override_.ready()) + manual_yaw_velocity_override_.make_and_bind_directly(nan_); if (!chassis_imu_pitch_.ready()) chassis_imu_pitch_.make_and_bind_directly(0.0); if (!chassis_imu_roll_.ready()) @@ -244,9 +249,10 @@ class DeformableChassis } update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_suspension_toggle_from_inputs_(switch_left, switch_right); + *ctrl_hold_active_ = ctrl_hold_requested_by_input_(); update_velocity_control(); update_lift_target_toggle(keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right); run_joint_intent_pipeline_(); } while (false); @@ -457,8 +463,12 @@ class DeformableChassis } } + bool ctrl_hold_requested_by_input_() const { + return prone_override_requested_by_keyboard() || suspension_on_by_switch; + } + bool suspension_requested_by_input_() const { - return active_suspension_enable_ && (prone_override_requested_by_keyboard() || suspension_on_by_switch); + return active_suspension_enable_ && ctrl_hold_requested_by_input_(); } bool symmetric_joint_target_requested_() const { @@ -621,6 +631,7 @@ class DeformableChassis void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; + *ctrl_hold_active_ = false; reset_attitude_correction_state_(); reset_chassis_imu_calibration_window_(); @@ -691,6 +702,13 @@ class DeformableChassis } double update_angular_velocity_control() { + if (*ctrl_hold_active_ && std::isfinite(*manual_yaw_velocity_override_)) { + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = nan_; + return std::clamp( + *manual_yaw_velocity_override_, -angular_velocity_max_, angular_velocity_max_); + } + double angular_velocity = 0.0; double chassis_control_angle = nan_; @@ -976,10 +994,12 @@ class DeformableChassis double last_rotary_knob_ = 0.0; InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface manual_yaw_velocity_override_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; OutputInterface chassis_control_velocity_; + OutputInterface ctrl_hold_active_; bool spinning_forward_ = true; bool apply_symmetric_target = true; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 1bc50158..f3ffe5c6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -35,6 +35,9 @@ class DeformableInfantryGimbalController get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); + get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); + get_parameter_or( + "ctrl_hold_chassis_yaw_velocity_max", ctrl_hold_chassis_yaw_velocity_max_, 30.0); } auto update() -> void override { @@ -48,6 +51,13 @@ class DeformableInfantryGimbalController return; } + if (ctrl_hold_requested()) { + update_ctrl_hold_control(); + return; + } + + deactivate_ctrl_hold(); + const auto auto_aim_active = auto_aim_requested() && input_.auto_aim_should_control.ready() && *input_.auto_aim_should_control && input_.auto_aim_control_direction.ready() @@ -100,6 +110,7 @@ class DeformableInfantryGimbalController private: static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { get_parameter(prefix + "_integral_min", calculator.integral_min); @@ -118,8 +129,11 @@ class DeformableInfantryGimbalController component.register_input("/remote/switch/left", switch_left); component.register_input("/remote/mouse/velocity", mouse_velocity); component.register_input("/remote/mouse", mouse); + component.register_input("/predefined/update_rate", update_rate, false); + component.register_input("/chassis/ctrl_hold_active", ctrl_hold_active, false); component.register_input("/tf", tf); + component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); @@ -141,8 +155,11 @@ class DeformableInfantryGimbalController InputInterface switch_left; InputInterface mouse_velocity; InputInterface mouse; + InputInterface update_rate; + InputInterface ctrl_hold_active; InputInterface tf; + InputInterface yaw_angle; InputInterface yaw_velocity; InputInterface pitch_velocity; InputInterface yaw_velocity_imu; @@ -159,20 +176,43 @@ class DeformableInfantryGimbalController struct Output { explicit Output(rmcs_executor::Component& component) { component.register_output("/gimbal/yaw/control_torque", yaw_control_torque, kNaN); + component.register_output("/gimbal/yaw/control_angle", yaw_control_angle, kNaN); component.register_output( "/gimbal/pitch/control_velocity", pitch_control_velocity, kNaN); component.register_output("/gimbal/pitch/control_torque", pitch_control_torque, kNaN); + component.register_output("/gimbal/pitch/control_angle", pitch_control_angle, kNaN); component.register_output("/gimbal/yaw/control_angle_error", yaw_angle_error, kNaN); component.register_output("/gimbal/pitch/control_angle_error", pitch_angle_error, kNaN); + component.register_output( + "/chassis/manual_yaw_velocity_override", chassis_manual_yaw_velocity_override, + kNaN); } OutputInterface yaw_control_torque; + OutputInterface yaw_control_angle; OutputInterface pitch_control_velocity; OutputInterface pitch_control_torque; + OutputInterface pitch_control_angle; OutputInterface yaw_angle_error; OutputInterface pitch_angle_error; + OutputInterface chassis_manual_yaw_velocity_override; } output_{*this}; + auto ctrl_hold_requested() const -> bool { + return input_.ctrl_hold_active.ready() && *input_.ctrl_hold_active; + } + + auto update_dt() const -> double { + if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) && *input_.update_rate > 1e-6) + return 1.0 / *input_.update_rate; + return kDefaultDt; + } + + auto manual_yaw_shift() const -> double { + return joystick_sensitivity_ * input_.joystick_left->y() + + mouse_sensitivity_ * input_.mouse_velocity->y(); + } + auto auto_aim_requested() const -> bool { return input_.mouse->right || *input_.switch_right == rmcs_msgs::Switch::UP; } @@ -187,27 +227,61 @@ class DeformableInfantryGimbalController if (!gimbal_solver_.enabled()) return gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevel{}); - const auto yaw_shift = joystick_sensitivity_ * input_.joystick_left->y() - + mouse_sensitivity_ * input_.mouse_velocity->y(); - if (input_.keyboard->ctrl) - return gimbal_solver_.update(TwoAxisGimbalSolver::SetToLevelYawShift{yaw_shift}); + const auto yaw_shift = manual_yaw_shift(); const auto pitch_shift = -joystick_sensitivity_ * input_.joystick_left->x() + mouse_sensitivity_ * input_.mouse_velocity->x(); return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + auto activate_ctrl_hold() -> void { + ctrl_hold_active_ = true; + locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) + ? *input_.yaw_angle + : 0.0; + gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + } + + auto deactivate_ctrl_hold() -> void { + if (!ctrl_hold_active_) + return; + + ctrl_hold_active_ = false; + locked_yaw_angle_ = kNaN; + gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); + } + + auto update_ctrl_hold_control() -> void { + if (!ctrl_hold_active_) + activate_ctrl_hold(); + + reset_control_outputs(); + *output_.yaw_angle_error = kNaN; + *output_.pitch_angle_error = kNaN; + *output_.yaw_control_angle = locked_yaw_angle_; + *output_.pitch_control_angle = ctrl_hold_pitch_target_angle_; + + const auto yaw_velocity_override = std::clamp( + manual_yaw_shift() / update_dt(), -ctrl_hold_chassis_yaw_velocity_max_, + ctrl_hold_chassis_yaw_velocity_max_); + *output_.chassis_manual_yaw_velocity_override = yaw_velocity_override; + } + auto reset_control_outputs() -> void { yaw_angle_pid_.reset(); yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; + *output_.chassis_manual_yaw_velocity_override = kNaN; } auto reset_all_controls() -> void { + deactivate_ctrl_hold(); gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; @@ -244,6 +318,10 @@ class DeformableInfantryGimbalController double yaw_vel_ff_gain_ = 0.0; double yaw_acc_ff_gain_ = 0.0; double pitch_acc_ff_gain_ = 0.0; + double ctrl_hold_pitch_target_angle_ = 0.0; + double ctrl_hold_chassis_yaw_velocity_max_ = 30.0; + bool ctrl_hold_active_ = false; + double locked_yaw_angle_ = kNaN; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 0acb879b..25d8fe1e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -317,7 +317,7 @@ class DeformableInfantryOmniB }); builder.can2_transmit({ .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ .can_id = 0x1FE, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index cfd8b398..67044424 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -290,7 +290,7 @@ class DeformableInfantryOmni }); builder.can2_transmit({ .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ .can_id = 0x1FE, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index c811d97c..9d6d10a7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -413,7 +413,7 @@ class DeformableInfantryV2 }); builder.can2_transmit({ .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), + .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); } } From cd088fd586ef15c8d7c83e437d6e07aaad04af75 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 9 Jun 2026 22:48:26 +0800 Subject: [PATCH 59/90] feat(gimbal): reset yaw and pitch control angles to NaN on disable --- .../gimbal/deformable_infantry_gimbal_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index f3ffe5c6..d60f9384 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -248,6 +248,9 @@ class DeformableInfantryGimbalController ctrl_hold_active_ = false; locked_yaw_angle_ = kNaN; + *output_.yaw_control_angle = kNaN; + *output_.pitch_control_angle = kNaN; + *output_.chassis_manual_yaw_velocity_override = kNaN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } From dc181e7742771aae47d5f993ec7d0a3ec4f984fd Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 11 Jun 2026 19:52:53 +0800 Subject: [PATCH 60/90] feat: update gimbal control parameters and remove unused librmcs files Co-authored-by: Copilot --- .../config/deformable-infantry-omni-b.yaml | 4 +- rmcs_ws/src/rmcs_core/CMakeLists.txt | 21 - .../deformable_infantry_gimbal_controller.cpp | 19 +- .../host/include/librmcs/protocol/handler.hpp | 61 --- .../librmcs/host/src/protocol/handler.cpp | 390 ----------------- .../librmcs/host/src/transport/transport.hpp | 56 --- .../librmcs/host/src/transport/usb/usb.cpp | 398 ------------------ 7 files changed, 18 insertions(+), 931 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp delete mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp delete mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp delete mode 100644 rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 8976c1ea..ff8dd430 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -35,8 +35,8 @@ rmcs_executor: value_broadcaster: ros__parameters: forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity + - /gimbal/pitch/angle + - /gimbal/pitch/control_torque deformable_infantry: diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 89fbdb4f..d275c6db 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -23,27 +23,6 @@ FetchContent_Declare( ) FetchContent_MakeAvailable(librmcs) -configure_file( - ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp - ${librmcs_SOURCE_DIR}/host/include/librmcs/protocol/handler.hpp - COPYONLY -) -configure_file( - ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/protocol/handler.cpp - ${librmcs_SOURCE_DIR}/host/src/protocol/handler.cpp - COPYONLY -) -configure_file( - ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/transport/transport.hpp - ${librmcs_SOURCE_DIR}/host/src/transport/transport.hpp - COPYONLY -) -configure_file( - ${PROJECT_SOURCE_DIR}/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp - ${librmcs_SOURCE_DIR}/host/src/transport/usb/usb.cpp - COPYONLY -) - file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp ${PROJECT_SOURCE_DIR}/src/*.c diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index d60f9384..f8169e63 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -47,10 +47,14 @@ class DeformableInfantryGimbalController using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + suspension_on_by_switch_ = false; + last_switch_right_ = switch_right; reset_all_controls(); return; } + update_ctrl_hold_request_state(switch_left, switch_right); + if (ctrl_hold_requested()) { update_ctrl_hold_control(); return; @@ -130,7 +134,6 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse/velocity", mouse_velocity); component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/chassis/ctrl_hold_active", ctrl_hold_active, false); component.register_input("/tf", tf); component.register_input("/gimbal/yaw/angle", yaw_angle); @@ -156,7 +159,6 @@ class DeformableInfantryGimbalController InputInterface mouse_velocity; InputInterface mouse; InputInterface update_rate; - InputInterface ctrl_hold_active; InputInterface tf; InputInterface yaw_angle; @@ -199,7 +201,16 @@ class DeformableInfantryGimbalController } output_{*this}; auto ctrl_hold_requested() const -> bool { - return input_.ctrl_hold_active.ready() && *input_.ctrl_hold_active; + return (input_.keyboard.ready() && input_.keyboard->ctrl) || suspension_on_by_switch_; + } + + auto update_ctrl_hold_request_state( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) -> void { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + suspension_on_by_switch_ = !suspension_on_by_switch_; + } + last_switch_right_ = switch_right; } auto update_dt() const -> double { @@ -323,7 +334,9 @@ class DeformableInfantryGimbalController double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; double ctrl_hold_chassis_yaw_velocity_max_ = 30.0; + bool suspension_on_by_switch_ = false; bool ctrl_hold_active_ = false; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; double locked_yaw_angle_ = kNaN; }; diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp deleted file mode 100644 index 90c0a5f1..00000000 --- a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/include/librmcs/protocol/handler.hpp +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include - -namespace librmcs::host::protocol { - -class LIBRMCS_API Handler { -public: - class LIBRMCS_API PacketBuilder { - public: - PacketBuilder(const PacketBuilder&) = delete; - PacketBuilder& operator=(const PacketBuilder&) = delete; - PacketBuilder(PacketBuilder&&) = delete; - PacketBuilder& operator=(PacketBuilder&&) = delete; - - ~PacketBuilder() noexcept; - - bool write_can(data::DataId field_id, const data::CanDataView& view) noexcept; - - bool write_uart(data::DataId field_id, const data::UartDataView& view) noexcept; - - bool write_gpio_digital_data( - uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept; - - bool write_gpio_digital_read_config( - uint8_t channel_index, const data::GpioReadConfigView& view) noexcept; - - bool write_gpio_analog_data( - uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept; - - private: - friend class Handler; - - explicit PacketBuilder(void* shared_state) noexcept; - - alignas(std::uintptr_t) std::uint8_t storage_[16 * sizeof(std::uintptr_t)]; - }; - - Handler( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const agent::AdvancedOptions& options, data::DataCallback& callback); - - Handler(const Handler&) = delete; - Handler& operator=(const Handler&) = delete; - Handler(Handler&& other) noexcept; - Handler& operator=(Handler&& other) noexcept; - - ~Handler() noexcept; - - PacketBuilder start_transmit() noexcept; - -private: - void* shared_state_ = nullptr; -}; - -} // namespace librmcs::host::protocol diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp deleted file mode 100644 index a52bdde7..00000000 --- a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/protocol/handler.cpp +++ /dev/null @@ -1,390 +0,0 @@ -#include "librmcs/protocol/handler.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "core/src/protocol/deserializer.hpp" -#include "core/src/protocol/protocol.hpp" -#include "core/src/protocol/serializer.hpp" -#include "core/src/utility/assert.hpp" -#include "host/src/logging/logging.hpp" -#include "host/src/protocol/stream_buffer.hpp" -#include "host/src/transport/transport.hpp" -#include "librmcs/agent/common.hpp" -#include "librmcs/data/datas.hpp" - -namespace librmcs::host::protocol { - -namespace { - -class NullTransport final : public transport::Transport { -public: - std::unique_ptr acquire_transmit_buffer() noexcept override { - return nullptr; - } - - void transmit(std::unique_ptr buffer, size_t) override { - (void)buffer; - } - - void release_transmit_buffer(std::unique_ptr buffer) override { - (void)buffer; - } - - void receive(std::function)> callback) override { - receive_callback_ = std::move(callback); - } - - bool healthy() const noexcept override { return false; } - -private: - std::function)> receive_callback_; -}; - -} // namespace - -class HandlerImpl : public core::protocol::DeserializeCallback { -public: - explicit HandlerImpl(std::unique_ptr transport, data::DataCallback& callback) - : transport_(std::move(transport)) - , callback_(callback) - , deserializer_(*this) { - transport_->receive([this](std::span buffer) { - // Operating system automatically assembles the packet - deserializer_.feed(buffer); - deserializer_.finish_transfer(); - }); - } - - transport::Transport& transport() noexcept { return *transport_; } - - bool healthy() const noexcept { return transport_->healthy(); } - - bool is_null_transport() const noexcept { - return dynamic_cast(transport_.get()) != nullptr; - } - - void can_deserialized_callback( - core::protocol::FieldId id, const data::CanDataView& data) override { - if (!callback_.can_receive_callback(id, data)) - logging::get_logger().error("Unexpected can field id: ", static_cast(id)); - } - - void uart_deserialized_callback( - core::protocol::FieldId id, const data::UartDataView& data) override { - if (!callback_.uart_receive_callback(id, data)) - logging::get_logger().error("Unexpected uart field id: ", static_cast(id)); - } - - void gpio_digital_data_deserialized_callback( - uint8_t channel_index, const data::GpioDigitalDataView& data) override { - callback_.gpio_digital_read_result_callback(channel_index, data); - } - - void gpio_analog_data_deserialized_callback( - uint8_t channel_index, const data::GpioAnalogDataView& data) override { - callback_.gpio_analog_read_result_callback(channel_index, data); - } - - void gpio_digital_read_config_deserialized_callback( - uint8_t channel_index, const data::GpioReadConfigView& data) override { - (void)channel_index; - (void)data; - logging::get_logger().error("Unexpected gpio digital read config field in uplink"); - } - - void gpio_analog_read_config_deserialized_callback( - uint8_t channel_index, const data::GpioReadConfigView& data) override { - (void)channel_index; - (void)data; - logging::get_logger().error("Unexpected gpio analog read config field in uplink"); - } - - void accelerometer_deserialized_callback(const data::AccelerometerDataView& data) override { - callback_.accelerometer_receive_callback(data); - } - - void gyroscope_deserialized_callback(const data::GyroscopeDataView& data) override { - callback_.gyroscope_receive_callback(data); - } - - void temperature_deserialized_callback(const data::TemperatureDataView& data) override { - callback_.temperature_receive_callback(data); - } - - void error_callback() override { - logging::get_logger().error("Deserializer encountered an error while parsing input"); - } - -private: - std::unique_ptr transport_; - data::DataCallback& callback_; - core::protocol::Deserializer deserializer_; -}; - -class HandlerSharedState { -public: - HandlerSharedState( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const agent::AdvancedOptions& options, data::DataCallback& callback) - : usb_vid_(usb_vid) - , usb_pid_(usb_pid) - , serial_filter_(serial_filter) - , options_(options) - , callback_(callback) - , impl_(std::make_shared(create_transport_(), callback_)) - , reconnect_thread_([this]() { reconnect_loop_(); }) {} - - ~HandlerSharedState() { - stop_reconnect_thread_.store(true, std::memory_order::relaxed); - if (reconnect_thread_.joinable()) - reconnect_thread_.join(); - } - - std::shared_ptr acquire_impl_for_transmit() noexcept { - maybe_restart_transport_(); - - const std::scoped_lock guard{mutex_}; - core::utility::assert_debug(static_cast(impl_)); - return impl_; - } - - void note_transmit_result(core::protocol::Serializer::SerializeResult result) noexcept { - using SerializeResult = core::protocol::Serializer::SerializeResult; - - if (result == SerializeResult::kSuccess) [[likely]] { - consecutive_acquire_failures_.store(0, std::memory_order::relaxed); - return; - } - - if (result == SerializeResult::kBadAlloc) { - logging::get_logger().error("Transmit buffer unavailable (acquire failed)"); - - const uint32_t failures = - consecutive_acquire_failures_.fetch_add(1, std::memory_order::relaxed) + 1; - if (failures >= kRestartFailureThreshold_ - && !transport_restart_requested_.exchange(true, std::memory_order::relaxed)) { - logging::get_logger().warn( - "USB transport restart scheduled after {} consecutive transmit buffer " - "acquire failures", - failures); - } - return; - } - - core::utility::assert_failed_debug(); - } - -private: - std::unique_ptr create_transport_() const { - return transport::usb::create_transport( - usb_vid_, usb_pid_, serial_filter_, transport::usb::ConnectionOptions{ - .dangerously_skip_version_checks = - options_.dangerously_skip_version_checks, - }); - } - - void maybe_restart_transport_() noexcept { - auto now = std::chrono::steady_clock::now(); - - const std::scoped_lock guard{mutex_}; - const bool reconnect_due_to_failure = - transport_restart_requested_.load(std::memory_order::relaxed); - const bool reconnect_due_to_unhealthy_transport = impl_ && !impl_->healthy(); - const bool reconnect_due_to_null_transport = impl_ && impl_->is_null_transport(); - if (!reconnect_due_to_failure && !reconnect_due_to_unhealthy_transport - && !reconnect_due_to_null_transport) - return; - if (now < next_restart_time_) - return; - - const uint32_t failures = - consecutive_acquire_failures_.load(std::memory_order::relaxed); - - std::shared_ptr old_impl; - old_impl.swap(impl_); - old_impl.reset(); - - try { - impl_ = std::make_shared(create_transport_(), callback_); - consecutive_acquire_failures_.store(0, std::memory_order::relaxed); - transport_restart_requested_.store(false, std::memory_order::relaxed); - next_restart_time_ = std::chrono::steady_clock::time_point::min(); - logging::get_logger().warn( - "USB transport restarted after {} consecutive transmit buffer acquire failures", - failures); - } catch (const std::exception& ex) { - impl_ = std::make_shared(std::make_unique(), callback_); - transport_restart_requested_.store(true, std::memory_order::relaxed); - next_restart_time_ = std::chrono::steady_clock::now() + kRestartRetryInterval_; - logging::get_logger().error("USB transport restart failed: {}", ex.what()); - } catch (...) { - impl_ = std::make_shared(std::make_unique(), callback_); - transport_restart_requested_.store(true, std::memory_order::relaxed); - next_restart_time_ = std::chrono::steady_clock::now() + kRestartRetryInterval_; - logging::get_logger().error("USB transport restart failed: unknown error"); - } - } - - void reconnect_loop_() { - while (!stop_reconnect_thread_.load(std::memory_order::relaxed)) { - maybe_restart_transport_(); - std::this_thread::sleep_for(kRestartRetryInterval_); - } - } - - static constexpr uint32_t kRestartFailureThreshold_ = 8; - static constexpr auto kRestartRetryInterval_ = std::chrono::milliseconds{500}; - - std::mutex mutex_; - uint16_t usb_vid_; - int32_t usb_pid_; - std::string serial_filter_; - agent::AdvancedOptions options_; - data::DataCallback& callback_; - std::shared_ptr impl_; - std::atomic consecutive_acquire_failures_{0}; - std::atomic transport_restart_requested_{false}; - std::atomic stop_reconnect_thread_{false}; - std::chrono::steady_clock::time_point next_restart_time_ = - std::chrono::steady_clock::time_point::min(); - std::thread reconnect_thread_; -}; - -namespace { - -struct PacketBuilderImpl { - explicit PacketBuilderImpl(HandlerSharedState& shared_state) noexcept - : impl_(shared_state.acquire_impl_for_transmit()) - , shared_state_(shared_state) - , buffer_(impl_->transport()) - , serializer_(buffer_) {} - - PacketBuilderImpl(PacketBuilderImpl&& other) noexcept - : impl_(std::move(other.impl_)) - , shared_state_(other.shared_state_) - , buffer_(std::move(other.buffer_)) - , serializer_(buffer_) {} - - PacketBuilderImpl& operator=(PacketBuilderImpl&&) = delete; - PacketBuilderImpl(const PacketBuilderImpl&) = delete; - PacketBuilderImpl& operator=(const PacketBuilderImpl&) = delete; - ~PacketBuilderImpl() = default; - - [[nodiscard]] bool write_can(data::DataId field_id, const data::CanDataView& view) noexcept { - return process_result(serializer_.write_can(field_id, view)); - } - - [[nodiscard]] bool write_uart(data::DataId field_id, const data::UartDataView& view) noexcept { - return process_result(serializer_.write_uart(field_id, view)); - } - - [[nodiscard]] bool write_gpio_digital_data( - uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept { - return process_result(serializer_.write_gpio_digital_data(channel_index, view)); - } - - [[nodiscard]] bool write_gpio_digital_read_config( - uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { - return process_result(serializer_.write_gpio_digital_read_config(channel_index, view)); - } - - [[nodiscard]] bool write_gpio_analog_data( - uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept { - return process_result(serializer_.write_gpio_analog_data(channel_index, view)); - } - -private: - bool process_result(core::protocol::Serializer::SerializeResult result) noexcept { - using SerializeResult = core::protocol::Serializer::SerializeResult; - if (result == SerializeResult::kInvalidArgument) - return false; - - shared_state_.note_transmit_result(result); - return true; - } - - std::shared_ptr impl_; - HandlerSharedState& shared_state_; - StreamBuffer buffer_; - core::protocol::Serializer serializer_; -}; - -} // namespace - -Handler::PacketBuilder::PacketBuilder(void* shared_state_ptr) noexcept { - static_assert(sizeof(PacketBuilderImpl) <= sizeof(storage_)); - static_assert(alignof(PacketBuilderImpl) <= alignof(std::uintptr_t)); - - auto& shared_state = *static_cast(shared_state_ptr); - std::construct_at(reinterpret_cast(storage_), shared_state); -} - -Handler::PacketBuilder::~PacketBuilder() noexcept { - std::destroy_at(std::launder(reinterpret_cast(storage_))); -} - -bool Handler::PacketBuilder::write_can( - data::DataId field_id, const data::CanDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_))->write_can(field_id, view); -} - -bool Handler::PacketBuilder::write_uart( - data::DataId field_id, const data::UartDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_)) - ->write_uart(field_id, view); -} - -bool Handler::PacketBuilder::write_gpio_digital_data( - uint8_t channel_index, const data::GpioDigitalDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_)) - ->write_gpio_digital_data(channel_index, view); -} - -bool Handler::PacketBuilder::write_gpio_digital_read_config( - uint8_t channel_index, const data::GpioReadConfigView& view) noexcept { - return std::launder(reinterpret_cast(storage_)) - ->write_gpio_digital_read_config(channel_index, view); -} - -bool Handler::PacketBuilder::write_gpio_analog_data( - uint8_t channel_index, const data::GpioAnalogDataView& view) noexcept { - return std::launder(reinterpret_cast(storage_)) - ->write_gpio_analog_data(channel_index, view); -} - -Handler::Handler( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const agent::AdvancedOptions& options, data::DataCallback& callback) - : shared_state_(new HandlerSharedState(usb_vid, usb_pid, serial_filter, options, callback)) {} - -Handler::Handler(Handler&& other) noexcept - : shared_state_(std::exchange(other.shared_state_, nullptr)) {} - -Handler& Handler::operator=(Handler&& other) noexcept { - if (this == &other) - return *this; - delete static_cast(shared_state_); - shared_state_ = std::exchange(other.shared_state_, nullptr); - return *this; -} - -Handler::~Handler() noexcept { delete static_cast(shared_state_); } - -Handler::PacketBuilder Handler::start_transmit() noexcept { - core::utility::assert_debug(shared_state_); - return PacketBuilder{shared_state_}; -} - -} // namespace librmcs::host::protocol diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp deleted file mode 100644 index c298680e..00000000 --- a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/transport.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "core/src/protocol/constant.hpp" - -namespace librmcs::host::transport { - -class TransportBuffer { -public: - using BufferSpanType = std::span; - - TransportBuffer() = default; - TransportBuffer(const TransportBuffer&) = delete; - TransportBuffer& operator=(const TransportBuffer&) = delete; - TransportBuffer(TransportBuffer&&) = delete; - TransportBuffer& operator=(TransportBuffer&&) = delete; - virtual ~TransportBuffer() noexcept = default; - - virtual BufferSpanType data() const noexcept = 0; -}; - -class Transport { -public: - Transport() = default; - virtual ~Transport() noexcept = default; - Transport(const Transport&) = delete; - Transport& operator=(const Transport&) = delete; - Transport(Transport&&) = delete; - Transport& operator=(Transport&&) = delete; - - virtual std::unique_ptr acquire_transmit_buffer() noexcept = 0; - virtual void transmit(std::unique_ptr buffer, size_t payload_size) = 0; - virtual void release_transmit_buffer(std::unique_ptr buffer) = 0; - virtual void receive(std::function)> callback) = 0; - virtual bool healthy() const noexcept { return true; } -}; - -namespace usb { - -struct ConnectionOptions { - bool dangerously_skip_version_checks = false; -}; - -std::unique_ptr create_transport( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const ConnectionOptions& options); - -} // namespace usb - -} // namespace librmcs::host::transport diff --git a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp b/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp deleted file mode 100644 index 738ae70e..00000000 --- a/rmcs_ws/src/rmcs_core/third_party_overrides/librmcs/host/src/transport/usb/usb.cpp +++ /dev/null @@ -1,398 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "core/src/protocol/constant.hpp" -#include "core/src/utility/assert.hpp" -#include "host/src/logging/logging.hpp" -#include "host/src/transport/transport.hpp" -#include "host/src/transport/usb/device_scanner.hpp" -#include "host/src/transport/usb/helper.hpp" -#include "host/src/utility/final_action.hpp" -#include "host/src/utility/ring_buffer.hpp" - -namespace librmcs::host::transport::usb { - -class Usb : public Transport { -public: - explicit Usb( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const ConnectionOptions& options) - : logger_(logging::get_logger()) - , free_transmit_transfers_(kTransmitTransferCount) { - - usb_init(usb_vid, usb_pid, serial_filter, options); - utility::FinalAction rollback_on_failure{[this]() noexcept { - destroy_free_transmit_transfers(); - libusb_release_interface(libusb_device_handle_, kTargetInterface); - libusb_close(libusb_device_handle_); - libusb_exit(libusb_context_); - }}; - - init_transmit_transfers(); - event_thread_ = std::thread{[this]() { handle_events(); }}; - - rollback_on_failure.disable(); - } - - Usb(const Usb&) = delete; - Usb& operator=(const Usb&) = delete; - Usb(Usb&&) = delete; - Usb& operator=(Usb&&) = delete; - - ~Usb() override { - { - const std::scoped_lock guard{transmit_transfer_push_mutex_}; - stop_handling_events_.store(true, std::memory_order::relaxed); - } - destroy_free_transmit_transfers(); - - libusb_release_interface(libusb_device_handle_, kTargetInterface); - - // libusb_close() reliably cancels all pending transfers and invokes their callbacks, - // avoiding race conditions present in other cancellation methods - libusb_close(libusb_device_handle_); - - if (event_thread_.joinable()) - event_thread_.join(); - - libusb_exit(libusb_context_); - } - - std::unique_ptr acquire_transmit_buffer() noexcept override { - if (!connection_healthy_.load(std::memory_order::relaxed)) - return nullptr; - - TransferWrapper* transfer = nullptr; - { - const std::scoped_lock guard{transmit_transfer_pop_mutex_}; - free_transmit_transfers_.pop_front( - [&transfer](TransferWrapper* value) noexcept { transfer = value; }); - } - if (!transfer) - return nullptr; - - return std::unique_ptr{transfer}; - } - - void transmit(std::unique_ptr buffer, size_t size) override { - core::utility::assert_debug(static_cast(buffer)); - - if (size > core::protocol::kProtocolBufferSize) - throw std::invalid_argument("Transmit size exceeds maximum transfer length"); - - if (!connection_healthy_.load(std::memory_order::relaxed)) { - release_transmit_buffer(std::move(buffer)); - return; - } - - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) - auto& transfer = static_cast(buffer.get())->transfer_; - transfer->length = static_cast(size); - - int ret = libusb_submit_transfer(transfer); - if (ret != 0) [[unlikely]] { - mark_connection_lost_( - std::format( - "Failed to submit transmit transfer: {} ({})", ret, - helper::libusb_errname(ret))); - release_transmit_buffer(std::move(buffer)); - return; - } - - // If success: Ownership is transferred to libusb - std::ignore = buffer.release(); - } - - void release_transmit_buffer(std::unique_ptr buffer) override { - core::utility::assert_debug(static_cast(buffer)); - - const std::scoped_lock guard{transmit_transfer_push_mutex_}; - - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) - auto* wrapper = static_cast(buffer.release()); - free_transmit_transfers_.emplace_back(wrapper); - } - - void receive(std::function)> callback) override { - if (!callback) - throw std::invalid_argument{"Callback function cannot be null"}; - if (receive_callback_) - throw std::logic_error{"Receive function can only be called once"}; - - receive_callback_ = std::move(callback); - init_receive_transfers(); - } - - bool healthy() const noexcept override { - return connection_healthy_.load(std::memory_order::relaxed); - } - -private: - class TransferWrapper : public TransportBuffer { - friend class Usb; - - public: - explicit TransferWrapper(Usb& self) - : self_(self) - , transfer_(self_.create_libusb_transfer()) {} - - TransferWrapper(const TransferWrapper&) = delete; - TransferWrapper& operator=(const TransferWrapper&) = delete; - TransferWrapper(TransferWrapper&&) = delete; - TransferWrapper& operator=(TransferWrapper&&) = delete; - - ~TransferWrapper() override { - if (transfer_) { - logging::get_logger().error( - "USB TransferBuffer {} was destroyed externally - this is undefined behavior. " - "Buffers must be returned via transmit() or " - "release_transmit_buffer(). ", - static_cast(this)); - destroy(); - } - } - - BufferSpanType data() const noexcept override { - return BufferSpanType{ - reinterpret_cast(transfer_->buffer), - core::protocol::kProtocolBufferSize}; - } - - void destroy() noexcept { - self_.destroy_libusb_transfer(transfer_); - transfer_ = nullptr; - } - - private: - Usb& self_; - - libusb_transfer* transfer_; - }; - - void usb_init( - uint16_t vendor_id, int32_t product_id, std::string_view serial_filter, - const ConnectionOptions& options) { - if (const int ret = libusb_init(&libusb_context_); ret != 0) [[unlikely]] { - throw std::runtime_error( - std::format( - "Failed to initialize libusb: {} ({})", ret, helper::libusb_errname(ret))); - } - utility::FinalAction exit_libusb{[this]() noexcept { libusb_exit(libusb_context_); }}; - - libusb_device_handle_ = DeviceScanner::select_device( - libusb_context_, vendor_id, product_id, serial_filter, options); - utility::FinalAction close_device_handle{ - [this]() noexcept { libusb_close(libusb_device_handle_); }}; - - if (const int ret = libusb_claim_interface(libusb_device_handle_, kTargetInterface); - ret != 0) [[unlikely]] { - throw std::runtime_error( - std::format( - "Failed to claim interface {}: {} ({})", kTargetInterface, ret, - helper::libusb_errname(ret))); - } - - close_device_handle.disable(); - exit_libusb.disable(); - } - - void init_transmit_transfers() { - TransferWrapper* transmit_transfers[kTransmitTransferCount] = {}; - try { - for (auto& wrapper : transmit_transfers) { - wrapper = new TransferWrapper{*this}; - auto* transfer = wrapper->transfer_; - - libusb_fill_bulk_transfer( - transfer, libusb_device_handle_, kOutEndpoint, - new unsigned char[core::protocol::kProtocolBufferSize], 0, - [](libusb_transfer* transfer) { - auto* wrapper = static_cast(transfer->user_data); - wrapper->self_.usb_transmit_complete_callback(wrapper); - }, - wrapper, 0); - transfer->flags = libusb_transfer_flags::LIBUSB_TRANSFER_FREE_BUFFER; - } - } catch (...) { - for (auto& wrapper : transmit_transfers) { - if (wrapper) { - wrapper->destroy(); - delete wrapper; - } - } - throw; - } - - auto* iter = transmit_transfers; - free_transmit_transfers_.push_back_n( - [&iter]() noexcept { return *iter++; }, kTransmitTransferCount); - } - - void handle_events() { - while (active_transfers_.load(std::memory_order::relaxed)) { - libusb_handle_events(libusb_context_); - } - } - - void init_receive_transfers() { - for (size_t i = 0; i < kReceiveTransferCount; i++) { - auto* transfer = create_libusb_transfer(); - - libusb_fill_bulk_transfer( - transfer, libusb_device_handle_, kInEndpoint, - new unsigned char[core::protocol::kProtocolBufferSize], - static_cast(core::protocol::kProtocolBufferSize), - [](libusb_transfer* transfer) { - static_cast(transfer->user_data)->usb_receive_complete_callback(transfer); - }, - this, 0); - transfer->flags = libusb_transfer_flags::LIBUSB_TRANSFER_FREE_BUFFER; - - int ret = libusb_submit_transfer(transfer); - if (ret != 0) [[unlikely]] { - destroy_libusb_transfer(transfer); - throw std::runtime_error( - std::format( - "Failed to submit receive transfer: {} ({})", ret, - helper::libusb_errname(ret))); - } - } - } - - void usb_transmit_complete_callback(TransferWrapper* wrapper) { - const std::scoped_lock guard{transmit_transfer_push_mutex_}; - - if (stop_handling_events_.load(std::memory_order::relaxed)) [[unlikely]] { - wrapper->destroy(); - delete wrapper; - return; - } - - if (wrapper->transfer_->status != LIBUSB_TRANSFER_COMPLETED) [[unlikely]] { - mark_connection_lost_( - std::format( - "Transmit transfer failed with status {}", - static_cast(wrapper->transfer_->status))); - wrapper->destroy(); - delete wrapper; - return; - } - - free_transmit_transfers_.emplace_back(wrapper); - } - - void usb_receive_complete_callback(libusb_transfer* transfer) { - if (stop_handling_events_.load(std::memory_order::relaxed)) [[unlikely]] { - destroy_libusb_transfer(transfer); - return; - } - - if (transfer->status != LIBUSB_TRANSFER_COMPLETED) [[unlikely]] { - mark_connection_lost_( - std::format( - "Receive transfer failed with status {}", - static_cast(transfer->status))); - destroy_libusb_transfer(transfer); - return; - } - - const auto now = std::chrono::steady_clock::now(); - const bool should_drop = now > last_rx_callback_timepoint_ + std::chrono::seconds{1}; - last_rx_callback_timepoint_ = now; - - if (!should_drop && transfer->actual_length > 0) { - const auto* first = reinterpret_cast(transfer->buffer); - const auto size = static_cast(transfer->actual_length); - receive_callback_({first, size}); - } - - int ret = libusb_submit_transfer(transfer); - if (ret != 0) [[unlikely]] { - mark_connection_lost_( - std::format( - "Failed to re-submit receive transfer: {} ({})", ret, - helper::libusb_errname(ret))); - destroy_libusb_transfer(transfer); - } - } - - void mark_connection_lost_(const std::string& reason) noexcept { - bool expected = true; - if (!connection_healthy_.compare_exchange_strong( - expected, false, std::memory_order::relaxed, std::memory_order::relaxed)) { - return; - } - - logger_.error("USB transport marked unhealthy: {}", reason); - } - - void destroy_free_transmit_transfers() noexcept { - free_transmit_transfers_.pop_front_n([](TransferWrapper* wrapper) noexcept { - wrapper->destroy(); - delete wrapper; - }); - } - - libusb_transfer* create_libusb_transfer() { - auto* transfer = libusb_alloc_transfer(0); - if (!transfer) - throw std::bad_alloc{}; - active_transfers_.fetch_add(1, std::memory_order::relaxed); - return transfer; - } - - void destroy_libusb_transfer(libusb_transfer* transfer) noexcept { - libusb_free_transfer(transfer); - active_transfers_.fetch_sub(1, std::memory_order::relaxed); - } - - static constexpr int kTargetInterface = 0x00; - - static constexpr unsigned char kOutEndpoint = 0x01; - static constexpr unsigned char kInEndpoint = 0x81; - - static constexpr size_t kTransmitTransferCount = 64; - static constexpr size_t kReceiveTransferCount = 4; - - logging::Logger& logger_; - - libusb_context* libusb_context_ = nullptr; - libusb_device_handle* libusb_device_handle_ = nullptr; - - std::thread event_thread_; - - std::atomic active_transfers_ = 0; - std::atomic stop_handling_events_ = false; - std::atomic connection_healthy_ = true; - - utility::RingBuffer free_transmit_transfers_; - std::mutex transmit_transfer_pop_mutex_, transmit_transfer_push_mutex_; - - std::function)> receive_callback_; - std::chrono::steady_clock::time_point last_rx_callback_timepoint_ = - std::chrono::steady_clock::time_point::min(); -}; - -std::unique_ptr create_transport( - uint16_t usb_vid, int32_t usb_pid, std::string_view serial_filter, - const ConnectionOptions& options) { - return std::make_unique(usb_vid, usb_pid, serial_filter, options); -} - -} // namespace librmcs::host::transport::usb From 235f08ff557a303034f43bc128311f31e6982c9b Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 11 Jun 2026 22:06:27 +0800 Subject: [PATCH 61/90] feat: update heat controller parameters and add shooter heat output --- .../config/deformable-infantry-omni-b.yaml | 9 +- .../config/deformable-infantry-omni.yaml | 4 +- .../config/deformable-infantry-steering.yaml | 4 +- .../controller/shooting/heat_controller.cpp | 91 ++++++++++++++++++- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 3 + 5 files changed, 98 insertions(+), 13 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index ff8dd430..0b0b59e9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -35,8 +35,9 @@ rmcs_executor: value_broadcaster: ros__parameters: forward_list: - - /gimbal/pitch/angle - - /gimbal/pitch/control_torque + - /shoot/heat + - /shoot/referee_heat + - /shoot/heat_limit deformable_infantry: @@ -132,8 +133,8 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10000 - reserved_heat: 15000 + heat_per_shot: 10 + reserved_heat: 15 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index fde52334..a1cabd8a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -131,8 +131,8 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10000 - reserved_heat: 15000 + heat_per_shot: 10 + reserved_heat: 0 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 9b8022bc..d8de651a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -123,8 +123,8 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10000 - reserved_heat: 5000 + heat_per_shot: 10 + reserved_heat: 0 bullet_feeder_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..a6d13b7c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include @@ -13,31 +15,104 @@ class HeatController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , heat_per_shot(get_parameter("heat_per_shot").as_int()) - , reserved_heat(get_parameter("reserved_heat").as_int()) { + , heat_per_shot(normalize_heat_parameter(get_parameter("heat_per_shot").as_int())) + , reserved_heat(normalize_heat_parameter(get_parameter("reserved_heat").as_int())) { register_input("/referee/shooter/cooling", shooter_cooling_); register_input("/referee/shooter/heat_limit", shooter_heat_limit_); + register_input("/referee/shooter/heat", referee_heat_); register_input("/gimbal/bullet_fired", bullet_fired_); register_output( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0); + register_output("/shoot/heat", local_heat_, 0.0); + register_output("/shoot/referee_heat", referee_heat_output_, 0.0); + register_output("/shoot/heat_limit", heat_limit_output_, 0.0); } void update() override { - shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + const auto now = Clock::now(); if (*bullet_fired_) - shooter_heat_ += heat_per_shot + 10; + shooter_heat_ += heat_per_shot; + if (*bullet_fired_) + shot_timestamps_.push_back(now); + + while (!shot_timestamps_.empty() && now - shot_timestamps_.front() > kRefereeDelayWindow) + shot_timestamps_.pop_front(); + + ++cooling_tick_counter_; + const auto cooling_delta_per_settlement = *shooter_cooling_ * kHeatScale / kCoolingRateHz; + if (cooling_tick_counter_ >= kCoolingPeriodTicks) { + cooling_tick_counter_ = 0; + shooter_heat_ = std::max(0, shooter_heat_ - cooling_delta_per_settlement); + } + + if (*referee_heat_ >= 0) { + const auto referee_heat_scaled = *referee_heat_ * kHeatScale; + const auto in_flight_shots = shot_timestamps_.size(); + const auto in_flight_heat = static_cast(in_flight_shots) * heat_per_shot; + + if (referee_heat_scaled > shooter_heat_) { + RCLCPP_WARN( + get_logger(), + "Sync local heat up to referee heat: local=%lld, referee=%lld, referee_scaled=%lld, in_flight_shots=%zu, in_flight_heat=%lld, limit=%lld", + static_cast(shooter_heat_), static_cast(*referee_heat_), + static_cast(referee_heat_scaled), in_flight_shots, + static_cast(in_flight_heat), + static_cast(*shooter_heat_limit_)); + shooter_heat_ = referee_heat_scaled; + } else { + const auto excess_heat = shooter_heat_ - referee_heat_scaled; + if (excess_heat > in_flight_heat) { + RCLCPP_WARN( + get_logger(), + "Sync local heat down to referee heat: local=%lld, referee=%lld, referee_scaled=%lld, excess=%lld, in_flight_shots=%zu, in_flight_heat=%lld, limit=%lld", + static_cast(shooter_heat_), + static_cast(*referee_heat_), + static_cast(referee_heat_scaled), + static_cast(excess_heat), in_flight_shots, + static_cast(in_flight_heat), + static_cast(*shooter_heat_limit_)); + shooter_heat_ = referee_heat_scaled; + } + } + } + + const auto over_limit = shooter_heat_ > *shooter_heat_limit_; + if (over_limit && !last_over_limit_) { + RCLCPP_WARN( + get_logger(), + "Shooter heat exceeded limit: heat=%lld, limit=%lld, referee_heat=%lld, cooling=%lld", + static_cast(shooter_heat_), static_cast(*shooter_heat_limit_), + static_cast(*referee_heat_), static_cast(*shooter_cooling_)); + } + last_over_limit_ = over_limit; *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + *local_heat_ = static_cast(shooter_heat_) / kHeatScale; + *referee_heat_output_ = static_cast(*referee_heat_); + *heat_limit_output_ = static_cast(*shooter_heat_limit_) / kHeatScale; } private: + using Clock = std::chrono::steady_clock; + + static constexpr int64_t kHeatScale = 1000; + static constexpr int64_t kCoolingRateHz = 10; + static constexpr int64_t kUpdateRateHz = 1000; + static constexpr int64_t kCoolingPeriodTicks = kUpdateRateHz / kCoolingRateHz; + static constexpr auto kRefereeDelayWindow = std::chrono::milliseconds(170); + + static constexpr int64_t normalize_heat_parameter(int64_t value) { + return value > 0 && value < kHeatScale ? value * kHeatScale : value; + } + InputInterface shooter_cooling_; InputInterface shooter_heat_limit_; + InputInterface referee_heat_; InputInterface bullet_fired_; @@ -45,7 +120,13 @@ class HeatController const int64_t reserved_heat; int64_t shooter_heat_ = 0; + bool last_over_limit_ = false; + int64_t cooling_tick_counter_ = 0; + std::deque shot_timestamps_; + OutputInterface local_heat_; + OutputInterface referee_heat_output_; + OutputInterface heat_limit_output_; OutputInterface control_bullet_allowance_; }; @@ -53,4 +134,4 @@ class HeatController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index 0ad65b0b..b655bfff 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -47,6 +47,7 @@ class Status register_output("/referee/id", robot_id_, rmcs_msgs::RobotId::UNKNOWN); register_output("/referee/shooter/cooling", robot_shooter_cooling_, 0); register_output("/referee/shooter/heat_limit", robot_shooter_heat_limit_, 0); + register_output("/referee/shooter/heat", robot_shooter_heat_, 0); register_output("/referee/chassis/power_limit", robot_chassis_power_limit_, 0.0); register_output("/referee/chassis/power", robot_chassis_power_, 0.0); register_output("/referee/chassis/buffer_energy", robot_buffer_energy_, 60.0); @@ -251,6 +252,7 @@ class Status auto& data = reinterpret_cast(frame_.body.data); *robot_buffer_energy_ = static_cast(data.buffer_energy); + *robot_shooter_heat_ = static_cast(data.shooter_17mm_barrel_heat); } void update_robot_position() { @@ -372,6 +374,7 @@ class Status OutputInterface map_command_target_position_y_; OutputInterface map_command_keyboard_; OutputInterface map_command_target_robot_id_; + OutputInterface robot_shooter_heat_; OutputInterface map_command_source_; OutputInterface map_command_received_timestamp_; OutputInterface map_command_event_target_position_x_; From d3f741f222687375cd956ae1ca095dc8f3ce32c9 Mon Sep 17 00:00:00 2001 From: eye-on Date: Fri, 12 Jun 2026 22:36:37 +0800 Subject: [PATCH 62/90] feat:fix pitch lock command --- .codex | 0 rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../deformable_infantry_gimbal_controller.cpp | 35 +++++++++++++++++-- 3 files changed, 34 insertions(+), 3 deletions(-) create mode 100644 .codex diff --git a/.codex b/.codex new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index feff0ed0..7fbe1bd4 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit feff0ed06de3cec12df3df600e3df3e2f1066d60 +Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index f8169e63..76a1511e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -138,6 +138,7 @@ class DeformableInfantryGimbalController component.register_input("/tf", tf); component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); + component.register_input("/gimbal/pitch/angle", pitch_angle); component.register_input("/gimbal/pitch/velocity", pitch_velocity); component.register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu); component.register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu); @@ -163,6 +164,7 @@ class DeformableInfantryGimbalController InputInterface tf; InputInterface yaw_angle; InputInterface yaw_velocity; + InputInterface pitch_angle; InputInterface pitch_velocity; InputInterface yaw_velocity_imu; InputInterface pitch_velocity_imu; @@ -250,6 +252,8 @@ class DeformableInfantryGimbalController locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) ? *input_.yaw_angle : 0.0; + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } @@ -269,11 +273,38 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) activate_ctrl_hold(); - reset_control_outputs(); + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); + *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; + *output_.chassis_manual_yaw_velocity_override = kNaN; *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; *output_.yaw_control_angle = locked_yaw_angle_; - *output_.pitch_control_angle = ctrl_hold_pitch_target_angle_; + *output_.pitch_control_angle = kNaN; + + if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { + auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; + if (pitch_target_error > std::numbers::pi) + pitch_target_error -= 2 * std::numbers::pi; + else if (pitch_target_error < -std::numbers::pi) + pitch_target_error += 2 * std::numbers::pi; + + *output_.pitch_angle_error = pitch_target_error; + const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); + if (pitch_torque_control_enabled_) { + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu); + } else { + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = pitch_velocity_ref; + *output_.pitch_control_torque = kNaN; + } + } const auto yaw_velocity_override = std::clamp( manual_yaw_shift() / update_dt(), -ctrl_hold_chassis_yaw_velocity_max_, From 15c61ea031ab92fe88e17d5a4aaca192d133ca15 Mon Sep 17 00:00:00 2001 From: eye-on Date: Sat, 27 Jun 2026 16:17:46 +0800 Subject: [PATCH 63/90] feat:delate .codex --- .codex | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 .codex diff --git a/.codex b/.codex deleted file mode 100644 index e69de29b..00000000 From f502448c1d38c5421e8af6077726973683a25fc2 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 3 Jul 2026 16:17:48 +0800 Subject: [PATCH 64/90] Refactor chassis control code for modularity and maintainability - Consolidated joint input and output registration in deformable_chassis.cpp and deformable_wheel_controller.cpp using loops and arrays to reduce redundancy. - Removed unused PID control structures and related parameters in deformable_chassis.cpp to simplify the class. - Updated input and output handling for joint states to utilize arrays, improving scalability for future joint additions. - Enhanced readability by using fmt library for formatted string outputs in input/output registration. - Cleaned up unnecessary variables and methods related to IMU calibration and attitude correction in deformable_chassis.cpp. --- .../config/deformable-infantry-omni-b.yaml | 61 ++- .../config/deformable-infantry-omni.yaml | 61 ++- .../config/deformable-infantry-steering.yaml | 61 ++- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../chassis/active_suspension_controller.cpp | 239 +++++++++ .../controller/chassis/deformable_chassis.cpp | 494 +++--------------- .../chassis/deformable_wheel_controller.cpp | 249 +++------ 7 files changed, 505 insertions(+), 661 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 0b0b59e9..58fc0510 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -64,27 +65,45 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 - # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 +active_suspension_controller: + ros__parameters: + min_angle: 8.0 + max_angle: 58.0 + active_suspension_enable: true + pitch_angle_diff_limit_deg: 45.0 + roll_angle_diff_limit_deg: 45.0 + + pitch_outer_kp: 8.0 + pitch_outer_ki: 0.35 + pitch_outer_kd: 0.28 + pitch_outer_integral_min: -20.0 + pitch_outer_integral_max: 20.0 + pitch_outer_integral_split_min: -30.0 + pitch_outer_integral_split_max: 30.0 + pitch_outer_output_min: -45.0 + pitch_outer_output_max: 45.0 + + pitch_inner_kp: 2.0 + pitch_inner_ki: 0.0 + pitch_inner_kd: 0.0 + pitch_inner_output_min: -45.0 + pitch_inner_output_max: 45.0 + + roll_outer_kp: 8.0 + roll_outer_ki: 0.35 + roll_outer_kd: 0.28 + roll_outer_integral_min: -20.0 + roll_outer_integral_max: 20.0 + roll_outer_integral_split_min: -30.0 + roll_outer_integral_split_max: 30.0 + roll_outer_output_min: -45.0 + roll_outer_output_max: 45.0 + + roll_inner_kp: 2.0 + roll_inner_ki: 0.0 + roll_inner_kd: 0.0 + roll_inner_output_min: -45.0 + roll_inner_output_max: 45.0 gimbal_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a1cabd8a..93f3465c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -62,27 +63,45 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 - # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 +active_suspension_controller: + ros__parameters: + min_angle: 8.0 + max_angle: 58.0 + active_suspension_enable: true + pitch_angle_diff_limit_deg: 45.0 + roll_angle_diff_limit_deg: 45.0 + + pitch_outer_kp: 8.0 + pitch_outer_ki: 0.35 + pitch_outer_kd: 0.28 + pitch_outer_integral_min: -20.0 + pitch_outer_integral_max: 20.0 + pitch_outer_integral_split_min: -30.0 + pitch_outer_integral_split_max: 30.0 + pitch_outer_output_min: -45.0 + pitch_outer_output_max: 45.0 + + pitch_inner_kp: 2.0 + pitch_inner_ki: 0.0 + pitch_inner_kd: 0.0 + pitch_inner_output_min: -45.0 + pitch_inner_output_max: 45.0 + + roll_outer_kp: 8.0 + roll_outer_ki: 0.35 + roll_outer_kd: 0.28 + roll_outer_integral_min: -20.0 + roll_outer_integral_max: 20.0 + roll_outer_integral_split_min: -30.0 + roll_outer_integral_split_max: 30.0 + roll_outer_output_min: -45.0 + roll_outer_output_max: 45.0 + + roll_inner_kp: 2.0 + roll_inner_ki: 0.0 + roll_inner_kd: 0.0 + roll_inner_output_min: -45.0 + roll_inner_output_max: 45.0 gimbal_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index d8de651a..e6846920 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableChassisController -> deformable_chassis_controller @@ -61,27 +62,45 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 - # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 +active_suspension_controller: + ros__parameters: + min_angle: 20.0 + max_angle: 50.0 + active_suspension_enable: true + pitch_angle_diff_limit_deg: 45.0 + roll_angle_diff_limit_deg: 45.0 + + pitch_outer_kp: 8.0 + pitch_outer_ki: 0.35 + pitch_outer_kd: 0.28 + pitch_outer_integral_min: -20.0 + pitch_outer_integral_max: 20.0 + pitch_outer_integral_split_min: -30.0 + pitch_outer_integral_split_max: 30.0 + pitch_outer_output_min: -45.0 + pitch_outer_output_max: 45.0 + + pitch_inner_kp: 2.0 + pitch_inner_ki: 0.0 + pitch_inner_kd: 0.0 + pitch_inner_output_min: -45.0 + pitch_inner_output_max: 45.0 + + roll_outer_kp: 8.0 + roll_outer_ki: 0.35 + roll_outer_kd: 0.28 + roll_outer_integral_min: -20.0 + roll_outer_integral_max: 20.0 + roll_outer_integral_split_min: -30.0 + roll_outer_integral_split_max: 30.0 + roll_outer_output_min: -45.0 + roll_outer_output_max: 45.0 + + roll_inner_kp: 2.0 + roll_inner_ki: 0.0 + roll_inner_kd: 0.0 + roll_inner_output_min: -45.0 + roll_inner_output_max: 45.0 gimbal_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1438f8a8..59dd7aab 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -13,6 +13,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp new file mode 100644 index 00000000..1dfaca29 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp @@ -0,0 +1,239 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class ActiveSuspensionController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ActiveSuspensionController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + load_pid_("pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28); + load_pid_("pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0); + load_pid_("roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28); + load_pid_("roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0); + + min_angle_deg_ = get_parameter_or("min_angle", 8.0); + max_angle_deg_ = get_parameter_or("max_angle", 58.0); + active_suspension_enable_ = get_parameter_or("active_suspension_enable", true); + pitch_angle_diff_limit_deg_ = get_parameter_or("pitch_angle_diff_limit_deg", 45.0); + roll_angle_diff_limit_deg_ = get_parameter_or("roll_angle_diff_limit_deg", 45.0); + + register_input("/chassis/ctrl_hold_active", ctrl_hold_active_); + register_input("/chassis/imu/pitch", chassis_imu_pitch_); + register_input("/chassis/imu/roll", chassis_imu_roll_); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_); + + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + fmt::format("/chassis/{}_joint/base_target_physical_angle", kJointNames[i]), + base_angles_[i]); + register_input( + fmt::format("/chassis/{}_joint/base_target_physical_velocity", kJointNames[i]), + base_velocities_[i], false); + register_input( + fmt::format("/chassis/{}_joint/base_target_physical_acceleration", kJointNames[i]), + base_accelerations_[i], false); + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), physical_angles_[i]); + + register_output( + fmt::format("/chassis/{}_joint/target_physical_angle", kJointNames[i]), + target_angles_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/target_physical_velocity", kJointNames[i]), + target_velocities_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointNames[i]), + target_accelerations_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/control_angle_error", kJointNames[i]), + angle_errors_[i], nan_); + } + } + + void update() override { + if (!base_targets_ready_()) { + reset_outputs_nan_(); + return; + } + + if (!active_suspension_enable_ || !ctrl_hold_active_ready_() || !feedback_ready_()) { + passthrough_outputs_(); + return; + } + + const double pitch = std::clamp(*chassis_imu_pitch_, -kMaxAttitude, kMaxAttitude); + const double roll = std::clamp(*chassis_imu_roll_, -kMaxAttitude, kMaxAttitude); + const double pitch_rate = *chassis_imu_pitch_rate_; + const double roll_rate = *chassis_imu_roll_rate_; + + const double pitch_outer = pitch_outer_pid_.update(-pitch); + const double roll_outer = roll_outer_pid_.update(-roll); + + const double pitch_inner = pitch_inner_pid_.update(pitch_outer - pitch_rate); + const double roll_inner = roll_inner_pid_.update(roll_outer - roll_rate); + + if (!std::isfinite(pitch_inner) || !std::isfinite(roll_inner)) { + reset_outputs_nan_(); + return; + } + + const double base_angle = deg_to_rad_(min_angle_deg_ - 5.0); + const double max_angle = deg_to_rad_(max_angle_deg_); + const double pitch_limit = deg_to_rad_(pitch_angle_diff_limit_deg_); + const double roll_limit = deg_to_rad_(roll_angle_diff_limit_deg_); + + const double pitch_delta = std::clamp(pitch_inner, -pitch_limit, pitch_limit); + const double roll_delta = std::clamp(roll_inner, -roll_limit, roll_limit); + + static constexpr std::array kPitchSign = {-1, 1, 1, -1}; + static constexpr std::array kRollSign = {1, 1, -1, -1}; + + for (size_t i = 0; i < kJointCount; ++i) + *target_angles_[i] = + std::clamp( + base_angle + std::max(kPitchSign[i] * pitch_delta, 0.0) + + std::max(kRollSign[i] * roll_delta, 0.0), + base_angle, max_angle); + + for (size_t i = 0; i < kJointCount; ++i) { + *target_velocities_[i] = optional_input_or_(base_velocities_[i], 0.0); + *target_accelerations_[i] = optional_input_or_(base_accelerations_[i], 0.0); + } + publish_angle_errors_(); + } + +private: + static constexpr size_t kJointCount = 4; + static constexpr std::array kJointNames = { + "left_front", "left_back", "right_back", "right_front"}; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kMaxAttitude = 30.0 * std::numbers::pi / 180.0; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + static double optional_input_or_(const InputInterface& input, double fallback) { + return input.ready() && std::isfinite(*input) ? *input : fallback; + } + + static double load_double_or_(rclcpp::Node& node, const std::string& name, double fallback) { + double value = fallback; + node.get_parameter_or(name, value, fallback); + return value; + } + + void load_pid_( + const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, + double kd_default) { + pid.kp = load_double_or_(*this, prefix + "kp", kp_default); + pid.ki = load_double_or_(*this, prefix + "ki", ki_default); + pid.kd = load_double_or_(*this, prefix + "kd", kd_default); + pid.integral_min = load_double_or_(*this, prefix + "integral_min", -1e9); + pid.integral_max = load_double_or_(*this, prefix + "integral_max", 1e9); + pid.integral_split_min = load_double_or_(*this, prefix + "integral_split_min", -1e9); + pid.integral_split_max = load_double_or_(*this, prefix + "integral_split_max", 1e9); + pid.output_min = load_double_or_(*this, prefix + "output_min", -1e9); + pid.output_max = load_double_or_(*this, prefix + "output_max", 1e9); + } + + bool ctrl_hold_active_ready_() const { + return ctrl_hold_active_.ready() && *ctrl_hold_active_; + } + + bool base_targets_ready_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (!base_angles_[i].ready()) + return false; + return true; + } + + bool feedback_ready_() const { + if (!chassis_imu_pitch_.ready() || !chassis_imu_roll_.ready() + || !chassis_imu_pitch_rate_.ready() || !chassis_imu_roll_rate_.ready()) + return false; + for (size_t i = 0; i < kJointCount; ++i) + if (!physical_angles_[i].ready()) + return false; + return true; + } + + void passthrough_outputs_() { + for (size_t i = 0; i < kJointCount; ++i) { + *target_angles_[i] = *base_angles_[i]; + *target_velocities_[i] = optional_input_or_(base_velocities_[i], 0.0); + *target_accelerations_[i] = optional_input_or_(base_accelerations_[i], 0.0); + } + publish_angle_errors_(); + } + + void reset_outputs_nan_() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + + for (size_t i = 0; i < kJointCount; ++i) { + *target_angles_[i] = nan_; + *target_velocities_[i] = nan_; + *target_accelerations_[i] = nan_; + *angle_errors_[i] = nan_; + } + } + + void publish_angle_errors_() { + for (size_t i = 0; i < kJointCount; ++i) + *angle_errors_[i] = physical_angles_[i].ready() && std::isfinite(*physical_angles_[i]) + ? *physical_angles_[i] - *target_angles_[i] + : nan_; + } + + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; + + InputInterface ctrl_hold_active_; + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + std::array, kJointCount> base_angles_; + std::array, kJointCount> base_velocities_; + std::array, kJointCount> base_accelerations_; + std::array, kJointCount> physical_angles_; + + std::array, kJointCount> target_angles_; + std::array, kJointCount> target_velocities_; + std::array, kJointCount> target_accelerations_; + std::array, kJointCount> angle_errors_; + + double min_angle_deg_ = 8.0; + double max_angle_deg_ = 58.0; + bool active_suspension_enable_ = true; + double pitch_angle_diff_limit_deg_ = 45.0; + double roll_angle_diff_limit_deg_ = 45.0; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ActiveSuspensionController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 9d17ebdf..27d76767 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -33,28 +34,6 @@ class DeformableChassis kJointCount = 4, }; - struct AttitudePidAxis { - double kp = 20.0; - double ki = 0.0; - double kd = 0.0; - double integral = 0.0; - double integral_limit = std::numeric_limits::infinity(); - double output_limit = std::numeric_limits::infinity(); - - void reset() { integral = 0.0; } - - double update(double error, double rate, double dt) { - if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { - reset(); - return std::numeric_limits::quiet_NaN(); - } - - integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); - const double output = kp * error + ki * integral - kd * rate; - return std::clamp(output, -output_limit, output_limit); - } - }; - DeformableChassis() : Node( get_component_name(), @@ -74,56 +53,10 @@ class DeformableChassis deg_to_rad( std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6)) - , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) - , pitch_kp_(get_parameter_or("active_suspension_pitch_kp", 200.0)) - , pitch_ki_(get_parameter_or("active_suspension_pitch_ki", 0.0)) - , pitch_kd_(get_parameter_or("active_suspension_pitch_kd", 20.0)) - , roll_kp_(get_parameter_or("active_suspension_roll_kp", 200.0)) - , roll_ki_(get_parameter_or("active_suspension_roll_ki", 0.0)) - , roll_kd_(get_parameter_or("active_suspension_roll_kd", 20.0)) - , suspension_velocity_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_velocity_limit_deg", - get_parameter_or("target_physical_velocity_limit", 180.0)))), - 1e-6)) - , suspension_acceleration_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - get_parameter_or("target_physical_acceleration_limit", 720.0)))), - 1e-6)) - , pitch_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_pitch_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , roll_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_roll_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , pid_integral_limit_( - std::abs(get_parameter_or( - "active_suspension_pid_integral_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , chassis_imu_calibration_wait_time_( - std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) - , chassis_imu_calibration_sample_time_( - std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6)) { + , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) { following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; - pitch_pid_.kp = pitch_kp_; - pitch_pid_.ki = pitch_ki_; - pitch_pid_.kd = pitch_kd_; - pitch_pid_.integral_limit = pid_integral_limit_; - pitch_pid_.output_limit = pitch_diff_limit_; - roll_pid_.kp = roll_kp_; - roll_pid_.ki = roll_ki_; - roll_pid_.kd = roll_kd_; - roll_pid_.integral_limit = pid_integral_limit_; - roll_pid_.output_limit = roll_diff_limit_; register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); @@ -136,19 +69,10 @@ class DeformableChassis register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); register_input("/chassis/manual_yaw_velocity_override", manual_yaw_velocity_override_, false); - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); - register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); - register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); - register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - + for (size_t i = 0; i < kJointCount; ++i) + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), + joint_physical_angle_[i], false); register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); register_output("/chassis/angle", chassis_angle_, nan_); @@ -158,47 +82,20 @@ class DeformableChassis register_output("/chassis/control_velocity", chassis_control_velocity_); register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); - register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); - register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); - register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); - register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); - - register_output( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, nan_); + for (size_t i = 0; i < kJointCount; ++i) { + register_output( + fmt::format("/chassis/{}_joint/base_control_angle_error", kJointNames[i]), + joint_angle_error_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/base_target_physical_angle", kJointNames[i]), + joint_target_physical_angle_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/base_target_physical_velocity", kJointNames[i]), + joint_target_physical_velocity_[i], nan_); + register_output( + fmt::format("/chassis/{}_joint/base_target_physical_acceleration", kJointNames[i]), + joint_target_physical_acceleration_[i], nan_); + } *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; @@ -223,14 +120,6 @@ class DeformableChassis } if (!manual_yaw_velocity_override_.ready()) manual_yaw_velocity_override_.make_and_bind_directly(nan_); - if (!chassis_imu_pitch_.ready()) - chassis_imu_pitch_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_.ready()) - chassis_imu_roll_.make_and_bind_directly(0.0); - if (!chassis_imu_pitch_rate_.ready()) - chassis_imu_pitch_rate_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_rate_.ready()) - chassis_imu_roll_rate_.make_and_bind_directly(0.0); validate_joint_feedback_inputs(); } @@ -265,17 +154,12 @@ class DeformableChassis static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; - static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; - static constexpr double imu_calibration_offset_limit_rad_ = - 1.0 * std::numbers::pi / 180.0; - void validate_joint_feedback_inputs() const { - if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() - && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) - return; - - throw std::runtime_error( - "missing deformable chassis feedback interfaces: expected /chassis/*_joint/physical_angle"); + for (size_t i = 0; i < kJointCount; ++i) + if (!joint_physical_angle_[i].ready()) + throw std::runtime_error( + "missing deformable chassis feedback interfaces: expected " + "/chassis/*_joint/physical_angle"); } void update_mode_from_inputs_( @@ -433,18 +317,12 @@ class DeformableChassis } std::array read_current_joint_physical_angles_() const { - const std::array*, kJointCount> physical_angle_inputs{ - &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, - &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; - std::array current_physical_angles{}; current_physical_angles.fill(nan_); for (size_t i = 0; i < kJointCount; ++i) { - if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { - current_physical_angles[i] = *(*physical_angle_inputs[i]); - } + if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) + current_physical_angles[i] = *joint_physical_angle_[i]; } - return current_physical_angles; } @@ -471,80 +349,6 @@ class DeformableChassis return active_suspension_enable_ && ctrl_hold_requested_by_input_(); } - bool symmetric_joint_target_requested_() const { - constexpr double epsilon = 1e-6; - return std::abs(lf_current_target_angle_ - lb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; - } - - void reset_attitude_correction_state_() { - pitch_pid_.reset(); - roll_pid_.reset(); - joint_suspension_active_.fill(false); - } - - void reset_chassis_imu_calibration_window_() { - chassis_imu_calibration_hold_elapsed_ = 0.0; - chassis_imu_calibration_sample_count_ = 0; - chassis_imu_pitch_sum_ = 0.0; - chassis_imu_roll_sum_ = 0.0; - chassis_imu_calibration_completed_for_window_ = false; - } - - void update_chassis_imu_calibration_() { - if (chassis_imu_calibrated_once_) - return; - - if (!symmetric_joint_target_requested_()) { - reset_chassis_imu_calibration_window_(); - return; - } - - const double raw_pitch = *chassis_imu_pitch_; - const double raw_roll = *chassis_imu_roll_; - if (!std::isfinite(raw_pitch) || !std::isfinite(raw_roll)) - return; - - chassis_imu_calibration_hold_elapsed_ += update_dt(); - if (chassis_imu_calibration_hold_elapsed_ < chassis_imu_calibration_wait_time_) - return; - - const double calibration_end_time = - chassis_imu_calibration_wait_time_ + chassis_imu_calibration_sample_time_; - if (chassis_imu_calibration_hold_elapsed_ < calibration_end_time) { - chassis_imu_pitch_sum_ += raw_pitch; - chassis_imu_roll_sum_ += raw_roll; - ++chassis_imu_calibration_sample_count_; - return; - } - - if (chassis_imu_calibration_completed_for_window_) - return; - - chassis_imu_calibration_completed_for_window_ = true; - if (chassis_imu_calibration_sample_count_ == 0) { - RCLCPP_WARN( - get_logger(), - "[chassis imu calibration] skipped because no valid samples were collected"); - return; - } - - chassis_imu_pitch_offset_ = std::clamp( - chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_roll_offset_ = std::clamp( - chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_calibrated_once_ = true; - RCLCPP_INFO( - get_logger(), - "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " - "(samples=%zu)", - chassis_imu_pitch_offset_ * rad_to_deg_, chassis_imu_roll_offset_ * rad_to_deg_, - chassis_imu_calibration_sample_count_); - } - bool ensure_joint_target_states_from_feedback( const std::array& current_physical_angles) { bool any_active = false; @@ -570,70 +374,9 @@ class DeformableChassis }); } - double active_suspension_min_angle_rad_() const { - return deg_to_rad(min_angle_ - 5.0); - } - - void update_active_suspension_() { - if (!suspension_requested_by_input_()) { - reset_attitude_correction_state_(); - return; - } - - constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - const double base_target_angle = active_suspension_min_angle_rad_(); - const double max_target_angle = deg_to_rad(max_angle_); - const double corrected_pitch = - std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); - const double corrected_roll = - std::clamp(*chassis_imu_roll_ - chassis_imu_roll_offset_, -max_attitude, max_attitude); - const double corrected_pitch_rate = *chassis_imu_pitch_rate_; - const double corrected_roll_rate = *chassis_imu_roll_rate_; - - const double dt = update_dt(); - const double pitch_angle_diff = - pitch_pid_.update(-corrected_pitch, corrected_pitch_rate, dt); - const double roll_angle_diff = - roll_pid_.update(corrected_roll, -corrected_roll_rate, dt); - if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { - reset_attitude_correction_state_(); - current_target_physical_angles_rad_.fill(base_target_angle); - return; - } - - // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left - // pair. Every leg starts from the active-suspension minimum and only receives additive - // corrections so at least one leg always stays at that minimum. - const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); - const double back_pitch_add = std::max(pitch_angle_diff, 0.0); - const double left_roll_add = std::max(roll_angle_diff, 0.0); - const double right_roll_add = std::max(-roll_angle_diff, 0.0); - - current_target_physical_angles_rad_[kLeftFront] = - std::clamp( - base_target_angle + front_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kLeftBack] = - std::clamp( - base_target_angle + back_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightBack] = - std::clamp( - base_target_angle + back_pitch_add + right_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightFront] = - std::clamp( - base_target_angle + front_pitch_add + right_roll_add, base_target_angle, - max_target_angle); - - joint_suspension_active_.fill(true); - } - void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; - reset_attitude_correction_state_(); - reset_chassis_imu_calibration_window_(); chassis_control_velocity_->vector << nan_, nan_, nan_; *chassis_angle_ = nan_; @@ -655,23 +398,12 @@ class DeformableChassis *scope_motor_control_torque = nan_; - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rf_angle_error_ = nan_; - *rb_angle_error_ = nan_; - - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_physical_angle_[i] = nan_; + *joint_target_physical_velocity_[i] = nan_; + *joint_target_physical_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } } @@ -835,13 +567,8 @@ class DeformableChassis current_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - if (suspension_requested) { - current_target_physical_angles_rad_.fill(active_suspension_min_angle_rad_()); - } - update_chassis_imu_calibration_(); scope_motor_control(suspension_requested); - update_active_suspension_(); update_joint_target_trajectory(); publish_joint_target_angles(current_physical_angles); @@ -868,12 +595,6 @@ class DeformableChassis double& velocity_state = joint_target_physical_velocity_state_rad_[i]; double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; const double target_angle = current_target_physical_angles_rad_[i]; - const double velocity_limit = joint_suspension_active_[i] - ? suspension_velocity_limit_ - : target_physical_velocity_limit_; - const double acceleration_limit = - joint_suspension_active_[i] ? suspension_acceleration_limit_ - : target_physical_acceleration_limit_; if (!std::isfinite(target_angle) || !std::isfinite(angle_state)) { continue; @@ -881,19 +602,25 @@ class DeformableChassis const double position_error = target_angle - angle_state; const double stopping_distance = - velocity_state * velocity_state / (2.0 * acceleration_limit); + velocity_state * velocity_state / (2.0 * target_physical_acceleration_limit_); double desired_velocity = 0.0; if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(velocity_limit, position_error); + desired_velocity = + std::copysign(target_physical_velocity_limit_, position_error); } const double velocity_error = desired_velocity - velocity_state; acceleration_state = - std::clamp(velocity_error / dt, -acceleration_limit, acceleration_limit); + std::clamp( + velocity_error / dt, -target_physical_acceleration_limit_, + target_physical_acceleration_limit_); velocity_state += acceleration_state * dt; - velocity_state = std::clamp(velocity_state, -velocity_limit, velocity_limit); + velocity_state = + std::clamp( + velocity_state, -target_physical_velocity_limit_, + target_physical_velocity_limit_); angle_state += velocity_state * dt; const double next_error = target_angle - angle_state; @@ -914,70 +641,34 @@ class DeformableChassis return; } - const auto publish_joint = [this, ¤t_physical_angles]( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output, - OutputInterface& acceleration_output, - OutputInterface& angle_error_output) { - if (!joint_target_active_[index]) { - *angle_output = nan_; - *velocity_output = nan_; - *acceleration_output = nan_; - *angle_error_output = nan_; - return; + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) { + *joint_target_physical_angle_[i] = nan_; + *joint_target_physical_velocity_[i] = nan_; + *joint_target_physical_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + continue; } - *angle_output = joint_target_physical_angle_state_rad_[index]; - *velocity_output = joint_target_physical_velocity_state_rad_[index]; - *acceleration_output = joint_target_physical_acceleration_state_rad_[index]; - *angle_error_output = std::isfinite(current_physical_angles[index]) - ? current_physical_angles[index] - - joint_target_physical_angle_state_rad_[index] - : nan_; - }; - - publish_joint( - kLeftFront, left_front_joint_target_physical_angle_, - left_front_joint_target_physical_velocity_, - left_front_joint_target_physical_acceleration_, lf_angle_error_); - publish_joint( - kLeftBack, left_back_joint_target_physical_angle_, - left_back_joint_target_physical_velocity_, - left_back_joint_target_physical_acceleration_, lb_angle_error_); - publish_joint( - kRightBack, right_back_joint_target_physical_angle_, - right_back_joint_target_physical_velocity_, - right_back_joint_target_physical_acceleration_, rb_angle_error_); - publish_joint( - kRightFront, right_front_joint_target_physical_angle_, - right_front_joint_target_physical_velocity_, - right_front_joint_target_physical_acceleration_, rf_angle_error_); + *joint_target_physical_angle_[i] = joint_target_physical_angle_state_rad_[i]; + *joint_target_physical_velocity_[i] = joint_target_physical_velocity_state_rad_[i]; + *joint_target_physical_acceleration_[i] = joint_target_physical_acceleration_state_rad_[i]; + *joint_angle_error_[i] = std::isfinite(current_physical_angles[i]) + ? current_physical_angles[i] + - joint_target_physical_angle_state_rad_[i] + : nan_; + } } void publish_nan_joint_targets() { - reset_attitude_correction_state_(); joint_target_active_.fill(false); - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; - - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rb_angle_error_ = nan_; - *rf_angle_error_ = nan_; - + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_physical_angle_[i] = nan_; + *joint_target_physical_velocity_[i] = nan_; + *joint_target_physical_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } } private: @@ -1012,34 +703,17 @@ class DeformableChassis const double spin_ratio_; const bool launch_ramp_shortcut_enabled_; - InputInterface left_front_joint_physical_angle_; - InputInterface left_back_joint_physical_angle_; - InputInterface right_front_joint_physical_angle_; - InputInterface right_back_joint_physical_angle_; - InputInterface chassis_imu_pitch_; - InputInterface chassis_imu_roll_; - InputInterface chassis_imu_pitch_rate_; - InputInterface chassis_imu_roll_rate_; + static constexpr std::array kJointNames = { + "left_front", "left_back", "right_back", "right_front"}; + std::array, kJointCount> joint_physical_angle_; OutputInterface scope_motor_control_torque; - OutputInterface lf_angle_error_; - OutputInterface lb_angle_error_; - OutputInterface rf_angle_error_; - OutputInterface rb_angle_error_; - - OutputInterface left_front_joint_target_physical_angle_; - OutputInterface left_back_joint_target_physical_angle_; - OutputInterface right_back_joint_target_physical_angle_; - OutputInterface right_front_joint_target_physical_angle_; - OutputInterface left_front_joint_target_physical_velocity_; - OutputInterface left_back_joint_target_physical_velocity_; - OutputInterface right_back_joint_target_physical_velocity_; - OutputInterface right_front_joint_target_physical_velocity_; - OutputInterface left_front_joint_target_physical_acceleration_; - OutputInterface left_back_joint_target_physical_acceleration_; - OutputInterface right_back_joint_target_physical_acceleration_; - OutputInterface right_front_joint_target_physical_acceleration_; + std::array, kJointCount> joint_angle_error_; + + std::array, kJointCount> joint_target_physical_angle_; + std::array, kJointCount> joint_target_physical_velocity_; + std::array, kJointCount> joint_target_physical_acceleration_; double min_angle_; double max_angle_; @@ -1061,30 +735,6 @@ class DeformableChassis double target_physical_acceleration_limit_; bool active_suspension_enable_; bool suspension_on_by_switch = false; - double pitch_kp_; - double pitch_ki_; - double pitch_kd_; - double roll_kp_; - double roll_ki_; - double roll_kd_; - double suspension_velocity_limit_; - double suspension_acceleration_limit_; - double pitch_diff_limit_; - double roll_diff_limit_; - double pid_integral_limit_; - std::array joint_suspension_active_ = {false, false, false, false}; - AttitudePidAxis pitch_pid_; - AttitudePidAxis roll_pid_; - double chassis_imu_pitch_offset_ = 0.0; - double chassis_imu_roll_offset_ = 0.0; - double chassis_imu_calibration_wait_time_; - double chassis_imu_calibration_sample_time_; - double chassis_imu_calibration_hold_elapsed_ = 0.0; - size_t chassis_imu_calibration_sample_count_ = 0; - double chassis_imu_pitch_sum_ = 0.0; - double chassis_imu_roll_sum_ = 0.0; - bool chassis_imu_calibration_completed_for_window_ = false; - bool chassis_imu_calibrated_once_ = false; static constexpr double default_dt_ = 1e-3; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp index ba3b68c0..1c02993a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -71,88 +72,45 @@ class DeformableChassisController register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); - register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); - register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); - register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); - register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); - - register_input("/chassis/left_front_steering/velocity", left_front_steering_velocity_); - register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); - register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); - register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); - - register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); - - register_input("/chassis/left_front_joint/physical_angle", left_front_joint_angle_); - register_input("/chassis/left_back_joint/physical_angle", left_back_joint_angle_); - register_input("/chassis/right_back_joint/physical_angle", right_back_joint_angle_); - register_input("/chassis/right_front_joint/physical_angle", right_front_joint_angle_); - - register_input("/chassis/left_front_joint/physical_velocity", left_front_joint_velocity_); - register_input("/chassis/left_back_joint/physical_velocity", left_back_joint_velocity_); - register_input("/chassis/right_back_joint/physical_velocity", right_back_joint_velocity_); - register_input("/chassis/right_front_joint/physical_velocity", right_front_joint_velocity_); - - register_input( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, false); - register_input( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, false); - register_input( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, false); - register_input( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, false); - register_input( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, false); - register_input( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, false); - register_input( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, false); + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format("/chassis/{}_steering/angle", kJointNames[i]), + steering_angle_[i]); + register_input( + fmt::format("/chassis/{}_steering/velocity", kJointNames[i]), + steering_velocity_[i]); + register_input( + fmt::format("/chassis/{}_wheel/velocity", kJointNames[i]), + wheel_velocity_[i]); + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), + joint_angle_[i]); + register_input( + fmt::format("/chassis/{}_joint/physical_velocity", kJointNames[i]), + joint_velocity_[i]); + register_input( + fmt::format("/chassis/{}_joint/target_physical_angle", kJointNames[i]), + joint_target_physical_angle_[i], false); + register_input( + fmt::format("/chassis/{}_joint/target_physical_velocity", kJointNames[i]), + joint_target_physical_velocity_[i], false); + register_input( + fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointNames[i]), + joint_target_physical_acceleration_[i], false); + } register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); - register_output( - "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); - register_output( - "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); - register_output( - "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); - register_output( - "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); - register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); + for (size_t i = 0; i < kWheelCount; ++i) { + register_output( + fmt::format("/chassis/{}_steering/control_torque", kJointNames[i]), + steering_control_torque_[i]); + register_output( + fmt::format("/chassis/{}_wheel/control_torque", kJointNames[i]), + wheel_control_torque_[i]); + } } @@ -270,19 +228,17 @@ class DeformableChassisController } [[nodiscard]] static Eigen::Vector4d read_required_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { - return {*left_front, *left_back, *right_back, *right_front}; + const std::array, kWheelCount>& inputs) { + return {*inputs[0], *inputs[1], *inputs[2], *inputs[3]}; } [[nodiscard]] static Eigen::Vector4d read_optional_inputs_( - const InputInterface& left_front, const InputInterface& left_back, - const InputInterface& right_back, const InputInterface& right_front) { + const std::array, kWheelCount>& inputs) { return { - left_front.ready() ? *left_front : nan_, - left_back.ready() ? *left_back : nan_, - right_back.ready() ? *right_back : nan_, - right_front.ready() ? *right_front : nan_, + inputs[0].ready() ? *inputs[0] : nan_, + inputs[1].ready() ? *inputs[1] : nan_, + inputs[2].ready() ? *inputs[2] : nan_, + inputs[3].ready() ? *inputs[3] : nan_, }; } @@ -298,12 +254,8 @@ class DeformableChassisController [[nodiscard]] JointFeedbackStates update_joint_feedback_states_() { JointFeedbackStates joint; - joint.alpha_rad = read_required_inputs_( - left_front_joint_angle_, left_back_joint_angle_, right_back_joint_angle_, - right_front_joint_angle_); - joint.alpha_dot_rad = read_required_inputs_( - left_front_joint_velocity_, left_back_joint_velocity_, right_back_joint_velocity_, - right_front_joint_velocity_); + joint.alpha_rad = read_required_inputs_(joint_angle_); + joint.alpha_dot_rad = read_required_inputs_(joint_velocity_); if (!joint.alpha_rad.array().isFinite().all() || !joint.alpha_dot_rad.array().isFinite().all()) @@ -329,17 +281,12 @@ class DeformableChassisController [[nodiscard]] JointTargetStates update_joint_target_states_() { JointTargetStates joint; - joint.alpha_rad = read_optional_inputs_( - left_front_joint_target_physical_angle_, left_back_joint_target_physical_angle_, - right_back_joint_target_physical_angle_, right_front_joint_target_physical_angle_); + joint.alpha_rad = read_optional_inputs_(joint_target_physical_angle_); if (!joint.alpha_rad.array().isFinite().all()) return joint; - const Eigen::Vector4d target_velocity = read_optional_inputs_( - left_front_joint_target_physical_velocity_, left_back_joint_target_physical_velocity_, - right_back_joint_target_physical_velocity_, - right_front_joint_target_physical_velocity_); + const Eigen::Vector4d target_velocity = read_optional_inputs_(joint_target_physical_velocity_); if (target_velocity.array().isFinite().all()) { joint.alpha_dot_rad = target_velocity; joint.has_velocity = true; @@ -349,10 +296,7 @@ class DeformableChassisController } const Eigen::Vector4d target_acceleration = read_optional_inputs_( - left_front_joint_target_physical_acceleration_, - left_back_joint_target_physical_acceleration_, - right_back_joint_target_physical_acceleration_, - right_front_joint_target_physical_acceleration_); + joint_target_physical_acceleration_); if (target_acceleration.array().isFinite().all()) { joint.alpha_ddot_rad = target_acceleration; joint.has_acceleration = true; @@ -382,15 +326,10 @@ class DeformableChassisController last_joint_target_angle_ = Eigen::Vector4d::Zero(); last_joint_target_angle_valid_ = false; - *left_front_steering_control_torque_ = 0.0; - *left_back_steering_control_torque_ = 0.0; - *right_back_steering_control_torque_ = 0.0; - *right_front_steering_control_torque_ = 0.0; - - *left_front_wheel_control_torque_ = 0.0; - *left_back_wheel_control_torque_ = 0.0; - *right_back_wheel_control_torque_ = 0.0; - *right_front_wheel_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) { + *steering_control_torque_[i] = 0.0; + *wheel_control_torque_[i] = 0.0; + } } void integral_yaw_angle_imu() { @@ -400,9 +339,7 @@ class DeformableChassisController [[nodiscard]] SteeringStatus calculate_steering_status() const { SteeringStatus steering_status; - steering_status.angle = read_required_inputs_( - left_front_steering_angle_, left_back_steering_angle_, right_back_steering_angle_, - right_front_steering_angle_); + steering_status.angle = read_required_inputs_(steering_angle_); steering_status.angle.array() -= std::numbers::pi / 4; steering_status.cos_angle = steering_status.angle.array().cos(); steering_status.sin_angle = steering_status.angle.array().sin(); @@ -413,16 +350,12 @@ class DeformableChassisController steering_status.cos_angle_minus_phi[i] = std::cos(angle_minus_phi); } - steering_status.velocity = read_required_inputs_( - left_front_steering_velocity_, left_back_steering_velocity_, - right_back_steering_velocity_, right_front_steering_velocity_); + steering_status.velocity = read_required_inputs_(steering_velocity_); return steering_status; } [[nodiscard]] Eigen::Vector4d calculate_wheel_velocities() const { - return read_required_inputs_( - left_front_wheel_velocity_, left_back_wheel_velocity_, right_back_wheel_velocity_, - right_front_wheel_velocity_); + return read_required_inputs_(wheel_velocity_); } /** @@ -696,15 +629,10 @@ class DeformableChassisController void update_control_torques( const Eigen::Vector4d& steering_torque, const Eigen::Vector4d& wheel_torque) { - *left_front_steering_control_torque_ = steering_torque[0]; - *left_back_steering_control_torque_ = steering_torque[1]; - *right_back_steering_control_torque_ = steering_torque[2]; - *right_front_steering_control_torque_ = steering_torque[3]; - - *left_front_wheel_control_torque_ = wheel_torque[0]; - *left_back_wheel_control_torque_ = wheel_torque[1]; - *right_back_wheel_control_torque_ = wheel_torque[2]; - *right_front_wheel_control_torque_ = wheel_torque[3]; + for (size_t i = 0; i < kWheelCount; ++i) { + *steering_control_torque_[i] = steering_torque[i]; + *wheel_control_torque_[i] = wheel_torque[i]; + } } void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { @@ -768,6 +696,9 @@ class DeformableChassisController -1.0, }; + static constexpr std::array kJointNames = { + "left_front", "left_back", "right_back", "right_front"}; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double dt_ = 1e-3; static constexpr double g_ = 9.81; @@ -800,57 +731,23 @@ class DeformableChassisController InputInterface joystick_right_; InputInterface joystick_left_; - InputInterface left_front_steering_angle_; - InputInterface left_back_steering_angle_; - InputInterface right_back_steering_angle_; - InputInterface right_front_steering_angle_; - - InputInterface left_front_steering_velocity_; - InputInterface left_back_steering_velocity_; - InputInterface right_back_steering_velocity_; - InputInterface right_front_steering_velocity_; - - InputInterface left_front_wheel_velocity_; - InputInterface left_back_wheel_velocity_; - InputInterface right_back_wheel_velocity_; - InputInterface right_front_wheel_velocity_; - - InputInterface left_front_joint_angle_; - InputInterface left_back_joint_angle_; - InputInterface right_back_joint_angle_; - InputInterface right_front_joint_angle_; - - InputInterface left_front_joint_velocity_; - InputInterface left_back_joint_velocity_; - InputInterface right_back_joint_velocity_; - InputInterface right_front_joint_velocity_; - - InputInterface left_front_joint_target_physical_angle_; - InputInterface left_back_joint_target_physical_angle_; - InputInterface right_back_joint_target_physical_angle_; - InputInterface right_front_joint_target_physical_angle_; - InputInterface left_front_joint_target_physical_velocity_; - InputInterface left_back_joint_target_physical_velocity_; - InputInterface right_back_joint_target_physical_velocity_; - InputInterface right_front_joint_target_physical_velocity_; - InputInterface left_front_joint_target_physical_acceleration_; - InputInterface left_back_joint_target_physical_acceleration_; - InputInterface right_back_joint_target_physical_acceleration_; - InputInterface right_front_joint_target_physical_acceleration_; + std::array, kWheelCount> steering_angle_; + std::array, kWheelCount> steering_velocity_; + std::array, kWheelCount> wheel_velocity_; + + std::array, kWheelCount> joint_angle_; + std::array, kWheelCount> joint_velocity_; + + std::array, kWheelCount> joint_target_physical_angle_; + std::array, kWheelCount> joint_target_physical_velocity_; + std::array, kWheelCount> joint_target_physical_acceleration_; InputInterface chassis_yaw_velocity_imu_; InputInterface chassis_control_velocity_; InputInterface power_limit_; - OutputInterface left_front_steering_control_torque_; - OutputInterface left_back_steering_control_torque_; - OutputInterface right_back_steering_control_torque_; - OutputInterface right_front_steering_control_torque_; - - OutputInterface left_front_wheel_control_torque_; - OutputInterface left_back_wheel_control_torque_; - OutputInterface right_back_wheel_control_torque_; - OutputInterface right_front_wheel_control_torque_; + std::array, kWheelCount> steering_control_torque_; + std::array, kWheelCount> wheel_control_torque_; QcpSolver qcp_solver_; filter::LowPassFilter<3> control_acceleration_filter_; From afed676bc0497d911fe5af740f06cbb3935af8fd Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 3 Jul 2026 18:55:39 +0800 Subject: [PATCH 65/90] Refactor Deformable Infantry Hardware and Remove IMU Board - Updated the configuration for the deformable infantry steering to correct serial filter values. - Refactored the chassis control code to remove unnecessary scope motor control logic and related variables. - Simplified the hardware implementation by removing the IMU board and adjusting the top and bottom board initializations accordingly. - Enhanced the handling of gimbal pitch and yaw velocity calculations using separate BMI088 instances for better clarity and maintainability. - Updated CAN communication logic to accommodate the removal of the scope motor and streamline data transmission. --- .../config/deformable-infantry-omni-b.yaml | 3 +- .../config/deformable-infantry-omni.yaml | 3 +- .../config/deformable-infantry-steering.yaml | 3 +- .../controller/chassis/deformable_chassis.cpp | 18 -- .../hardware/deformable-infantry-omni-b.cpp | 185 ++++++---------- .../src/hardware/deformable-infantry-omni.cpp | 160 +++----------- .../hardware/deformable-infantry-steering.cpp | 199 ++++++------------ 7 files changed, 169 insertions(+), 402 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 58fc0510..6e2dd6d6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -44,8 +44,7 @@ value_broadcaster: deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 93f3465c..29a129bd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -42,8 +42,7 @@ value_broadcaster: deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-8AE3-4EC1-03C3-C494-88FE-2DC4-3018-0298" - serial_filter_imu: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" + serial_filter_top_board: "AF-7A42-07AA-D181-0356-7715-6D7C-4C65-5762" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index e6846920..95c71b22 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -42,8 +42,7 @@ value_broadcaster: deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" - serial_filter_imu: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" + serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" left_front_zero_point: 7173 left_back_zero_point: 5167 right_back_zero_point: 3098 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 27d76767..b85d1af6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -73,8 +73,6 @@ class DeformableChassis register_input( fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), joint_physical_angle_[i], false); - register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); - register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); @@ -396,8 +394,6 @@ class DeformableChassis deactivate_complex_spin_(); deactivate_qe_complex_spin_(); - *scope_motor_control_torque = nan_; - for (size_t i = 0; i < kJointCount; ++i) { *joint_target_physical_angle_[i] = nan_; *joint_target_physical_velocity_[i] = nan_; @@ -556,7 +552,6 @@ class DeformableChassis // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. void run_joint_intent_pipeline_() { const auto current_physical_angles = read_current_joint_physical_angles_(); - const bool suspension_requested = suspension_requested_by_input_(); if (!ensure_joint_target_states_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); @@ -568,23 +563,12 @@ class DeformableChassis current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - scope_motor_control(suspension_requested); - update_joint_target_trajectory(); publish_joint_target_angles(current_physical_angles); } static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - void scope_motor_control(bool suspension_requested = false) { - const bool prone_target_active = suspension_requested; - if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { - *scope_motor_control_torque = -0.3; - } else { - *scope_motor_control_torque = 0.3; - } - } - void update_joint_target_trajectory() { const double dt = update_dt(); for (size_t i = 0; i < kJointCount; ++i) { @@ -707,8 +691,6 @@ class DeformableChassis "left_front", "left_back", "right_back", "right_front"}; std::array, kJointCount> joint_physical_angle_; - OutputInterface scope_motor_control_torque; - std::array, kJointCount> joint_angle_error_; std::array, kJointCount> joint_target_physical_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 25d8fe1e..16eba5d5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -63,18 +63,13 @@ class DeformableInfantryOmniB status_service_callback(response); }); - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); - - rmcs_board_lite = std::make_unique( + bottom_board_ = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); - remote_control_ = std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); + *this, *deformable_infantry_command_, vt13_, + get_parameter("serial_filter_top_board").as_string()); + remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); } ~DeformableInfantryOmniB() override = default; @@ -85,24 +80,21 @@ class DeformableInfantryOmniB } void update() override { - rmcs_board_lite->update(); + bottom_board_->update(); top_board_->update(); - if (imu_board_) - imu_board_->update(); vt13_.update_status(); remote_control_->update(); } void command_update() { const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); + bottom_board_->command_update(even); top_board_->command_update(); } private: class DeformableInfantryOmniBCommand; class BottomBoard; - class ImuBoard; class TopBoard; class DeformableInfantryOmniBCommand : public rmcs_executor::Component { @@ -615,61 +607,6 @@ class DeformableInfantryOmniB OutputInterface radius_; }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmniB; - - public: - explicit ImuBoard( - DeformableInfantryOmniB& deformableInfantry, device::Vt13& vt13, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{deformableInfantry.tf_} - , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; - }; - class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class DeformableInfantryOmniB; @@ -677,20 +614,20 @@ class DeformableInfantryOmniB explicit TopBoard( DeformableInfantryOmniB& deformableInfantry, DeformableInfantryOmniBCommand& deformableInfantry_command, - std::string serial_filter = {}, bool has_external_imu_board = false) + device::Vt13& vt13, const std::string& serial_filter = {}) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) + , vt13_{vt13} + , bmi088_pitch_{1000, 0.2, 0.0} + , bmi088_yaw_{1000, 0.2, 0.0} , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") , gimbal_left_friction_( deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { + deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -706,19 +643,15 @@ class DeformableInfantryOmniB gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); deformableInfantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); - }); + bmi088_pitch_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + bmi088_yaw_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; @@ -729,34 +662,22 @@ class DeformableInfantryOmniB } void update() { - bmi088_.update_status(); + bmi088_pitch_.update_status(); + bmi088_yaw_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } + *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); + *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( pitch_encoder_angle); } @@ -767,14 +688,24 @@ class DeformableInfantryOmniB .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, } .as_bytes(), @@ -782,6 +713,10 @@ class DeformableInfantryOmniB } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -796,33 +731,38 @@ class DeformableInfantryOmniB return; if (data.can_id == 0x201) gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x202) gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); } void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); } std::atomic& hard_sync_pending_; - bool has_external_imu_board_ = false; OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + device::Vt13& vt13_; - device::Bmi088 bmi088_; + device::Bmi088 bmi088_pitch_; + device::Bmi088 bmi088_yaw_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; }; auto status_service_callback(const std::shared_ptr& response) @@ -835,14 +775,14 @@ class DeformableInfantryOmniB }; text("Gimbal Status"); - text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); text("Chassis Status"); - text("- left front: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); - text("- left back: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); - text("- right back: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); - text("- right front: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); + text("- left front: {}", bottom_board_->chassis_joint_motors_[0].last_raw_angle()); + text("- left back: {}", bottom_board_->chassis_joint_motors_[1].last_raw_angle()); + text("- right back: {}", bottom_board_->chassis_joint_motors_[2].last_raw_angle()); + text("- right front: {}", bottom_board_->chassis_joint_motors_[3].last_raw_angle()); response->message = feedback_message.str(); } @@ -855,8 +795,7 @@ class DeformableInfantryOmniB Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr imu_board_; + std::unique_ptr bottom_board_; std::unique_ptr top_board_; std::unique_ptr remote_control_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 67044424..c5cedc35 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -54,9 +54,7 @@ class DeformableInfantryOmni bottom_board_ = std::make_unique( *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( - *this, *command_, get_parameter("serial_filter_top_board").as_string(), true); - imu_board_ = std::make_unique( - *this, vt13_, get_parameter("serial_filter_imu").as_string()); + *this, *command_, vt13_, get_parameter("serial_filter_top_board").as_string()); remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); // For command: remote-status @@ -75,7 +73,6 @@ class DeformableInfantryOmni void update() override { bottom_board_->update(); top_board_->update(); - imu_board_->update(); vt13_.update_status(); remote_control_->update(); } @@ -575,77 +572,23 @@ class DeformableInfantryOmni } }; - class ImuBoard final : private librmcs::agent::RmcsBoardLite { + class TopBoard final : private librmcs::agent::RmcsBoardLite { friend class DeformableInfantryOmni; public: - explicit ImuBoard( - DeformableInfantryOmni& status, device::Vt13& vt13, + explicit TopBoard( + DeformableInfantryOmni& status, Command& command, device::Vt13& vt13, const std::string& serial_filter = {}) : RmcsBoardLite{ serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryOmni; - - public: - explicit TopBoard( - DeformableInfantryOmni& status, Command& command, std::string serial_filter = {}, - bool has_external_imu_board = false) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , has_external_imu_board_(has_external_imu_board) - , tf_(status.tf_) - , bmi088_(1000, 0.2, 0.0) + , bmi088_pitch_{1000, 0.2, 0.0} + , bmi088_yaw_{1000, 0.2, 0.0} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") - , gimbal_right_friction_(status, command, "/gimbal/right_friction") - , scope_motor_(status, command, "/gimbal/scope") { + , gimbal_right_friction_(status, command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -660,55 +603,36 @@ class DeformableInfantryOmni gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - + status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(-x, -y, z); - }); + bmi088_pitch_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + bmi088_yaw_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } + void request_hard_sync_read() {} void update() { - bmi088_.update_status(); + bmi088_pitch_.update_status(); + bmi088_yaw_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> - // OdomImu, so use the encoder pitch from the TF tree to move the - // yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } + *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); + *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( pitch_encoder_angle); } @@ -719,7 +643,6 @@ class DeformableInfantryOmni .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ .can_id = 0x200, .can_data = @@ -731,7 +654,6 @@ class DeformableInfantryOmni } .as_bytes(), }); - builder.can2_transmit({ .can_id = 0x200, .can_data = @@ -743,21 +665,13 @@ class DeformableInfantryOmni } .as_bytes(), }); - - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - scope_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -781,32 +695,27 @@ class DeformableInfantryOmni gimbal_right_friction_.store_status(data.can_data); } - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); - } - void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); } - bool has_external_imu_board_ = false; OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + device::Vt13& vt13_; - device::Bmi088 bmi088_; + device::Bmi088 bmi088_pitch_; + device::Bmi088 bmi088_yaw_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; }; auto status_service_callback(const std::shared_ptr& response) @@ -840,7 +749,6 @@ class DeformableInfantryOmni InputInterface timestamp_; device::Vt13 vt13_; - std::unique_ptr imu_board_; std::unique_ptr top_board_; std::unique_ptr bottom_board_; std::unique_ptr remote_control_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index 9d6d10a7..334ef6e0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -63,18 +63,14 @@ class DeformableInfantryV2 status_service_callback(response); }); - rmcs_board_lite = std::make_unique( + bottom_board_ = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); - std::string serial_filter_imu; - get_parameter_or("serial_filter_imu", serial_filter_imu, std::string{}); top_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_top_board").as_string(), !serial_filter_imu.empty()); - if (!serial_filter_imu.empty()) - imu_board_ = std::make_unique(*this, vt13_, serial_filter_imu); + *this, *deformable_infantry_command_, vt13_, + get_parameter("serial_filter_top_board").as_string()); remote_control_ = - std::make_unique(*this, rmcs_board_lite->dr16_, vt13_); + std::make_unique(*this, bottom_board_->dr16_, vt13_); } ~DeformableInfantryV2() override = default; @@ -85,24 +81,21 @@ class DeformableInfantryV2 } void update() override { - rmcs_board_lite->update(); + bottom_board_->update(); top_board_->update(); - if (imu_board_) - imu_board_->update(); vt13_.update_status(); remote_control_->update(); } void command_update() { const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_lite->command_update(even); + bottom_board_->command_update(even); top_board_->command_update(); } private: class DeformableInfantryV2Command; class BottomBoard; - class ImuBoard; class TopBoard; class DeformableInfantryV2Command : public rmcs_executor::Component { @@ -688,20 +681,20 @@ class DeformableInfantryV2 explicit TopBoard( DeformableInfantryV2& deformableInfantry, DeformableInfantryV2Command& deformableInfantry_command, - std::string serial_filter = {}, bool has_external_imu_board = false) + device::Vt13& vt13, const std::string& serial_filter = {}) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , hard_sync_pending_(deformableInfantry.hard_sync_pending_) - , has_external_imu_board_(has_external_imu_board) , tf_(deformableInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) + , vt13_{vt13} + , bmi088_pitch_{1000, 0.2, 0.0} + , bmi088_yaw_{1000, 0.2, 0.0} , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") , gimbal_left_friction_( deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") - , scope_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/scope") { + deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") { gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} @@ -717,19 +710,15 @@ class DeformableInfantryV2 gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - scope_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); deformableInfantry.register_output( "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - if (!has_external_imu_board_) - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). - return std::make_tuple(y, -x, z); - }); + bmi088_pitch_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + bmi088_yaw_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; @@ -739,33 +728,22 @@ class DeformableInfantryV2 } void update() { - bmi088_.update_status(); + bmi088_pitch_.update_status(); + bmi088_yaw_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); - scope_motor_.update_status(); - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - if (!has_external_imu_board_) { - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()}} - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); - } + *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); + *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( pitch_encoder_angle); } @@ -776,14 +754,24 @@ class DeformableInfantryV2 .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, gimbal_right_friction_.generate_command(), - scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, } .as_bytes(), @@ -791,6 +779,10 @@ class DeformableInfantryV2 } private: + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + vt13_.store_status(data.uart_data); + } + void uart1_receive_callback(const librmcs::data::UartDataView&) override {} void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -805,88 +797,38 @@ class DeformableInfantryV2 return; if (data.can_id == 0x201) gimbal_left_friction_.store_status(data.can_data); - else if (data.can_id == 0x202) + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + if (data.can_id == 0x202) gimbal_right_friction_.store_status(data.can_data); - else if (data.can_id == 0x203) - scope_motor_.store_status(data.can_data); } void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); + bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); + bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); } std::atomic& hard_sync_pending_; - bool has_external_imu_board_ = false; OutputInterface& tf_; + OutputInterface gimbal_pitch_velocity_imu_; OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; + device::Vt13& vt13_; - device::Bmi088 bmi088_; + device::Bmi088 bmi088_pitch_; + device::Bmi088 bmi088_yaw_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; - device::DjiMotor scope_motor_; - }; - - class ImuBoard final : private librmcs::agent::RmcsBoardLite { - friend class DeformableInfantryV2; - - public: - explicit ImuBoard( - DeformableInfantryV2& deformableInfantry, device::Vt13& vt13, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , tf_{deformableInfantry.tf_} - , vt13_{vt13} - , bmi088_{1000, 0.2, 0.0} { - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~ImuBoard() override = default; - - void update() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - OutputInterface gimbal_pitch_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 bmi088_; }; auto status_service_callback(const std::shared_ptr& response) @@ -899,22 +841,22 @@ class DeformableInfantryV2 }; text("Gimbal Status"); - text("- Yaw: {}", rmcs_board_lite->gimbal_yaw_motor_.last_raw_angle()); + text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); text("Chassis Status"); - text("- left front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[0].last_raw_angle()); - text("- left back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[1].last_raw_angle()); - text("- right back wheel: {}", rmcs_board_lite->chassis_wheel_motors_[2].last_raw_angle()); - text("- right front wheel: {}", rmcs_board_lite->chassis_wheel_motors_[3].last_raw_angle()); - text("- left front steer: {}", rmcs_board_lite->chassis_steer_motors_[0].last_raw_angle()); - text("- left back steer: {}", rmcs_board_lite->chassis_steer_motors_[1].last_raw_angle()); - text("- right back steer: {}", rmcs_board_lite->chassis_steer_motors_[2].last_raw_angle()); - text("- right front steer: {}", rmcs_board_lite->chassis_steer_motors_[3].last_raw_angle()); - text("- left front joint: {}", rmcs_board_lite->chassis_joint_motors_[0].last_raw_angle()); - text("- left back joint: {}", rmcs_board_lite->chassis_joint_motors_[1].last_raw_angle()); - text("- right back joint: {}", rmcs_board_lite->chassis_joint_motors_[2].last_raw_angle()); - text("- right front joint: {}", rmcs_board_lite->chassis_joint_motors_[3].last_raw_angle()); + text("- left front wheel: {}", bottom_board_->chassis_wheel_motors_[0].last_raw_angle()); + text("- left back wheel: {}", bottom_board_->chassis_wheel_motors_[1].last_raw_angle()); + text("- right back wheel: {}", bottom_board_->chassis_wheel_motors_[2].last_raw_angle()); + text("- right front wheel: {}", bottom_board_->chassis_wheel_motors_[3].last_raw_angle()); + text("- left front steer: {}", bottom_board_->chassis_steer_motors_[0].last_raw_angle()); + text("- left back steer: {}", bottom_board_->chassis_steer_motors_[1].last_raw_angle()); + text("- right back steer: {}", bottom_board_->chassis_steer_motors_[2].last_raw_angle()); + text("- right front steer: {}", bottom_board_->chassis_steer_motors_[3].last_raw_angle()); + text("- left front joint: {}", bottom_board_->chassis_joint_motors_[0].last_raw_angle()); + text("- left back joint: {}", bottom_board_->chassis_joint_motors_[1].last_raw_angle()); + text("- right back joint: {}", bottom_board_->chassis_joint_motors_[2].last_raw_angle()); + text("- right front joint: {}", bottom_board_->chassis_joint_motors_[3].last_raw_angle()); response->message = feedback_message.str(); } @@ -927,8 +869,7 @@ class DeformableInfantryV2 Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_lite; - std::unique_ptr imu_board_; + std::unique_ptr bottom_board_; std::unique_ptr top_board_; std::unique_ptr remote_control_; From a973e70eb6879df0cfa6faa7c2ffc1fd61f9b18d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sat, 4 Jul 2026 19:09:27 +0800 Subject: [PATCH 66/90] fix(gimbal): preserve yaw hold control and unify infantry IMU handling Reset the yaw loops only on ctrl-hold state transitions so yaw hold can keep producing torque while active. Remove stale hard-sync and duplicate BMI088 config paths across deformable infantry variants to match the single IMU pipeline. --- .../config/deformable-infantry-omni-b.yaml | 3 -- .../config/deformable-infantry-omni.yaml | 5 +- .../config/deformable-infantry-steering.yaml | 3 -- .../deformable_infantry_gimbal_controller.cpp | 21 +++++++-- .../hardware/deformable-infantry-omni-b.cpp | 46 +++++-------------- .../src/hardware/deformable-infantry-omni.cpp | 35 +++++--------- .../hardware/deformable-infantry-steering.cpp | 45 +++++------------- 7 files changed, 52 insertions(+), 106 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 6e2dd6d6..2fe429bc 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -134,9 +134,6 @@ gimbal_controller: pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 29a129bd..02345be4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -120,7 +120,7 @@ gimbal_controller: yaw_vel_ff_gain: 0.47 yaw_acc_ff_gain: 0.00 - pitch_angle_kp: 25.0 + pitch_angle_kp: 7.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 @@ -132,9 +132,6 @@ gimbal_controller: pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 95c71b22..da45146b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -103,9 +103,6 @@ active_suspension_controller: gimbal_controller: ros__parameters: - inertia: 1.0 # kg·m² - friction: 1.65 # Nm/(rad/s) - upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg ctrl_hold_pitch_target_angle: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 76a1511e..5b4baba6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -252,6 +252,8 @@ class DeformableInfantryGimbalController locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) ? *input_.yaw_angle : 0.0; + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); @@ -263,6 +265,8 @@ class DeformableInfantryGimbalController ctrl_hold_active_ = false; locked_yaw_angle_ = kNaN; + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); *output_.yaw_control_angle = kNaN; *output_.pitch_control_angle = kNaN; *output_.chassis_manual_yaw_velocity_override = kNaN; @@ -273,8 +277,6 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) activate_ctrl_hold(); - yaw_angle_pid_.reset(); - yaw_velocity_pid_.reset(); *output_.yaw_control_torque = kNaN; *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; @@ -283,9 +285,22 @@ class DeformableInfantryGimbalController *output_.chassis_manual_yaw_velocity_override = kNaN; *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; - *output_.yaw_control_angle = locked_yaw_angle_; *output_.pitch_control_angle = kNaN; + if (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle) && input_.yaw_velocity.ready() + && std::isfinite(*input_.yaw_velocity)) { + auto yaw_target_error = locked_yaw_angle_ - *input_.yaw_angle; + if (yaw_target_error > std::numbers::pi) + yaw_target_error -= 2 * std::numbers::pi; + else if (yaw_target_error < -std::numbers::pi) + yaw_target_error += 2 * std::numbers::pi; + + *output_.yaw_angle_error = yaw_target_error; + const auto yaw_velocity_ref = yaw_angle_pid_.update(yaw_target_error); + *output_.yaw_control_torque = + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity); + } + if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; if (pitch_target_error > std::numbers::pi) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 16eba5d5..a6596b03 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -50,7 +50,6 @@ class DeformableInfantryOmniB get_component_name() + "_command", *this)) { using namespace rmcs_description; - register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -74,11 +73,6 @@ class DeformableInfantryOmniB ~DeformableInfantryOmniB() override = default; - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - void update() override { bottom_board_->update(); top_board_->update(); @@ -618,11 +612,9 @@ class DeformableInfantryOmniB : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) , tf_(deformableInfantry.tf_) , vt13_{vt13} - , bmi088_pitch_{1000, 0.2, 0.0} - , bmi088_yaw_{1000, 0.2, 0.0} + , gimbal_imu_{1000, 0.2, 0.0} , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") , gimbal_left_friction_( deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") @@ -646,36 +638,28 @@ class DeformableInfantryOmniB deformableInfantry.register_output( "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - bmi088_pitch_.set_coordinate_mapping( + gimbal_imu_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - bmi088_yaw_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request - // path. - } - void update() { - bmi088_pitch_.update_status(); - bmi088_yaw_.update_status(); + gimbal_imu_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); - *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( @@ -742,24 +726,20 @@ class DeformableInfantryOmniB void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); - bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); + gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); - bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); + gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); } - std::atomic& hard_sync_pending_; OutputInterface& tf_; OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_; device::Vt13& vt13_; - device::Bmi088 bmi088_pitch_; - device::Bmi088 bmi088_yaw_; + device::Bmi088 gimbal_imu_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; @@ -788,11 +768,7 @@ class DeformableInfantryOmniB } OutputInterface tf_; - InputInterface timestamp_; device::Vt13 vt13_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; std::unique_ptr bottom_board_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index c5cedc35..3560c5b6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -46,7 +46,6 @@ class DeformableInfantryOmni , command_(create_partner_component(get_component_name() + "_command", *this)) { using namespace rmcs_description; - register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -68,8 +67,6 @@ class DeformableInfantryOmni ~DeformableInfantryOmni() override = default; - void before_updating() override { top_board_->request_hard_sync_read(); } - void update() override { bottom_board_->update(); top_board_->update(); @@ -584,8 +581,7 @@ class DeformableInfantryOmni librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} , tf_{status.tf_} , vt13_{vt13} - , bmi088_pitch_{1000, 0.2, 0.0} - , bmi088_yaw_{1000, 0.2, 0.0} + , gimbal_imu_{1000, 0.2, 0.0} , gimbal_pitch_motor_(status, command, "/gimbal/pitch") , gimbal_left_friction_(status, command, "/gimbal/left_friction") , gimbal_right_friction_(status, command, "/gimbal/right_friction") { @@ -604,33 +600,28 @@ class DeformableInfantryOmni device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); status.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + status.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - bmi088_pitch_.set_coordinate_mapping( + gimbal_imu_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - bmi088_yaw_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; - void request_hard_sync_read() {} - void update() { - bmi088_pitch_.update_status(); - bmi088_yaw_.update_status(); + gimbal_imu_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); - *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( @@ -697,22 +688,19 @@ class DeformableInfantryOmni void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); - bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); + gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); - bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); + gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); } OutputInterface& tf_; OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_; device::Vt13& vt13_; - device::Bmi088 bmi088_pitch_; - device::Bmi088 bmi088_yaw_; + device::Bmi088 gimbal_imu_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; @@ -746,7 +734,6 @@ class DeformableInfantryOmni } OutputInterface tf_; - InputInterface timestamp_; device::Vt13 vt13_; std::unique_ptr top_board_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp index 334ef6e0..bdac3e64 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp @@ -50,7 +50,6 @@ class DeformableInfantryV2 get_component_name() + "_command", *this)) { using namespace rmcs_description; - register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -75,11 +74,6 @@ class DeformableInfantryV2 ~DeformableInfantryV2() override = default; - void before_updating() override { - top_board_->request_hard_sync_read(); - next_hard_sync_log_time_ = Clock::now() + std::chrono::seconds(1); - } - void update() override { bottom_board_->update(); top_board_->update(); @@ -685,11 +679,9 @@ class DeformableInfantryV2 : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , hard_sync_pending_(deformableInfantry.hard_sync_pending_) , tf_(deformableInfantry.tf_) , vt13_{vt13} - , bmi088_pitch_{1000, 0.2, 0.0} - , bmi088_yaw_{1000, 0.2, 0.0} + , gimbal_imu_{1000, 0.2, 0.0} , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") , gimbal_left_friction_( deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") @@ -713,35 +705,28 @@ class DeformableInfantryV2 deformableInfantry.register_output( "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - bmi088_pitch_.set_coordinate_mapping( + gimbal_imu_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - bmi088_yaw_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } ~TopBoard() override = default; - void request_hard_sync_read() { - // RMCS-lite top board variant currently has no GPIO hard-sync request path. - } - void update() { - bmi088_pitch_.update_status(); - bmi088_yaw_.update_status(); + gimbal_imu_.update_status(); gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_pitch_.q0(), bmi088_pitch_.q1(), bmi088_pitch_.q2(), bmi088_pitch_.q3()}; + gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_pitch_velocity_imu_ = bmi088_pitch_.gy(); - *gimbal_yaw_velocity_bmi088_ = bmi088_yaw_.gz(); + *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( @@ -808,24 +793,20 @@ class DeformableInfantryV2 void accelerometer_receive_callback( const librmcs::data::AccelerometerDataView& data) override { - bmi088_pitch_.store_accelerometer_status(data.x, data.y, data.z); - bmi088_yaw_.store_accelerometer_status(data.x, data.y, data.z); + gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_pitch_.store_gyroscope_status(data.x, data.y, data.z); - bmi088_yaw_.store_gyroscope_status(data.x, data.y, data.z); + gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); } - std::atomic& hard_sync_pending_; OutputInterface& tf_; OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_yaw_velocity_imu_; device::Vt13& vt13_; - device::Bmi088 bmi088_pitch_; - device::Bmi088 bmi088_yaw_; + device::Bmi088 gimbal_imu_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; device::DjiMotor gimbal_right_friction_; @@ -862,11 +843,7 @@ class DeformableInfantryV2 } OutputInterface tf_; - InputInterface timestamp_; device::Vt13 vt13_; - std::atomic hard_sync_pending_{false}; - size_t hard_sync_snapshot_count_ = 0; - Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; std::unique_ptr bottom_board_; From 512d61b160f75f802aef69ffe783abcb4ad3a253 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Sun, 5 Jul 2026 01:07:40 +0800 Subject: [PATCH 67/90] fix(chassis): merge active suspension back into deformable_chassis and remove ctrl-hold yaw lock - Delete standalone ActiveSuspensionController component - Restore active suspension logic (AttitudePidAxis, IMU calibration, joint I/O) inside DeformableChassis - Remove ctrl-hold yaw lock from both chassis angular-velocity override and gimbal PID lock - Gimbal yaw now follows normal solver path during ctrl-hold, pitch remains locked to level - Simplify configs: remove active_suspension_controller section, restore single-loop PID params --- .../config/deformable-infantry-omni-b.yaml | 61 +- .../config/deformable-infantry-omni.yaml | 75 +-- .../config/deformable-infantry-steering.yaml | 53 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 - .../chassis/active_suspension_controller.cpp | 239 -------- .../controller/chassis/deformable_chassis.cpp | 523 +++++++++++++++--- .../deformable_infantry_gimbal_controller.cpp | 127 ++--- 7 files changed, 562 insertions(+), 517 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 2fe429bc..89b4ef02 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -21,7 +21,6 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -64,45 +63,27 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 -active_suspension_controller: - ros__parameters: - min_angle: 8.0 - max_angle: 58.0 - active_suspension_enable: true - pitch_angle_diff_limit_deg: 45.0 - roll_angle_diff_limit_deg: 45.0 - - pitch_outer_kp: 8.0 - pitch_outer_ki: 0.35 - pitch_outer_kd: 0.28 - pitch_outer_integral_min: -20.0 - pitch_outer_integral_max: 20.0 - pitch_outer_integral_split_min: -30.0 - pitch_outer_integral_split_max: 30.0 - pitch_outer_output_min: -45.0 - pitch_outer_output_max: 45.0 - - pitch_inner_kp: 2.0 - pitch_inner_ki: 0.0 - pitch_inner_kd: 0.0 - pitch_inner_output_min: -45.0 - pitch_inner_output_max: 45.0 - - roll_outer_kp: 8.0 - roll_outer_ki: 0.35 - roll_outer_kd: 0.28 - roll_outer_integral_min: -20.0 - roll_outer_integral_max: 20.0 - roll_outer_integral_split_min: -30.0 - roll_outer_integral_split_max: 30.0 - roll_outer_output_min: -45.0 - roll_outer_output_max: 45.0 - - roll_inner_kp: 2.0 - roll_inner_ki: 0.0 - roll_inner_kd: 0.0 - roll_inner_output_min: -45.0 - roll_inner_output_max: 45.0 + # IMU attitude correction at min-angle stance. + active_suspension_pitch_kp: 8.0 + active_suspension_pitch_ki: 0.35 + active_suspension_pitch_kd: 0.28 + + active_suspension_roll_kp: 8.0 + active_suspension_roll_ki: 0.35 + active_suspension_roll_kd: 0.28 + + active_suspension_pitch_angle_diff_limit_deg: 45.0 + active_suspension_roll_angle_diff_limit_deg: 45.0 + active_suspension_pid_integral_limit_deg: 20.0 + + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 + + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 02345be4..a5877e2c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -21,7 +21,6 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -56,51 +55,33 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 - max_angle: 58.0 + min_angle: 12.0 + max_angle: 50.0 launch_ramp_shortcut_enabled: false active_suspension_enable: true spin_ratio: 1.0 -active_suspension_controller: - ros__parameters: - min_angle: 8.0 - max_angle: 58.0 - active_suspension_enable: true - pitch_angle_diff_limit_deg: 45.0 - roll_angle_diff_limit_deg: 45.0 - - pitch_outer_kp: 8.0 - pitch_outer_ki: 0.35 - pitch_outer_kd: 0.28 - pitch_outer_integral_min: -20.0 - pitch_outer_integral_max: 20.0 - pitch_outer_integral_split_min: -30.0 - pitch_outer_integral_split_max: 30.0 - pitch_outer_output_min: -45.0 - pitch_outer_output_max: 45.0 - - pitch_inner_kp: 2.0 - pitch_inner_ki: 0.0 - pitch_inner_kd: 0.0 - pitch_inner_output_min: -45.0 - pitch_inner_output_max: 45.0 - - roll_outer_kp: 8.0 - roll_outer_ki: 0.35 - roll_outer_kd: 0.28 - roll_outer_integral_min: -20.0 - roll_outer_integral_max: 20.0 - roll_outer_integral_split_min: -30.0 - roll_outer_integral_split_max: 30.0 - roll_outer_output_min: -45.0 - roll_outer_output_max: 45.0 - - roll_inner_kp: 2.0 - roll_inner_ki: 0.0 - roll_inner_kd: 0.0 - roll_inner_output_min: -45.0 - roll_inner_output_max: 45.0 + # IMU attitude correction at min-angle stance. + active_suspension_pitch_kp: 8.0 + active_suspension_pitch_ki: 0.35 + active_suspension_pitch_kd: 0.28 + + active_suspension_roll_kp: 8.0 + active_suspension_roll_ki: 0.35 + active_suspension_roll_kd: 0.28 + + active_suspension_pitch_angle_diff_limit_deg: 45.0 + active_suspension_roll_angle_diff_limit_deg: 45.0 + active_suspension_pid_integral_limit_deg: 20.0 + + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 + + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: @@ -193,7 +174,7 @@ deformable_chassis_controller: moment_of_inertia: 1.0 chassis_radius: 0.2341741 rod_length: 0.150 - wheel_radius: 0.07 + wheel_radius: 0.055 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -207,7 +188,7 @@ lf_joint_controller: control: /chassis/left_front_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -229,7 +210,7 @@ lb_joint_controller: setpoint_angle: /chassis/left_back_joint/target_physical_angle control: /chassis/left_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -251,7 +232,7 @@ rb_joint_controller: setpoint_angle: /chassis/right_back_joint/target_physical_angle control: /chassis/right_back_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -273,7 +254,7 @@ rf_joint_controller: setpoint_angle: /chassis/right_front_joint/target_physical_angle control: /chassis/right_front_joint/control_torque dt: 0.001 - b0: -0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index da45146b..63b00d67 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -21,7 +21,6 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::ActiveSuspensionController -> active_suspension_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableChassisController -> deformable_chassis_controller @@ -61,45 +60,27 @@ chassis_controller: active_suspension_enable: true spin_ratio: 1.0 -active_suspension_controller: - ros__parameters: - min_angle: 20.0 - max_angle: 50.0 - active_suspension_enable: true - pitch_angle_diff_limit_deg: 45.0 - roll_angle_diff_limit_deg: 45.0 + # IMU attitude correction at min-angle stance. + active_suspension_pitch_kp: 8.0 + active_suspension_pitch_ki: 0.35 + active_suspension_pitch_kd: 0.28 - pitch_outer_kp: 8.0 - pitch_outer_ki: 0.35 - pitch_outer_kd: 0.28 - pitch_outer_integral_min: -20.0 - pitch_outer_integral_max: 20.0 - pitch_outer_integral_split_min: -30.0 - pitch_outer_integral_split_max: 30.0 - pitch_outer_output_min: -45.0 - pitch_outer_output_max: 45.0 + active_suspension_roll_kp: 8.0 + active_suspension_roll_ki: 0.35 + active_suspension_roll_kd: 0.28 - pitch_inner_kp: 2.0 - pitch_inner_ki: 0.0 - pitch_inner_kd: 0.0 - pitch_inner_output_min: -45.0 - pitch_inner_output_max: 45.0 + active_suspension_pitch_angle_diff_limit_deg: 45.0 + active_suspension_roll_angle_diff_limit_deg: 45.0 + active_suspension_pid_integral_limit_deg: 20.0 - roll_outer_kp: 8.0 - roll_outer_ki: 0.35 - roll_outer_kd: 0.28 - roll_outer_integral_min: -20.0 - roll_outer_integral_max: 20.0 - roll_outer_integral_split_min: -30.0 - roll_outer_integral_split_max: 30.0 - roll_outer_output_min: -45.0 - roll_outer_output_max: 45.0 + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 - roll_inner_kp: 2.0 - roll_inner_ki: 0.0 - roll_inner_kd: 0.0 - roll_inner_output_min: -45.0 - roll_inner_output_max: 45.0 + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 59dd7aab..1438f8a8 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -13,7 +13,6 @@ - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp deleted file mode 100644 index 1dfaca29..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/active_suspension_controller.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "controller/pid/pid_calculator.hpp" - -namespace rmcs_core::controller::chassis { - -class ActiveSuspensionController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - ActiveSuspensionController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - load_pid_("pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28); - load_pid_("pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0); - load_pid_("roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28); - load_pid_("roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0); - - min_angle_deg_ = get_parameter_or("min_angle", 8.0); - max_angle_deg_ = get_parameter_or("max_angle", 58.0); - active_suspension_enable_ = get_parameter_or("active_suspension_enable", true); - pitch_angle_diff_limit_deg_ = get_parameter_or("pitch_angle_diff_limit_deg", 45.0); - roll_angle_diff_limit_deg_ = get_parameter_or("roll_angle_diff_limit_deg", 45.0); - - register_input("/chassis/ctrl_hold_active", ctrl_hold_active_); - register_input("/chassis/imu/pitch", chassis_imu_pitch_); - register_input("/chassis/imu/roll", chassis_imu_roll_); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_); - - for (size_t i = 0; i < kJointCount; ++i) { - register_input( - fmt::format("/chassis/{}_joint/base_target_physical_angle", kJointNames[i]), - base_angles_[i]); - register_input( - fmt::format("/chassis/{}_joint/base_target_physical_velocity", kJointNames[i]), - base_velocities_[i], false); - register_input( - fmt::format("/chassis/{}_joint/base_target_physical_acceleration", kJointNames[i]), - base_accelerations_[i], false); - register_input( - fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), physical_angles_[i]); - - register_output( - fmt::format("/chassis/{}_joint/target_physical_angle", kJointNames[i]), - target_angles_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/target_physical_velocity", kJointNames[i]), - target_velocities_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointNames[i]), - target_accelerations_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/control_angle_error", kJointNames[i]), - angle_errors_[i], nan_); - } - } - - void update() override { - if (!base_targets_ready_()) { - reset_outputs_nan_(); - return; - } - - if (!active_suspension_enable_ || !ctrl_hold_active_ready_() || !feedback_ready_()) { - passthrough_outputs_(); - return; - } - - const double pitch = std::clamp(*chassis_imu_pitch_, -kMaxAttitude, kMaxAttitude); - const double roll = std::clamp(*chassis_imu_roll_, -kMaxAttitude, kMaxAttitude); - const double pitch_rate = *chassis_imu_pitch_rate_; - const double roll_rate = *chassis_imu_roll_rate_; - - const double pitch_outer = pitch_outer_pid_.update(-pitch); - const double roll_outer = roll_outer_pid_.update(-roll); - - const double pitch_inner = pitch_inner_pid_.update(pitch_outer - pitch_rate); - const double roll_inner = roll_inner_pid_.update(roll_outer - roll_rate); - - if (!std::isfinite(pitch_inner) || !std::isfinite(roll_inner)) { - reset_outputs_nan_(); - return; - } - - const double base_angle = deg_to_rad_(min_angle_deg_ - 5.0); - const double max_angle = deg_to_rad_(max_angle_deg_); - const double pitch_limit = deg_to_rad_(pitch_angle_diff_limit_deg_); - const double roll_limit = deg_to_rad_(roll_angle_diff_limit_deg_); - - const double pitch_delta = std::clamp(pitch_inner, -pitch_limit, pitch_limit); - const double roll_delta = std::clamp(roll_inner, -roll_limit, roll_limit); - - static constexpr std::array kPitchSign = {-1, 1, 1, -1}; - static constexpr std::array kRollSign = {1, 1, -1, -1}; - - for (size_t i = 0; i < kJointCount; ++i) - *target_angles_[i] = - std::clamp( - base_angle + std::max(kPitchSign[i] * pitch_delta, 0.0) - + std::max(kRollSign[i] * roll_delta, 0.0), - base_angle, max_angle); - - for (size_t i = 0; i < kJointCount; ++i) { - *target_velocities_[i] = optional_input_or_(base_velocities_[i], 0.0); - *target_accelerations_[i] = optional_input_or_(base_accelerations_[i], 0.0); - } - publish_angle_errors_(); - } - -private: - static constexpr size_t kJointCount = 4; - static constexpr std::array kJointNames = { - "left_front", "left_back", "right_back", "right_front"}; - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double kMaxAttitude = 30.0 * std::numbers::pi / 180.0; - - static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } - - static double optional_input_or_(const InputInterface& input, double fallback) { - return input.ready() && std::isfinite(*input) ? *input : fallback; - } - - static double load_double_or_(rclcpp::Node& node, const std::string& name, double fallback) { - double value = fallback; - node.get_parameter_or(name, value, fallback); - return value; - } - - void load_pid_( - const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, - double kd_default) { - pid.kp = load_double_or_(*this, prefix + "kp", kp_default); - pid.ki = load_double_or_(*this, prefix + "ki", ki_default); - pid.kd = load_double_or_(*this, prefix + "kd", kd_default); - pid.integral_min = load_double_or_(*this, prefix + "integral_min", -1e9); - pid.integral_max = load_double_or_(*this, prefix + "integral_max", 1e9); - pid.integral_split_min = load_double_or_(*this, prefix + "integral_split_min", -1e9); - pid.integral_split_max = load_double_or_(*this, prefix + "integral_split_max", 1e9); - pid.output_min = load_double_or_(*this, prefix + "output_min", -1e9); - pid.output_max = load_double_or_(*this, prefix + "output_max", 1e9); - } - - bool ctrl_hold_active_ready_() const { - return ctrl_hold_active_.ready() && *ctrl_hold_active_; - } - - bool base_targets_ready_() const { - for (size_t i = 0; i < kJointCount; ++i) - if (!base_angles_[i].ready()) - return false; - return true; - } - - bool feedback_ready_() const { - if (!chassis_imu_pitch_.ready() || !chassis_imu_roll_.ready() - || !chassis_imu_pitch_rate_.ready() || !chassis_imu_roll_rate_.ready()) - return false; - for (size_t i = 0; i < kJointCount; ++i) - if (!physical_angles_[i].ready()) - return false; - return true; - } - - void passthrough_outputs_() { - for (size_t i = 0; i < kJointCount; ++i) { - *target_angles_[i] = *base_angles_[i]; - *target_velocities_[i] = optional_input_or_(base_velocities_[i], 0.0); - *target_accelerations_[i] = optional_input_or_(base_accelerations_[i], 0.0); - } - publish_angle_errors_(); - } - - void reset_outputs_nan_() { - pitch_outer_pid_.reset(); - pitch_inner_pid_.reset(); - roll_outer_pid_.reset(); - roll_inner_pid_.reset(); - - for (size_t i = 0; i < kJointCount; ++i) { - *target_angles_[i] = nan_; - *target_velocities_[i] = nan_; - *target_accelerations_[i] = nan_; - *angle_errors_[i] = nan_; - } - } - - void publish_angle_errors_() { - for (size_t i = 0; i < kJointCount; ++i) - *angle_errors_[i] = physical_angles_[i].ready() && std::isfinite(*physical_angles_[i]) - ? *physical_angles_[i] - *target_angles_[i] - : nan_; - } - - pid::PidCalculator pitch_outer_pid_{}; - pid::PidCalculator pitch_inner_pid_{}; - pid::PidCalculator roll_outer_pid_{}; - pid::PidCalculator roll_inner_pid_{}; - - InputInterface ctrl_hold_active_; - InputInterface chassis_imu_pitch_; - InputInterface chassis_imu_roll_; - InputInterface chassis_imu_pitch_rate_; - InputInterface chassis_imu_roll_rate_; - - std::array, kJointCount> base_angles_; - std::array, kJointCount> base_velocities_; - std::array, kJointCount> base_accelerations_; - std::array, kJointCount> physical_angles_; - - std::array, kJointCount> target_angles_; - std::array, kJointCount> target_velocities_; - std::array, kJointCount> target_accelerations_; - std::array, kJointCount> angle_errors_; - - double min_angle_deg_ = 8.0; - double max_angle_deg_ = 58.0; - bool active_suspension_enable_ = true; - double pitch_angle_diff_limit_deg_ = 45.0; - double roll_angle_diff_limit_deg_ = 45.0; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::ActiveSuspensionController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index b85d1af6..b4a72628 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -34,6 +33,28 @@ class DeformableChassis kJointCount = 4, }; + struct AttitudePidAxis { + double kp = 20.0; + double ki = 0.0; + double kd = 0.0; + double integral = 0.0; + double integral_limit = std::numeric_limits::infinity(); + double output_limit = std::numeric_limits::infinity(); + + void reset() { integral = 0.0; } + + double update(double error, double rate, double dt) { + if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { + reset(); + return std::numeric_limits::quiet_NaN(); + } + + integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); + const double output = kp * error + ki * integral - kd * rate; + return std::clamp(output, -output_limit, output_limit); + } + }; + DeformableChassis() : Node( get_component_name(), @@ -53,10 +74,56 @@ class DeformableChassis deg_to_rad( std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6)) - , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) { + , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) + , pitch_kp_(get_parameter_or("active_suspension_pitch_kp", 200.0)) + , pitch_ki_(get_parameter_or("active_suspension_pitch_ki", 0.0)) + , pitch_kd_(get_parameter_or("active_suspension_pitch_kd", 20.0)) + , roll_kp_(get_parameter_or("active_suspension_roll_kp", 200.0)) + , roll_ki_(get_parameter_or("active_suspension_roll_ki", 0.0)) + , roll_kd_(get_parameter_or("active_suspension_roll_kd", 20.0)) + , suspension_velocity_limit_( + std::max( + deg_to_rad( + std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6)) + , suspension_acceleration_limit_( + std::max( + deg_to_rad( + std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6)) + , pitch_diff_limit_( + std::abs(get_parameter_or( + "active_suspension_pitch_angle_diff_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , roll_diff_limit_( + std::abs(get_parameter_or( + "active_suspension_roll_angle_diff_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , pid_integral_limit_( + std::abs(get_parameter_or( + "active_suspension_pid_integral_limit_deg", max_angle_ - min_angle_)) + * std::numbers::pi / 180.0) + , chassis_imu_calibration_wait_time_( + std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) + , chassis_imu_calibration_sample_time_( + std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6)) { following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; + pitch_pid_.kp = pitch_kp_; + pitch_pid_.ki = pitch_ki_; + pitch_pid_.kd = pitch_kd_; + pitch_pid_.integral_limit = pid_integral_limit_; + pitch_pid_.output_limit = pitch_diff_limit_; + roll_pid_.kp = roll_kp_; + roll_pid_.ki = roll_ki_; + roll_pid_.kd = roll_kd_; + roll_pid_.integral_limit = pid_integral_limit_; + roll_pid_.output_limit = roll_diff_limit_; register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); @@ -69,10 +136,21 @@ class DeformableChassis register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); register_input("/chassis/manual_yaw_velocity_override", manual_yaw_velocity_override_, false); - for (size_t i = 0; i < kJointCount; ++i) - register_input( - fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), - joint_physical_angle_[i], false); + register_input( + "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); + register_input( + "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); + register_input( + "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); + register_input( + "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); + register_input("/chassis/imu/roll", chassis_imu_roll_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + + register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); + register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); @@ -80,20 +158,47 @@ class DeformableChassis register_output("/chassis/control_velocity", chassis_control_velocity_); register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); - for (size_t i = 0; i < kJointCount; ++i) { - register_output( - fmt::format("/chassis/{}_joint/base_control_angle_error", kJointNames[i]), - joint_angle_error_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/base_target_physical_angle", kJointNames[i]), - joint_target_physical_angle_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/base_target_physical_velocity", kJointNames[i]), - joint_target_physical_velocity_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/base_target_physical_acceleration", kJointNames[i]), - joint_target_physical_acceleration_[i], nan_); - } + register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); + register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); + register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); + register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); + + register_output( + "/chassis/left_front_joint/target_physical_angle", + left_front_joint_target_physical_angle_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_angle", + left_back_joint_target_physical_angle_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_angle", + right_back_joint_target_physical_angle_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_angle", + right_front_joint_target_physical_angle_, nan_); + register_output( + "/chassis/left_front_joint/target_physical_velocity", + left_front_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_velocity", + left_back_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_velocity", + right_back_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_velocity", + right_front_joint_target_physical_velocity_, nan_); + register_output( + "/chassis/left_front_joint/target_physical_acceleration", + left_front_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/left_back_joint/target_physical_acceleration", + left_back_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/right_back_joint/target_physical_acceleration", + right_back_joint_target_physical_acceleration_, nan_); + register_output( + "/chassis/right_front_joint/target_physical_acceleration", + right_front_joint_target_physical_acceleration_, nan_); *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; @@ -118,6 +223,14 @@ class DeformableChassis } if (!manual_yaw_velocity_override_.ready()) manual_yaw_velocity_override_.make_and_bind_directly(nan_); + if (!chassis_imu_pitch_.ready()) + chassis_imu_pitch_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_.ready()) + chassis_imu_roll_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_rate_.ready()) + chassis_imu_pitch_rate_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_rate_.ready()) + chassis_imu_roll_rate_.make_and_bind_directly(0.0); validate_joint_feedback_inputs(); } @@ -152,12 +265,17 @@ class DeformableChassis static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; + static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; + static constexpr double imu_calibration_offset_limit_rad_ = + 1.0 * std::numbers::pi / 180.0; + void validate_joint_feedback_inputs() const { - for (size_t i = 0; i < kJointCount; ++i) - if (!joint_physical_angle_[i].ready()) - throw std::runtime_error( - "missing deformable chassis feedback interfaces: expected " - "/chassis/*_joint/physical_angle"); + if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() + && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) + return; + + throw std::runtime_error( + "missing deformable chassis feedback interfaces: expected /chassis/*_joint/physical_angle"); } void update_mode_from_inputs_( @@ -315,12 +433,18 @@ class DeformableChassis } std::array read_current_joint_physical_angles_() const { + const std::array*, kJointCount> physical_angle_inputs{ + &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, + &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; + std::array current_physical_angles{}; current_physical_angles.fill(nan_); for (size_t i = 0; i < kJointCount; ++i) { - if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) - current_physical_angles[i] = *joint_physical_angle_[i]; + if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { + current_physical_angles[i] = *(*physical_angle_inputs[i]); + } } + return current_physical_angles; } @@ -347,6 +471,80 @@ class DeformableChassis return active_suspension_enable_ && ctrl_hold_requested_by_input_(); } + bool symmetric_joint_target_requested_() const { + constexpr double epsilon = 1e-6; + return std::abs(lf_current_target_angle_ - lb_current_target_angle_) <= epsilon + && std::abs(lf_current_target_angle_ - rb_current_target_angle_) <= epsilon + && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; + } + + void reset_attitude_correction_state_() { + pitch_pid_.reset(); + roll_pid_.reset(); + joint_suspension_active_.fill(false); + } + + void reset_chassis_imu_calibration_window_() { + chassis_imu_calibration_hold_elapsed_ = 0.0; + chassis_imu_calibration_sample_count_ = 0; + chassis_imu_pitch_sum_ = 0.0; + chassis_imu_roll_sum_ = 0.0; + chassis_imu_calibration_completed_for_window_ = false; + } + + void update_chassis_imu_calibration_() { + if (chassis_imu_calibrated_once_) + return; + + if (!symmetric_joint_target_requested_()) { + reset_chassis_imu_calibration_window_(); + return; + } + + const double raw_pitch = *chassis_imu_pitch_; + const double raw_roll = *chassis_imu_roll_; + if (!std::isfinite(raw_pitch) || !std::isfinite(raw_roll)) + return; + + chassis_imu_calibration_hold_elapsed_ += update_dt(); + if (chassis_imu_calibration_hold_elapsed_ < chassis_imu_calibration_wait_time_) + return; + + const double calibration_end_time = + chassis_imu_calibration_wait_time_ + chassis_imu_calibration_sample_time_; + if (chassis_imu_calibration_hold_elapsed_ < calibration_end_time) { + chassis_imu_pitch_sum_ += raw_pitch; + chassis_imu_roll_sum_ += raw_roll; + ++chassis_imu_calibration_sample_count_; + return; + } + + if (chassis_imu_calibration_completed_for_window_) + return; + + chassis_imu_calibration_completed_for_window_ = true; + if (chassis_imu_calibration_sample_count_ == 0) { + RCLCPP_WARN( + get_logger(), + "[chassis imu calibration] skipped because no valid samples were collected"); + return; + } + + chassis_imu_pitch_offset_ = std::clamp( + chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_), + -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); + chassis_imu_roll_offset_ = std::clamp( + chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_), + -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); + chassis_imu_calibrated_once_ = true; + RCLCPP_INFO( + get_logger(), + "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " + "(samples=%zu)", + chassis_imu_pitch_offset_ * rad_to_deg_, chassis_imu_roll_offset_ * rad_to_deg_, + chassis_imu_calibration_sample_count_); + } + bool ensure_joint_target_states_from_feedback( const std::array& current_physical_angles) { bool any_active = false; @@ -372,9 +570,70 @@ class DeformableChassis }); } + double active_suspension_min_angle_rad_() const { + return deg_to_rad(min_angle_ - 5.0); + } + + void update_active_suspension_() { + if (!suspension_requested_by_input_()) { + reset_attitude_correction_state_(); + return; + } + + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + const double base_target_angle = active_suspension_min_angle_rad_(); + const double max_target_angle = deg_to_rad(max_angle_); + const double corrected_pitch = + std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); + const double corrected_roll = + std::clamp(*chassis_imu_roll_ - chassis_imu_roll_offset_, -max_attitude, max_attitude); + const double corrected_pitch_rate = *chassis_imu_pitch_rate_; + const double corrected_roll_rate = *chassis_imu_roll_rate_; + + const double dt = update_dt(); + const double pitch_angle_diff = + pitch_pid_.update(-corrected_pitch, corrected_pitch_rate, dt); + const double roll_angle_diff = + roll_pid_.update(corrected_roll, -corrected_roll_rate, dt); + if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { + reset_attitude_correction_state_(); + current_target_physical_angles_rad_.fill(base_target_angle); + return; + } + + // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left + // pair. Every leg starts from the active-suspension minimum and only receives additive + // corrections so at least one leg always stays at that minimum. + const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); + const double back_pitch_add = std::max(pitch_angle_diff, 0.0); + const double left_roll_add = std::max(roll_angle_diff, 0.0); + const double right_roll_add = std::max(-roll_angle_diff, 0.0); + + current_target_physical_angles_rad_[kLeftFront] = + std::clamp( + base_target_angle + front_pitch_add + left_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kLeftBack] = + std::clamp( + base_target_angle + back_pitch_add + left_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kRightBack] = + std::clamp( + base_target_angle + back_pitch_add + right_roll_add, base_target_angle, + max_target_angle); + current_target_physical_angles_rad_[kRightFront] = + std::clamp( + base_target_angle + front_pitch_add + right_roll_add, base_target_angle, + max_target_angle); + + joint_suspension_active_.fill(true); + } + void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; + reset_attitude_correction_state_(); + reset_chassis_imu_calibration_window_(); chassis_control_velocity_->vector << nan_, nan_, nan_; *chassis_angle_ = nan_; @@ -394,12 +653,25 @@ class DeformableChassis deactivate_complex_spin_(); deactivate_qe_complex_spin_(); - for (size_t i = 0; i < kJointCount; ++i) { - *joint_target_physical_angle_[i] = nan_; - *joint_target_physical_velocity_[i] = nan_; - *joint_target_physical_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; - } + *scope_motor_control_torque = nan_; + + *lf_angle_error_ = nan_; + *lb_angle_error_ = nan_; + *rf_angle_error_ = nan_; + *rb_angle_error_ = nan_; + + *left_front_joint_target_physical_angle_ = nan_; + *left_back_joint_target_physical_angle_ = nan_; + *right_back_joint_target_physical_angle_ = nan_; + *right_front_joint_target_physical_angle_ = nan_; + *left_front_joint_target_physical_velocity_ = nan_; + *left_back_joint_target_physical_velocity_ = nan_; + *right_back_joint_target_physical_velocity_ = nan_; + *right_front_joint_target_physical_velocity_ = nan_; + *left_front_joint_target_physical_acceleration_ = nan_; + *left_back_joint_target_physical_acceleration_ = nan_; + *right_back_joint_target_physical_acceleration_ = nan_; + *right_front_joint_target_physical_acceleration_ = nan_; } @@ -430,13 +702,6 @@ class DeformableChassis } double update_angular_velocity_control() { - if (*ctrl_hold_active_ && std::isfinite(*manual_yaw_velocity_override_)) { - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = nan_; - return std::clamp( - *manual_yaw_velocity_override_, -angular_velocity_max_, angular_velocity_max_); - } - double angular_velocity = 0.0; double chassis_control_angle = nan_; @@ -552,6 +817,7 @@ class DeformableChassis // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. void run_joint_intent_pipeline_() { const auto current_physical_angles = read_current_joint_physical_angles_(); + const bool suspension_requested = suspension_requested_by_input_(); if (!ensure_joint_target_states_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); @@ -562,6 +828,13 @@ class DeformableChassis current_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); + if (suspension_requested) { + current_target_physical_angles_rad_.fill(active_suspension_min_angle_rad_()); + } + + update_chassis_imu_calibration_(); + scope_motor_control(suspension_requested); + update_active_suspension_(); update_joint_target_trajectory(); publish_joint_target_angles(current_physical_angles); @@ -569,6 +842,15 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + void scope_motor_control(bool suspension_requested = false) { + const bool prone_target_active = suspension_requested; + if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { + *scope_motor_control_torque = -0.3; + } else { + *scope_motor_control_torque = 0.3; + } + } + void update_joint_target_trajectory() { const double dt = update_dt(); for (size_t i = 0; i < kJointCount; ++i) { @@ -579,6 +861,12 @@ class DeformableChassis double& velocity_state = joint_target_physical_velocity_state_rad_[i]; double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; const double target_angle = current_target_physical_angles_rad_[i]; + const double velocity_limit = joint_suspension_active_[i] + ? suspension_velocity_limit_ + : target_physical_velocity_limit_; + const double acceleration_limit = + joint_suspension_active_[i] ? suspension_acceleration_limit_ + : target_physical_acceleration_limit_; if (!std::isfinite(target_angle) || !std::isfinite(angle_state)) { continue; @@ -586,25 +874,19 @@ class DeformableChassis const double position_error = target_angle - angle_state; const double stopping_distance = - velocity_state * velocity_state / (2.0 * target_physical_acceleration_limit_); + velocity_state * velocity_state / (2.0 * acceleration_limit); double desired_velocity = 0.0; if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = - std::copysign(target_physical_velocity_limit_, position_error); + desired_velocity = std::copysign(velocity_limit, position_error); } const double velocity_error = desired_velocity - velocity_state; acceleration_state = - std::clamp( - velocity_error / dt, -target_physical_acceleration_limit_, - target_physical_acceleration_limit_); + std::clamp(velocity_error / dt, -acceleration_limit, acceleration_limit); velocity_state += acceleration_state * dt; - velocity_state = - std::clamp( - velocity_state, -target_physical_velocity_limit_, - target_physical_velocity_limit_); + velocity_state = std::clamp(velocity_state, -velocity_limit, velocity_limit); angle_state += velocity_state * dt; const double next_error = target_angle - angle_state; @@ -625,34 +907,70 @@ class DeformableChassis return; } - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_target_active_[i]) { - *joint_target_physical_angle_[i] = nan_; - *joint_target_physical_velocity_[i] = nan_; - *joint_target_physical_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; - continue; + const auto publish_joint = [this, ¤t_physical_angles]( + size_t index, OutputInterface& angle_output, + OutputInterface& velocity_output, + OutputInterface& acceleration_output, + OutputInterface& angle_error_output) { + if (!joint_target_active_[index]) { + *angle_output = nan_; + *velocity_output = nan_; + *acceleration_output = nan_; + *angle_error_output = nan_; + return; } - *joint_target_physical_angle_[i] = joint_target_physical_angle_state_rad_[i]; - *joint_target_physical_velocity_[i] = joint_target_physical_velocity_state_rad_[i]; - *joint_target_physical_acceleration_[i] = joint_target_physical_acceleration_state_rad_[i]; - *joint_angle_error_[i] = std::isfinite(current_physical_angles[i]) - ? current_physical_angles[i] - - joint_target_physical_angle_state_rad_[i] - : nan_; - } + *angle_output = joint_target_physical_angle_state_rad_[index]; + *velocity_output = joint_target_physical_velocity_state_rad_[index]; + *acceleration_output = joint_target_physical_acceleration_state_rad_[index]; + *angle_error_output = std::isfinite(current_physical_angles[index]) + ? current_physical_angles[index] + - joint_target_physical_angle_state_rad_[index] + : nan_; + }; + + publish_joint( + kLeftFront, left_front_joint_target_physical_angle_, + left_front_joint_target_physical_velocity_, + left_front_joint_target_physical_acceleration_, lf_angle_error_); + publish_joint( + kLeftBack, left_back_joint_target_physical_angle_, + left_back_joint_target_physical_velocity_, + left_back_joint_target_physical_acceleration_, lb_angle_error_); + publish_joint( + kRightBack, right_back_joint_target_physical_angle_, + right_back_joint_target_physical_velocity_, + right_back_joint_target_physical_acceleration_, rb_angle_error_); + publish_joint( + kRightFront, right_front_joint_target_physical_angle_, + right_front_joint_target_physical_velocity_, + right_front_joint_target_physical_acceleration_, rf_angle_error_); } void publish_nan_joint_targets() { + reset_attitude_correction_state_(); joint_target_active_.fill(false); - for (size_t i = 0; i < kJointCount; ++i) { - *joint_target_physical_angle_[i] = nan_; - *joint_target_physical_velocity_[i] = nan_; - *joint_target_physical_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; - } + *left_front_joint_target_physical_angle_ = nan_; + *left_back_joint_target_physical_angle_ = nan_; + *right_back_joint_target_physical_angle_ = nan_; + *right_front_joint_target_physical_angle_ = nan_; + + *left_front_joint_target_physical_velocity_ = nan_; + *left_back_joint_target_physical_velocity_ = nan_; + *right_back_joint_target_physical_velocity_ = nan_; + *right_front_joint_target_physical_velocity_ = nan_; + + *left_front_joint_target_physical_acceleration_ = nan_; + *left_back_joint_target_physical_acceleration_ = nan_; + *right_back_joint_target_physical_acceleration_ = nan_; + *right_front_joint_target_physical_acceleration_ = nan_; + + *lf_angle_error_ = nan_; + *lb_angle_error_ = nan_; + *rb_angle_error_ = nan_; + *rf_angle_error_ = nan_; + } private: @@ -687,15 +1005,34 @@ class DeformableChassis const double spin_ratio_; const bool launch_ramp_shortcut_enabled_; - static constexpr std::array kJointNames = { - "left_front", "left_back", "right_back", "right_front"}; - std::array, kJointCount> joint_physical_angle_; - - std::array, kJointCount> joint_angle_error_; - - std::array, kJointCount> joint_target_physical_angle_; - std::array, kJointCount> joint_target_physical_velocity_; - std::array, kJointCount> joint_target_physical_acceleration_; + InputInterface left_front_joint_physical_angle_; + InputInterface left_back_joint_physical_angle_; + InputInterface right_front_joint_physical_angle_; + InputInterface right_back_joint_physical_angle_; + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + OutputInterface scope_motor_control_torque; + + OutputInterface lf_angle_error_; + OutputInterface lb_angle_error_; + OutputInterface rf_angle_error_; + OutputInterface rb_angle_error_; + + OutputInterface left_front_joint_target_physical_angle_; + OutputInterface left_back_joint_target_physical_angle_; + OutputInterface right_back_joint_target_physical_angle_; + OutputInterface right_front_joint_target_physical_angle_; + OutputInterface left_front_joint_target_physical_velocity_; + OutputInterface left_back_joint_target_physical_velocity_; + OutputInterface right_back_joint_target_physical_velocity_; + OutputInterface right_front_joint_target_physical_velocity_; + OutputInterface left_front_joint_target_physical_acceleration_; + OutputInterface left_back_joint_target_physical_acceleration_; + OutputInterface right_back_joint_target_physical_acceleration_; + OutputInterface right_front_joint_target_physical_acceleration_; double min_angle_; double max_angle_; @@ -717,10 +1054,34 @@ class DeformableChassis double target_physical_acceleration_limit_; bool active_suspension_enable_; bool suspension_on_by_switch = false; + double pitch_kp_; + double pitch_ki_; + double pitch_kd_; + double roll_kp_; + double roll_ki_; + double roll_kd_; + double suspension_velocity_limit_; + double suspension_acceleration_limit_; + double pitch_diff_limit_; + double roll_diff_limit_; + double pid_integral_limit_; + std::array joint_suspension_active_ = {false, false, false, false}; + AttitudePidAxis pitch_pid_; + AttitudePidAxis roll_pid_; + double chassis_imu_pitch_offset_ = 0.0; + double chassis_imu_roll_offset_ = 0.0; + double chassis_imu_calibration_wait_time_; + double chassis_imu_calibration_sample_time_; + double chassis_imu_calibration_hold_elapsed_ = 0.0; + size_t chassis_imu_calibration_sample_count_ = 0; + double chassis_imu_pitch_sum_ = 0.0; + double chassis_imu_roll_sum_ = 0.0; + bool chassis_imu_calibration_completed_for_window_ = false; + bool chassis_imu_calibrated_once_ = false; static constexpr double default_dt_ = 1e-3; }; } // namespace rmcs_core::controller::chassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 5b4baba6..875920ed 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -57,11 +57,10 @@ class DeformableInfantryGimbalController if (ctrl_hold_requested()) { update_ctrl_hold_control(); - return; + } else { + deactivate_ctrl_hold(); } - deactivate_ctrl_hold(); - const auto auto_aim_active = auto_aim_requested() && input_.auto_aim_should_control.ready() && *input_.auto_aim_should_control && input_.auto_aim_control_direction.ready() @@ -71,44 +70,61 @@ class DeformableInfantryGimbalController auto_aim_active ? update_auto_aim_control() : update_manual_control(); *output_.yaw_angle_error = angle_error.yaw_angle_error; - *output_.pitch_angle_error = angle_error.pitch_angle_error; + if (!ctrl_hold_active_) + *output_.pitch_angle_error = angle_error.pitch_angle_error; - if (!std::isfinite(angle_error.yaw_angle_error) - || !std::isfinite(angle_error.pitch_angle_error)) { - reset_control_outputs(); - return; + if (!std::isfinite(angle_error.yaw_angle_error)) { + yaw_angle_pid_.reset(); + yaw_velocity_pid_.reset(); + *output_.yaw_control_torque = kNaN; } const auto feedforward_enabled = auto_aim_active && input_.auto_aim_feedforward_valid.ready() && *input_.auto_aim_feedforward_valid; - const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() - && std::isfinite(*input_.auto_aim_yaw_rate) - ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate - : 0.0; - const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() - && std::isfinite(*input_.auto_aim_yaw_acc) - ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc - : 0.0; - const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() - && std::isfinite(*input_.auto_aim_pitch_acc) - ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc - : 0.0; - - const auto yaw_velocity_ref = - yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; - const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); - - *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; - if (pitch_torque_control_enabled_) { - *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_acc_ff; - } else { - pitch_velocity_pid_.reset(); - *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + + if (std::isfinite(angle_error.yaw_angle_error)) { + const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() + && std::isfinite(*input_.auto_aim_yaw_rate) + ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate + : 0.0; + const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() + && std::isfinite(*input_.auto_aim_yaw_acc) + ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc + : 0.0; + + const auto yaw_velocity_ref = + yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; + *output_.yaw_control_torque = + yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu) + yaw_acc_ff; + } + + if (!ctrl_hold_active_) { + if (!std::isfinite(angle_error.pitch_angle_error)) { + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = kNaN; + } else { + const auto pitch_acc_ff = feedforward_enabled + && input_.auto_aim_pitch_acc.ready() + && std::isfinite(*input_.auto_aim_pitch_acc) + ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc + : 0.0; + const auto pitch_velocity_ref = + pitch_angle_pid_.update(angle_error.pitch_angle_error); + + if (pitch_torque_control_enabled_) { + *output_.pitch_control_velocity = kNaN; + *output_.pitch_control_torque = + pitch_velocity_pid_.update( + pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_acc_ff; + } else { + pitch_velocity_pid_.reset(); + *output_.pitch_control_velocity = pitch_velocity_ref; + *output_.pitch_control_torque = kNaN; + } + } } } @@ -249,14 +265,8 @@ class DeformableInfantryGimbalController auto activate_ctrl_hold() -> void { ctrl_hold_active_ = true; - locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) - ? *input_.yaw_angle - : 0.0; - yaw_angle_pid_.reset(); - yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } auto deactivate_ctrl_hold() -> void { @@ -264,42 +274,19 @@ class DeformableInfantryGimbalController return; ctrl_hold_active_ = false; - locked_yaw_angle_ = kNaN; - yaw_angle_pid_.reset(); - yaw_velocity_pid_.reset(); - *output_.yaw_control_angle = kNaN; + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; - gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); } auto update_ctrl_hold_control() -> void { if (!ctrl_hold_active_) activate_ctrl_hold(); - *output_.yaw_control_torque = kNaN; *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = kNaN; *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; - *output_.yaw_angle_error = kNaN; - *output_.pitch_angle_error = kNaN; - *output_.pitch_control_angle = kNaN; - - if (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle) && input_.yaw_velocity.ready() - && std::isfinite(*input_.yaw_velocity)) { - auto yaw_target_error = locked_yaw_angle_ - *input_.yaw_angle; - if (yaw_target_error > std::numbers::pi) - yaw_target_error -= 2 * std::numbers::pi; - else if (yaw_target_error < -std::numbers::pi) - yaw_target_error += 2 * std::numbers::pi; - - *output_.yaw_angle_error = yaw_target_error; - const auto yaw_velocity_ref = yaw_angle_pid_.update(yaw_target_error); - *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity); - } if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; @@ -320,11 +307,6 @@ class DeformableInfantryGimbalController *output_.pitch_control_torque = kNaN; } } - - const auto yaw_velocity_override = std::clamp( - manual_yaw_shift() / update_dt(), -ctrl_hold_chassis_yaw_velocity_max_, - ctrl_hold_chassis_yaw_velocity_max_); - *output_.chassis_manual_yaw_velocity_override = yaw_velocity_override; } auto reset_control_outputs() -> void { @@ -383,7 +365,6 @@ class DeformableInfantryGimbalController bool suspension_on_by_switch_ = false; bool ctrl_hold_active_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - double locked_yaw_angle_ = kNaN; }; } // namespace rmcs_core::controller::gimbal From 65a8480aa3b93e593233fd5a6ff528227eec41d6 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 6 Jul 2026 00:52:32 +0800 Subject: [PATCH 68/90] feat(chassis): enhance active suspension control with inner and outer PID parameters --- .../config/deformable-infantry-omni-b.yaml | 54 +++-- .../config/deformable-infantry-omni.yaml | 54 +++-- .../config/deformable-infantry-steering.yaml | 44 +++- .../controller/chassis/deformable_chassis.cpp | 222 ++++++++++-------- .../chassis/deformable_joint_controller.cpp | 24 +- 5 files changed, 266 insertions(+), 132 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 89b4ef02..21cbb86d 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -64,21 +64,43 @@ chassis_controller: spin_ratio: 1.0 # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_pitch_outer_kp: 8.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 8.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -172,11 +194,11 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 23.0 + mass: 25.5 moment_of_inertia: 1.0 chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 + rod_length: 0.140 + wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -187,6 +209,7 @@ lf_joint_controller: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque dt: 0.001 @@ -210,6 +233,7 @@ lb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -232,6 +256,7 @@ rb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -254,6 +279,7 @@ rf_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a5877e2c..4ac8e02c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -62,21 +62,43 @@ chassis_controller: spin_ratio: 1.0 # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 - - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 - - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_pitch_outer_kp: 12.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 + + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 + + active_suspension_roll_outer_kp: 12.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -170,11 +192,11 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mass: 22.5 + mass: 25.5 moment_of_inertia: 1.0 chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.055 + rod_length: 0.140 + wheel_radius: 0.075 friction_coefficient: 6.6 k1: 2.958580e+00 k2: 3.082190e-03 @@ -185,6 +207,7 @@ lf_joint_controller: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque dt: 0.001 @@ -208,6 +231,7 @@ lb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -230,6 +254,7 @@ rb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -252,6 +277,7 @@ rf_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml index 63b00d67..acc8ebd7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml @@ -61,21 +61,43 @@ chassis_controller: spin_ratio: 1.0 # IMU attitude correction at min-angle stance. - active_suspension_pitch_kp: 8.0 - active_suspension_pitch_ki: 0.35 - active_suspension_pitch_kd: 0.28 + active_suspension_pitch_outer_kp: 8.0 + active_suspension_pitch_outer_ki: 0.02 + active_suspension_pitch_outer_kd: 0.0 + active_suspension_pitch_outer_integral_min: -2.0 + active_suspension_pitch_outer_integral_max: 2.0 + active_suspension_pitch_outer_output_min: -3.0 + active_suspension_pitch_outer_output_max: 3.0 - active_suspension_roll_kp: 8.0 - active_suspension_roll_ki: 0.35 - active_suspension_roll_kd: 0.28 + active_suspension_pitch_inner_kp: 0.45 + active_suspension_pitch_inner_ki: 0.0 + active_suspension_pitch_inner_kd: 0.0 + active_suspension_pitch_inner_integral_min: -1.0 + active_suspension_pitch_inner_integral_max: 1.0 + active_suspension_pitch_inner_output_min: -0.785 + active_suspension_pitch_inner_output_max: 0.785 - active_suspension_pitch_angle_diff_limit_deg: 45.0 - active_suspension_roll_angle_diff_limit_deg: 45.0 - active_suspension_pid_integral_limit_deg: 20.0 + active_suspension_roll_outer_kp: 8.0 + active_suspension_roll_outer_ki: 0.02 + active_suspension_roll_outer_kd: 0.0 + active_suspension_roll_outer_integral_min: -2.0 + active_suspension_roll_outer_integral_max: 2.0 + active_suspension_roll_outer_output_min: -3.0 + active_suspension_roll_outer_output_max: 3.0 + + active_suspension_roll_inner_kp: 0.45 + active_suspension_roll_inner_ki: 0.0 + active_suspension_roll_inner_kd: 0.0 + active_suspension_roll_inner_integral_min: -1.0 + active_suspension_roll_inner_integral_max: 1.0 + active_suspension_roll_inner_output_min: -0.785 + active_suspension_roll_inner_output_max: 0.785 # Chassis-owned joint intent trajectory limits while attitude correction is active. active_suspension_target_velocity_limit_deg: 80.0 active_suspension_target_acceleration_limit_deg: 360.0 + active_suspension_correction_velocity_limit_deg: 720.0 + active_suspension_correction_acceleration_limit_deg: 3600.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. @@ -177,6 +199,7 @@ lf_joint_controller: # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque dt: 0.001 @@ -200,6 +223,7 @@ lb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle + setpoint_velocity: /chassis/left_back_joint/target_physical_velocity control: /chassis/left_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -222,6 +246,7 @@ rb_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle + setpoint_velocity: /chassis/right_back_joint/target_physical_velocity control: /chassis/right_back_joint/control_torque dt: 0.001 b0: -1.0 @@ -244,6 +269,7 @@ rf_joint_controller: # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle + setpoint_velocity: /chassis/right_front_joint/target_physical_velocity control: /chassis/right_front_joint/control_torque dt: 0.001 b0: -1.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index b4a72628..4b25a86c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -33,28 +34,6 @@ class DeformableChassis kJointCount = 4, }; - struct AttitudePidAxis { - double kp = 20.0; - double ki = 0.0; - double kd = 0.0; - double integral = 0.0; - double integral_limit = std::numeric_limits::infinity(); - double output_limit = std::numeric_limits::infinity(); - - void reset() { integral = 0.0; } - - double update(double error, double rate, double dt) { - if (!std::isfinite(error) || !std::isfinite(rate) || !std::isfinite(dt) || dt <= 0.0) { - reset(); - return std::numeric_limits::quiet_NaN(); - } - - integral = std::clamp(integral + error * dt, -integral_limit, integral_limit); - const double output = kp * error + ki * integral - kd * rate; - return std::clamp(output, -output_limit, output_limit); - } - }; - DeformableChassis() : Node( get_component_name(), @@ -75,12 +54,6 @@ class DeformableChassis std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6)) , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) - , pitch_kp_(get_parameter_or("active_suspension_pitch_kp", 200.0)) - , pitch_ki_(get_parameter_or("active_suspension_pitch_ki", 0.0)) - , pitch_kd_(get_parameter_or("active_suspension_pitch_kd", 20.0)) - , roll_kp_(get_parameter_or("active_suspension_roll_kp", 200.0)) - , roll_ki_(get_parameter_or("active_suspension_roll_ki", 0.0)) - , roll_kd_(get_parameter_or("active_suspension_roll_kd", 20.0)) , suspension_velocity_limit_( std::max( deg_to_rad( @@ -95,18 +68,16 @@ class DeformableChassis "active_suspension_target_acceleration_limit_deg", get_parameter_or("target_physical_acceleration_limit", 720.0)))), 1e-6)) - , pitch_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_pitch_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , roll_diff_limit_( - std::abs(get_parameter_or( - "active_suspension_roll_angle_diff_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) - , pid_integral_limit_( - std::abs(get_parameter_or( - "active_suspension_pid_integral_limit_deg", max_angle_ - min_angle_)) - * std::numbers::pi / 180.0) + , suspension_correction_velocity_limit_( + std::max( + deg_to_rad(std::abs(get_parameter_or( + "active_suspension_correction_velocity_limit_deg", 720.0))), + 1e-6)) + , suspension_correction_acceleration_limit_( + std::max( + deg_to_rad(std::abs(get_parameter_or( + "active_suspension_correction_acceleration_limit_deg", 3600.0))), + 1e-6)) , chassis_imu_calibration_wait_time_( std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) , chassis_imu_calibration_sample_time_( @@ -114,16 +85,18 @@ class DeformableChassis following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; - pitch_pid_.kp = pitch_kp_; - pitch_pid_.ki = pitch_ki_; - pitch_pid_.kd = pitch_kd_; - pitch_pid_.integral_limit = pid_integral_limit_; - pitch_pid_.output_limit = pitch_diff_limit_; - roll_pid_.kp = roll_kp_; - roll_pid_.ki = roll_ki_; - roll_pid_.kd = roll_kd_; - roll_pid_.integral_limit = pid_integral_limit_; - roll_pid_.output_limit = roll_diff_limit_; + load_active_suspension_pid_( + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, + -2.0, 2.0, -3.0, 3.0); + load_active_suspension_pid_( + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, + -1.0, 1.0, -0.785, 0.785); + load_active_suspension_pid_( + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, + -2.0, 2.0, -3.0, 3.0); + load_active_suspension_pid_( + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, + -1.0, 1.0, -0.785, 0.785); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); @@ -269,6 +242,19 @@ class DeformableChassis static constexpr double imu_calibration_offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + void load_active_suspension_pid_( + const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, + double kd_default, double integral_min_default, double integral_max_default, + double output_min_default, double output_max_default) { + pid.kp = get_parameter_or(prefix + "kp", kp_default); + pid.ki = get_parameter_or(prefix + "ki", ki_default); + pid.kd = get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = get_parameter_or(prefix + "output_max", output_max_default); + } + void validate_joint_feedback_inputs() const { if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) @@ -479,9 +465,19 @@ class DeformableChassis } void reset_attitude_correction_state_() { - pitch_pid_.reset(); - roll_pid_.reset(); + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); joint_suspension_active_.fill(false); + suspension_correction_target_rad_.fill(0.0); + } + + void reset_active_suspension_correction_state_() { + suspension_correction_target_rad_.fill(0.0); + suspension_correction_state_rad_.fill(0.0); + suspension_correction_velocity_state_rad_.fill(0.0); + suspension_correction_acceleration_state_rad_.fill(0.0); } void reset_chassis_imu_calibration_window_() { @@ -581,8 +577,6 @@ class DeformableChassis } constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - const double base_target_angle = active_suspension_min_angle_rad_(); - const double max_target_angle = deg_to_rad(max_angle_); const double corrected_pitch = std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); const double corrected_roll = @@ -590,14 +584,12 @@ class DeformableChassis const double corrected_pitch_rate = *chassis_imu_pitch_rate_; const double corrected_roll_rate = *chassis_imu_roll_rate_; - const double dt = update_dt(); - const double pitch_angle_diff = - pitch_pid_.update(-corrected_pitch, corrected_pitch_rate, dt); - const double roll_angle_diff = - roll_pid_.update(corrected_roll, -corrected_roll_rate, dt); + const double pitch_outer = pitch_outer_pid_.update(-corrected_pitch); + const double roll_outer = roll_outer_pid_.update(corrected_roll); + const double pitch_angle_diff = pitch_inner_pid_.update(pitch_outer - corrected_pitch_rate); + const double roll_angle_diff = roll_inner_pid_.update(roll_outer + corrected_roll_rate); if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { reset_attitude_correction_state_(); - current_target_physical_angles_rad_.fill(base_target_angle); return; } @@ -609,26 +601,63 @@ class DeformableChassis const double left_roll_add = std::max(roll_angle_diff, 0.0); const double right_roll_add = std::max(-roll_angle_diff, 0.0); - current_target_physical_angles_rad_[kLeftFront] = - std::clamp( - base_target_angle + front_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kLeftBack] = - std::clamp( - base_target_angle + back_pitch_add + left_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightBack] = - std::clamp( - base_target_angle + back_pitch_add + right_roll_add, base_target_angle, - max_target_angle); - current_target_physical_angles_rad_[kRightFront] = - std::clamp( - base_target_angle + front_pitch_add + right_roll_add, base_target_angle, - max_target_angle); + suspension_correction_target_rad_[kLeftFront] = front_pitch_add + left_roll_add; + suspension_correction_target_rad_[kLeftBack] = back_pitch_add + left_roll_add; + suspension_correction_target_rad_[kRightBack] = back_pitch_add + right_roll_add; + suspension_correction_target_rad_[kRightFront] = front_pitch_add + right_roll_add; joint_suspension_active_.fill(true); } + void update_active_suspension_correction_trajectory() { + const double dt = update_dt(); + const double max_target_angle = deg_to_rad(max_angle_); + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_suspension_active_[i]) { + suspension_correction_target_rad_[i] = 0.0; + } + + const double base_angle = std::isfinite(joint_target_physical_angle_state_rad_[i]) + ? joint_target_physical_angle_state_rad_[i] + : active_suspension_min_angle_rad_(); + const double correction_max = std::max(max_target_angle - base_angle, 0.0); + const double target = std::clamp( + suspension_correction_target_rad_[i], 0.0, correction_max); + + double& angle_state = suspension_correction_state_rad_[i]; + double& velocity_state = suspension_correction_velocity_state_rad_[i]; + double& acceleration_state = suspension_correction_acceleration_state_rad_[i]; + const double position_error = target - angle_state; + const double stopping_distance = + velocity_state * velocity_state / (2.0 * suspension_correction_acceleration_limit_); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { + desired_velocity = std::copysign(suspension_correction_velocity_limit_, position_error); + } + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = std::clamp( + velocity_error / dt, -suspension_correction_acceleration_limit_, + suspension_correction_acceleration_limit_); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp( + velocity_state, -suspension_correction_velocity_limit_, + suspension_correction_velocity_limit_); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; @@ -649,6 +678,7 @@ class DeformableChassis joint_target_physical_angle_state_rad_.fill(nan_); joint_target_physical_velocity_state_rad_.fill(0.0); joint_target_physical_acceleration_state_rad_.fill(0.0); + reset_active_suspension_correction_state_(); suspension_on_by_switch = false; deactivate_complex_spin_(); deactivate_qe_complex_spin_(); @@ -837,6 +867,7 @@ class DeformableChassis update_active_suspension_(); update_joint_target_trajectory(); + update_active_suspension_correction_trajectory(); publish_joint_target_angles(current_physical_angles); } @@ -920,12 +951,17 @@ class DeformableChassis return; } - *angle_output = joint_target_physical_angle_state_rad_[index]; - *velocity_output = joint_target_physical_velocity_state_rad_[index]; - *acceleration_output = joint_target_physical_acceleration_state_rad_[index]; + const double target_angle = joint_target_physical_angle_state_rad_[index] + + suspension_correction_state_rad_[index]; + *angle_output = std::clamp( + target_angle, active_suspension_min_angle_rad_(), deg_to_rad(max_angle_)); + *velocity_output = joint_target_physical_velocity_state_rad_[index] + + suspension_correction_velocity_state_rad_[index]; + *acceleration_output = joint_target_physical_acceleration_state_rad_[index] + + suspension_correction_acceleration_state_rad_[index]; *angle_error_output = std::isfinite(current_physical_angles[index]) ? current_physical_angles[index] - - joint_target_physical_angle_state_rad_[index] + - *angle_output : nan_; }; @@ -949,6 +985,7 @@ class DeformableChassis void publish_nan_joint_targets() { reset_attitude_correction_state_(); + reset_active_suspension_correction_state_(); joint_target_active_.fill(false); *left_front_joint_target_physical_angle_ = nan_; @@ -1054,20 +1091,21 @@ class DeformableChassis double target_physical_acceleration_limit_; bool active_suspension_enable_; bool suspension_on_by_switch = false; - double pitch_kp_; - double pitch_ki_; - double pitch_kd_; - double roll_kp_; - double roll_ki_; - double roll_kd_; double suspension_velocity_limit_; double suspension_acceleration_limit_; - double pitch_diff_limit_; - double roll_diff_limit_; - double pid_integral_limit_; + double suspension_correction_velocity_limit_; + double suspension_correction_acceleration_limit_; std::array joint_suspension_active_ = {false, false, false, false}; - AttitudePidAxis pitch_pid_; - AttitudePidAxis roll_pid_; + std::array suspension_correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array suspension_correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array suspension_correction_velocity_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array suspension_correction_acceleration_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; double chassis_imu_pitch_offset_ = 0.0; double chassis_imu_roll_offset_ = 0.0; double chassis_imu_calibration_wait_time_; @@ -1084,4 +1122,4 @@ class DeformableChassis } // namespace rmcs_core::controller::chassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index 14623175..830f5eb1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -45,6 +45,7 @@ class DeformableJointController struct InputSnapshot { double measurement_angle = std::numeric_limits::quiet_NaN(); double setpoint_angle = std::numeric_limits::quiet_NaN(); + double setpoint_velocity = std::numeric_limits::quiet_NaN(); }; DeformableJointController() @@ -77,6 +78,10 @@ class DeformableJointController void register_interfaces_() { register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); + if (has_parameter("setpoint_velocity")) { + register_input(get_parameter("setpoint_velocity").as_string(), setpoint_velocity_, false); + use_setpoint_velocity_ = true; + } register_output(get_parameter("control").as_string(), control_torque_, nan_); } @@ -124,6 +129,10 @@ class DeformableJointController bool read_inputs_(InputSnapshot& inputs) const { inputs.measurement_angle = *measurement_angle_; inputs.setpoint_angle = *setpoint_angle_; + if (use_setpoint_velocity_ && setpoint_velocity_.ready() + && std::isfinite(*setpoint_velocity_)) { + inputs.setpoint_velocity = *setpoint_velocity_; + } return std::isfinite(inputs.measurement_angle) && std::isfinite(inputs.setpoint_angle); } @@ -142,11 +151,18 @@ class DeformableJointController } bool run_joint_servo_(const InputSnapshot& inputs, double& control_torque) { - const auto td_out = td_.update(inputs.setpoint_angle); const auto eso_out = eso_.update(inputs.measurement_angle, last_u_); - const double e1 = td_out.x1 - eso_out.z1; - const double e2 = td_out.x2 - eso_out.z2; + double reference_angle = inputs.setpoint_angle; + double reference_velocity = inputs.setpoint_velocity; + if (!std::isfinite(reference_velocity)) { + const auto td_out = td_.update(inputs.setpoint_angle); + reference_angle = td_out.x1; + reference_velocity = td_out.x2; + } + + const double e1 = reference_angle - eso_out.z1; + const double e2 = reference_velocity - eso_out.z2; control_torque = kt_ * nlesf_.compute(e1, e2, eso_out.z3, b0_).u; control_torque = std::clamp(control_torque, config_.output_min, config_.output_max); @@ -174,6 +190,7 @@ class DeformableJointController InputInterface measurement_angle_; InputInterface setpoint_angle_; + InputInterface setpoint_velocity_; OutputInterface control_torque_; @@ -187,6 +204,7 @@ class DeformableJointController double b0_ = 1.0; double kt_ = 1.0; double last_u_ = 0.0; + bool use_setpoint_velocity_ = false; bool initialized_ = false; }; From bec3e2c6515c86293bf10b85ba3c0a9da180bab9 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 6 Jul 2026 22:31:48 +0800 Subject: [PATCH 69/90] Remove DeformableChassisController implementation This commit deletes the DeformableChassisController class implementation from the deformable_wheel_controller.cpp file. The removal includes all associated methods, member variables, and dependencies, streamlining the codebase by eliminating unused components. --- .../config/deformable-infantry-steering.yaml | 289 ------- .../chassis/deformable_wheel_controller.cpp | 770 ------------------ 2 files changed, 1059 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml deleted file mode 100644 index acc8ebd7..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-steering.yaml +++ /dev/null @@ -1,289 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::DeformableInfantryV2 -> deformable_infantry - - - 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::DeformableInfantry -> referee_ui_infantry - - - rmcs_core::controller::gimbal::DeformableInfantryGimbalController -> gimbal_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::DeformableChassis -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::DeformableChassisController -> deformable_chassis_controller - - - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - -value_broadcaster: - ros__parameters: - forward_list: - - /gimbal/yaw/angle - - /gimbal/yaw/velocity - - -deformable_infantry: - ros__parameters: - serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" - serial_filter_top_board: "AF-C26A-0C9C-CF41-3E3C-1596-524B-7527-5744" - left_front_zero_point: 7173 - left_back_zero_point: 5167 - right_back_zero_point: 3098 - right_front_zero_point: 6485 - yaw_motor_zero_point: 39442 - pitch_motor_zero_point: 56556 - debug_log_supercap: false - debug_log_wheel_motor: false - debug_log_deformable_joint_motor: false - -chassis_controller: - ros__parameters: - # Deploy geometry / chassis-owned joint intent - min_angle: 20.0 - max_angle: 50.0 - active_suspension_enable: true - spin_ratio: 1.0 - - # IMU attitude correction at min-angle stance. - active_suspension_pitch_outer_kp: 8.0 - active_suspension_pitch_outer_ki: 0.02 - active_suspension_pitch_outer_kd: 0.0 - active_suspension_pitch_outer_integral_min: -2.0 - active_suspension_pitch_outer_integral_max: 2.0 - active_suspension_pitch_outer_output_min: -3.0 - active_suspension_pitch_outer_output_max: 3.0 - - active_suspension_pitch_inner_kp: 0.45 - active_suspension_pitch_inner_ki: 0.0 - active_suspension_pitch_inner_kd: 0.0 - active_suspension_pitch_inner_integral_min: -1.0 - active_suspension_pitch_inner_integral_max: 1.0 - active_suspension_pitch_inner_output_min: -0.785 - active_suspension_pitch_inner_output_max: 0.785 - - active_suspension_roll_outer_kp: 8.0 - active_suspension_roll_outer_ki: 0.02 - active_suspension_roll_outer_kd: 0.0 - active_suspension_roll_outer_integral_min: -2.0 - active_suspension_roll_outer_integral_max: 2.0 - active_suspension_roll_outer_output_min: -3.0 - active_suspension_roll_outer_output_max: 3.0 - - active_suspension_roll_inner_kp: 0.45 - active_suspension_roll_inner_ki: 0.0 - active_suspension_roll_inner_kd: 0.0 - active_suspension_roll_inner_integral_min: -1.0 - active_suspension_roll_inner_integral_max: 1.0 - active_suspension_roll_inner_output_min: -0.785 - active_suspension_roll_inner_output_max: 0.785 - - # Chassis-owned joint intent trajectory limits while attitude correction is active. - active_suspension_target_velocity_limit_deg: 80.0 - active_suspension_target_acceleration_limit_deg: 360.0 - active_suspension_correction_velocity_limit_deg: 720.0 - active_suspension_correction_acceleration_limit_deg: 3600.0 - - # Automatic IMU mounting-error calibration. - # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. - chassis_imu_calibration_wait_s: 2.0 - chassis_imu_calibration_sample_s: 3.0 - -gimbal_controller: - ros__parameters: - upper_limit: -0.65 # -35 deg - lower_limit: 0.05 # 6 deg - ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 - - yaw_angle_kp: 10.0 - yaw_angle_ki: 0.0 - yaw_angle_kd: 0.0 - - yaw_velocity_kp: 8.0 - yaw_velocity_ki: 0.0 - yaw_velocity_kd: 0.0 - - pitch_angle_kp: 40.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - - pitch_velocity_kp: 3.0 - pitch_velocity_ki: 0.0 - pitch_velocity_kd: 0.0 - - pitch_torque_control: true - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/left_friction - - /gimbal/right_friction - friction_velocities: - - 580.0 - - 580.0 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 10 - reserved_heat: 0 - -bullet_feeder_controller: - ros__parameters: - bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 - safe_shot_frequency: 10.0 - eject_frequency: 10.0 - eject_time: 0.05 - deep_eject_frequency: 5.0 - deep_eject_time: 0.2 - 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.00 - 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.00 - 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.4 - ki: 0.0 - kd: 0.0 - -deformable_chassis_controller: - ros__parameters: - mass: 23.0 - moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 - wheel_radius: 0.07 - friction_coefficient: 0.6 - k1: 2.958580e+00 - k2: 3.082190e-03 - no_load_power: 11.37 - -lf_joint_controller: - ros__parameters: - # Joint-local servo inputs produced by chassis intent generation - measurement_angle: /chassis/left_front_joint/physical_angle - setpoint_angle: /chassis/left_front_joint/target_physical_angle - setpoint_velocity: /chassis/left_front_joint/target_physical_velocity - control: /chassis/left_front_joint/control_torque - - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -lb_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/left_back_joint/physical_angle - setpoint_angle: /chassis/left_back_joint/target_physical_angle - setpoint_velocity: /chassis/left_back_joint/target_physical_velocity - control: /chassis/left_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -rb_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/right_back_joint/physical_angle - setpoint_angle: /chassis/right_back_joint/target_physical_angle - setpoint_velocity: /chassis/right_back_joint/target_physical_velocity - control: /chassis/right_back_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 -rf_joint_controller: - ros__parameters: - # Same joint-servo layout as lf_joint_controller - measurement_angle: /chassis/right_front_joint/physical_angle - setpoint_angle: /chassis/right_front_joint/target_physical_angle - setpoint_velocity: /chassis/right_front_joint/target_physical_velocity - control: /chassis/right_front_joint/control_torque - dt: 0.001 - b0: -1.0 - kt: 1.0 - td_h: 0.001 - td_r: 50.0 - eso_w0: 250.0 - eso_auto_beta: true - k1: 30.0 - k2: 17.0 - alpha1: 0.75 - alpha2: 0.7 - delta: 0.02 - u_min: -200.0 - u_max: 200.0 - output_min: -200.0 - output_max: 200.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp deleted file mode 100644 index 1c02993a..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp +++ /dev/null @@ -1,770 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "controller/chassis/qcp_solver.hpp" -#include "controller/pid/matrix_pid_calculator.hpp" -#include "controller/pid/pid_calculator.hpp" -#include "filter/low_pass_filter.hpp" - -namespace rmcs_core::controller::chassis { - -class DeformableChassisController - : public rmcs_executor::Component - , public rclcpp::Node { - - enum class WheelIndex : size_t { - LeftFront = 0, - LeftBack = 1, - RightBack = 2, - RightFront = 3, - Count = 4 - }; - - static constexpr size_t kWheelCount = static_cast(WheelIndex::Count); - - struct EllipseParameters { - double a, b, c, d, e, f; - }; - -public: - explicit DeformableChassisController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , mass_(get_parameter("mass").as_double()) - , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) - , chassis_radius_(get_parameter("chassis_radius").as_double()) - , rod_length_(get_parameter("rod_length").as_double()) - , wheel_radius_(get_parameter("wheel_radius").as_double()) - , friction_coefficient_(get_parameter("friction_coefficient").as_double()) - , k1_(get_parameter("k1").as_double()) - , k2_(get_parameter("k2").as_double()) - , no_load_power_(get_parameter("no_load_power").as_double()) - , ellipse_coeff_quadratic_translational_( - k1_ * mass_ * mass_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_cross_term_( - k1_ * mass_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 8.0) - , ellipse_coeff_quadratic_angular_( - k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_ / 16.0) - , ellipse_coeff_linear_translational_(mass_ * wheel_radius_ / 4.0) - , ellipse_coeff_linear_angular_(moment_of_inertia_ * wheel_radius_ / 4.0) - , vehicle_radius_(Eigen::Vector4d::Constant(chassis_radius_ + rod_length_)) - , control_acceleration_filter_(5.0, 1000.0) - , chassis_velocity_expected_(Eigen::Vector3d::Zero()) - , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) - , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) - , steering_velocity_pid_(0.15, 0.0, 0.0) - , steering_angle_pid_(30.0, 0.0, 0.0) - , wheel_velocity_pid_(0.6, 0.0, 0.0) { - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - - for (size_t i = 0; i < kWheelCount; ++i) { - register_input( - fmt::format("/chassis/{}_steering/angle", kJointNames[i]), - steering_angle_[i]); - register_input( - fmt::format("/chassis/{}_steering/velocity", kJointNames[i]), - steering_velocity_[i]); - register_input( - fmt::format("/chassis/{}_wheel/velocity", kJointNames[i]), - wheel_velocity_[i]); - register_input( - fmt::format("/chassis/{}_joint/physical_angle", kJointNames[i]), - joint_angle_[i]); - register_input( - fmt::format("/chassis/{}_joint/physical_velocity", kJointNames[i]), - joint_velocity_[i]); - register_input( - fmt::format("/chassis/{}_joint/target_physical_angle", kJointNames[i]), - joint_target_physical_angle_[i], false); - register_input( - fmt::format("/chassis/{}_joint/target_physical_velocity", kJointNames[i]), - joint_target_physical_velocity_[i], false); - register_input( - fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointNames[i]), - joint_target_physical_acceleration_[i], false); - } - - register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); - register_input("/chassis/control_velocity", chassis_control_velocity_); - register_input("/chassis/control_power_limit", power_limit_); - - for (size_t i = 0; i < kWheelCount; ++i) { - register_output( - fmt::format("/chassis/{}_steering/control_torque", kJointNames[i]), - steering_control_torque_[i]); - register_output( - fmt::format("/chassis/{}_wheel/control_torque", kJointNames[i]), - wheel_control_torque_[i]); - } - - } - - void update() override { - if (std::isnan(chassis_control_velocity_->vector[0])) { - reset_all_controls(); - return; - } - - const JointFeedbackStates joint_feedback = update_joint_feedback_states_(); - const JointTargetStates joint_target = update_joint_target_states_(); - if (joint_feedback.valid) { - vehicle_radius_ = joint_feedback.radius; - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 1000, - "physical joint angle[deg] lf=%.2f lb=%.2f rb=%.2f rf=%.2f, radius[m] lf=%.3f " - "lb=%.3f rb=%.3f rf=%.3f", - joint_feedback.alpha_rad[0] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[1] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[2] * 180.0 / std::numbers::pi, - joint_feedback.alpha_rad[3] * 180.0 / std::numbers::pi, vehicle_radius_[0], - vehicle_radius_[1], vehicle_radius_[2], vehicle_radius_[3]); - } - - integral_yaw_angle_imu(); - - const auto steering_status = calculate_steering_status(); - const auto wheel_velocities = calculate_wheel_velocities(); - const auto chassis_velocity = - calculate_chassis_velocity(steering_status, wheel_velocities, joint_feedback); - auto chassis_status_expected = - calculate_chassis_status_expected(chassis_velocity, joint_target, joint_feedback); - const auto chassis_control_velocity = calculate_chassis_control_velocity(); - const auto chassis_acceleration = calculate_chassis_control_acceleration( - chassis_status_expected.velocity, chassis_control_velocity); - const double power_limit = - *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); - const auto wheel_pid_torques = - calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); - const auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( - steering_status, wheel_velocities, joint_target, chassis_acceleration, - wheel_pid_torques, power_limit); - const auto filtered_chassis_acceleration = - odom_to_base_link_vector(control_acceleration_filter_.update( - base_link_to_odom_vector(constrained_chassis_acceleration))); - const auto steering_torques = calculate_steering_control_torques( - steering_status, chassis_status_expected, joint_target, joint_feedback, - filtered_chassis_acceleration); - const auto wheel_torques = calculate_wheel_control_torques( - steering_status, joint_target, joint_feedback, filtered_chassis_acceleration, - wheel_pid_torques); - - update_control_torques(steering_torques, wheel_torques); - update_chassis_velocity_expected(filtered_chassis_acceleration); - } - -private: - struct SteeringStatus { - Eigen::Vector4d angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle = Eigen::Vector4d::Zero(); - Eigen::Vector4d velocity = Eigen::Vector4d::Zero(); - Eigen::Vector4d sin_angle_minus_phi = Eigen::Vector4d::Zero(); - Eigen::Vector4d cos_angle_minus_phi = Eigen::Vector4d::Zero(); - }; - - struct ChassisStatus { - Eigen::Vector3d velocity = Eigen::Vector3d::Zero(); - Eigen::Vector4d wheel_velocity_x = Eigen::Vector4d::Zero(); - Eigen::Vector4d wheel_velocity_y = Eigen::Vector4d::Zero(); - }; - - struct JointStateData { - Eigen::Vector4d alpha_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_dot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d alpha_ddot_rad = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_dot = Eigen::Vector4d::Zero(); - Eigen::Vector4d radius_ddot = Eigen::Vector4d::Zero(); - bool valid = false; - }; - - struct JointFeedbackStates : JointStateData {}; - - struct JointTargetStates : JointStateData { - bool has_velocity = false; - bool has_acceleration = false; - }; - - enum class JointStateSource : uint8_t { Target, Feedback }; - - struct JointStateView { - const Eigen::Vector4d& alpha_rad; - const Eigen::Vector4d& alpha_dot_rad; - const Eigen::Vector4d& alpha_ddot_rad; - const Eigen::Vector4d& radius; - const Eigen::Vector4d& radius_dot; - const Eigen::Vector4d& radius_ddot; - JointStateSource source; - bool valid; - }; - - static JointStateView - select_joint_state(const JointTargetStates& target, const JointFeedbackStates& feedback) { - if (target.valid) { - return { - target.alpha_rad, target.alpha_dot_rad, target.alpha_ddot_rad, target.radius, - target.radius_dot, target.radius_ddot, JointStateSource::Target, target.valid}; - } - - return {feedback.alpha_rad, feedback.alpha_dot_rad, - feedback.alpha_ddot_rad, feedback.radius, - feedback.radius_dot, feedback.radius_ddot, - JointStateSource::Feedback, feedback.valid}; - } - - [[nodiscard]] static Eigen::Vector4d read_required_inputs_( - const std::array, kWheelCount>& inputs) { - return {*inputs[0], *inputs[1], *inputs[2], *inputs[3]}; - } - - [[nodiscard]] static Eigen::Vector4d read_optional_inputs_( - const std::array, kWheelCount>& inputs) { - return { - inputs[0].ready() ? *inputs[0] : nan_, - inputs[1].ready() ? *inputs[1] : nan_, - inputs[2].ready() ? *inputs[2] : nan_, - inputs[3].ready() ? *inputs[3] : nan_, - }; - } - - static void populate_joint_geometry_( - const Eigen::Vector4d& alpha_rad, const Eigen::Vector4d& alpha_dot_rad, - const Eigen::Vector4d& alpha_ddot_rad, double chassis_radius, double rod_length, - Eigen::Vector4d& radius, Eigen::Vector4d& radius_dot, Eigen::Vector4d& radius_ddot) { - radius = chassis_radius + rod_length * alpha_rad.array().cos(); - radius_dot = -rod_length * alpha_rad.array().sin() * alpha_dot_rad.array(); - radius_ddot = -rod_length * alpha_rad.array().cos() * alpha_dot_rad.array().square() - - rod_length * alpha_rad.array().sin() * alpha_ddot_rad.array(); - } - - [[nodiscard]] JointFeedbackStates update_joint_feedback_states_() { - JointFeedbackStates joint; - joint.alpha_rad = read_required_inputs_(joint_angle_); - joint.alpha_dot_rad = read_required_inputs_(joint_velocity_); - - if (!joint.alpha_rad.array().isFinite().all() - || !joint.alpha_dot_rad.array().isFinite().all()) - return joint; - - if (last_joint_velocity_valid_) { - joint.alpha_ddot_rad = (joint.alpha_dot_rad - last_joint_velocity_) / dt_; - } - - last_joint_velocity_ = joint.alpha_dot_rad; - last_joint_velocity_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - - return joint; - } - - [[nodiscard]] JointTargetStates update_joint_target_states_() { - JointTargetStates joint; - joint.alpha_rad = read_optional_inputs_(joint_target_physical_angle_); - - if (!joint.alpha_rad.array().isFinite().all()) - return joint; - - const Eigen::Vector4d target_velocity = read_optional_inputs_(joint_target_physical_velocity_); - if (target_velocity.array().isFinite().all()) { - joint.alpha_dot_rad = target_velocity; - joint.has_velocity = true; - } else if (last_joint_target_angle_valid_) { - joint.alpha_dot_rad = (joint.alpha_rad - last_joint_target_angle_) / dt_; - joint.has_velocity = true; - } - - const Eigen::Vector4d target_acceleration = read_optional_inputs_( - joint_target_physical_acceleration_); - if (target_acceleration.array().isFinite().all()) { - joint.alpha_ddot_rad = target_acceleration; - joint.has_acceleration = true; - } - - last_joint_target_angle_ = joint.alpha_rad; - last_joint_target_angle_valid_ = true; - - populate_joint_geometry_( - joint.alpha_rad, joint.alpha_dot_rad, joint.alpha_ddot_rad, chassis_radius_, - rod_length_, joint.radius, joint.radius_dot, joint.radius_ddot); - - joint.valid = joint.radius.array().isFinite().all() - && joint.radius_dot.array().isFinite().all() - && joint.radius_ddot.array().isFinite().all(); - return joint; - } - - void reset_all_controls() { - control_acceleration_filter_.reset(); - - chassis_yaw_angle_imu_ = 0.0; - chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - vehicle_radius_ = Eigen::Vector4d::Constant(chassis_radius_ + rod_length_); - last_joint_velocity_ = Eigen::Vector4d::Zero(); - last_joint_velocity_valid_ = false; - last_joint_target_angle_ = Eigen::Vector4d::Zero(); - last_joint_target_angle_valid_ = false; - - for (size_t i = 0; i < kWheelCount; ++i) { - *steering_control_torque_[i] = 0.0; - *wheel_control_torque_[i] = 0.0; - } - } - - void integral_yaw_angle_imu() { - chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; - chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); - } - - [[nodiscard]] SteeringStatus calculate_steering_status() const { - SteeringStatus steering_status; - steering_status.angle = read_required_inputs_(steering_angle_); - steering_status.angle.array() -= std::numbers::pi / 4; - steering_status.cos_angle = steering_status.angle.array().cos(); - steering_status.sin_angle = steering_status.angle.array().sin(); - - for (size_t i = 0; i < kWheelCount; ++i) { - const double angle_minus_phi = steering_status.angle[i] - phi_[i]; - steering_status.sin_angle_minus_phi[i] = std::sin(angle_minus_phi); - steering_status.cos_angle_minus_phi[i] = std::cos(angle_minus_phi); - } - - steering_status.velocity = read_required_inputs_(steering_velocity_); - return steering_status; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_velocities() const { - return read_required_inputs_(wheel_velocity_); - } - - /** - * @brief Observe chassis velocity from wheel velocities using least squares - * - * Solves: A·x = b for x = [vx, vy, ωz] - * where A_i = [cos(ζᵢ), sin(ζᵢ), R_i·sin(ζᵢ - φᵢ)] - * b_i = r·ωᵢ - Ṙᵢ·cos(ζᵢ - φᵢ) - */ - [[nodiscard]] Eigen::Vector3d calculate_chassis_velocity( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointFeedbackStates& joint) const { - Eigen::Vector4d wheel_velocities_eff = wheel_velocities; - if (joint.valid) { - const Eigen::Vector4d clamped_radius_dot = - joint.radius_dot.cwiseMax(-0.1).cwiseMin(0.1); - const Eigen::Vector4d wheel_omega_mech = - (clamped_radius_dot.array() * phi_cos_vec_.array() - * steering_status.cos_angle.array() - + clamped_radius_dot.array() * phi_sin_vec_.array() - * steering_status.sin_angle.array()) - / wheel_radius_; - wheel_velocities_eff -= wheel_omega_mech; - } - - const double one_quarter_r = wheel_radius_ / 4.0; - Eigen::Vector3d velocity; - velocity.x() = one_quarter_r * wheel_velocities_eff.dot(steering_status.cos_angle); - velocity.y() = one_quarter_r * wheel_velocities_eff.dot(steering_status.sin_angle); - velocity.z() = - -one_quarter_r - * (-wheel_velocities_eff[0] * steering_status.sin_angle[0] / vehicle_radius_[0] - + wheel_velocities_eff[1] * steering_status.cos_angle[1] / vehicle_radius_[1] - + wheel_velocities_eff[2] * steering_status.sin_angle[2] / vehicle_radius_[2] - - wheel_velocities_eff[3] * steering_status.cos_angle[3] / vehicle_radius_[3]); - return velocity; - } - - /** - * @brief Calculate expected chassis status with energy scaling - * - * Wheel center velocity: v_i = v + ω·R_i·e_t,i + Ṙᵢ·e_r,i - */ - [[nodiscard]] ChassisStatus calculate_chassis_status_expected( - Eigen::Ref chassis_velocity, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback) { - const double chassis_energy = calculate_chassis_energy(chassis_velocity); - const double chassis_energy_expected = calculate_chassis_energy(chassis_velocity_expected_); - - if (std::isfinite(chassis_energy) && std::isfinite(chassis_energy_expected) - && chassis_energy_expected > chassis_energy && chassis_energy_expected > 1e-12) { - const double k = std::sqrt(chassis_energy / chassis_energy_expected); - if (std::isfinite(k) && k >= 0.0) - chassis_velocity_expected_ *= k; - } - - ChassisStatus chassis_status_expected; - chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); - - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - for (size_t i = 0; i < kWheelCount; ++i) { - const double radius = joint.valid ? joint.radius[i] : vehicle_radius_[i]; - const double clamped_radius_dot = - joint.valid ? std::clamp(joint.radius_dot[i], -0.1, 0.1) : 0.0; - const Eigen::Vector2d wheel_velocity = Eigen::Vector2d(vx, vy) - + vz * radius * tangential_unit_fast_(i) - + clamped_radius_dot * radial_unit_fast_(i); - chassis_status_expected.wheel_velocity_x[i] = wheel_velocity.x(); - chassis_status_expected.wheel_velocity_y[i] = wheel_velocity.y(); - } - - return chassis_status_expected; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_velocity() const { - Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; - chassis_control_velocity.head<2>() = - Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); - return chassis_control_velocity; - } - - [[nodiscard]] Eigen::Vector3d calculate_chassis_control_acceleration( - Eigen::Ref chassis_velocity_expected, - Eigen::Ref chassis_control_velocity) { - Eigen::Vector2d translational_control_acceleration = - chassis_translational_velocity_pid_.update( - chassis_control_velocity.head<2>() - chassis_velocity_expected.head<2>()); - - const double angular_control_acceleration = chassis_angular_velocity_pid_.update( - chassis_control_velocity[2] - chassis_velocity_expected[2]); - - Eigen::Vector3d chassis_control_acceleration; - chassis_control_acceleration << translational_control_acceleration, - angular_control_acceleration; - if (chassis_control_acceleration.lpNorm<1>() < 1e-1) - chassis_control_acceleration.setZero(); - return chassis_control_acceleration; - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_pid_torques( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const ChassisStatus& chassis_status_expected) { - const Eigen::Vector4d wheel_control_velocity = - chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() - + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); - return wheel_velocity_pid_.update( - wheel_control_velocity / wheel_radius_ - wheel_velocities); - } - - [[nodiscard]] Eigen::Vector3d constrain_chassis_control_acceleration( - const SteeringStatus& steering_status, Eigen::Ref wheel_velocities, - const JointTargetStates& joint_target, - Eigen::Ref chassis_acceleration, - Eigen::Ref wheel_pid_torques, const double& power_limit) { - Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); - double translational_acceleration_max = translational_acceleration_direction.norm(); - if (translational_acceleration_max > 0.0) - translational_acceleration_direction /= translational_acceleration_max; - - double angular_acceleration_max = chassis_acceleration.z(); - double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; - angular_acceleration_max *= angular_acceleration_direction; - - const double rhombus_right = friction_coefficient_ * g_; - const double constraint_radius = - joint_target.valid ? joint_target.radius.mean() : vehicle_radius_.mean(); - const double rhombus_top = rhombus_right * mass_ * constraint_radius / moment_of_inertia_; - - const auto params = calculate_ellipse_parameters( - steering_status, wheel_velocities, joint_target, translational_acceleration_direction, - angular_acceleration_direction, wheel_pid_torques); - - const QcpSolver::QuadraticConstraint quadratic_constraint{ - params.a, params.b, params.c, params.d, params.e, params.f - power_limit}; - - Eigen::Vector2d best_point = qcp_solver_.solve( - {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, - {rhombus_right, rhombus_top}, quadratic_constraint); - - const double min_translational = 0.3 * rhombus_right; - if (best_point.x() < min_translational && translational_acceleration_max > min_translational) - best_point.x() = min_translational; - - Eigen::Vector3d best_acceleration; - best_acceleration << best_point.x() * translational_acceleration_direction, - best_point.y() * angular_acceleration_direction; - return best_acceleration; - } - - [[nodiscard]] EllipseParameters calculate_ellipse_parameters( - const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, - const JointTargetStates& joint_target, - const Eigen::Vector2d& translational_acceleration_direction, - const double& angular_acceleration_direction, - const Eigen::Vector4d& wheel_torque_base) const { - EllipseParameters params{0, 0, 0, 0, 0, 0}; - - for (size_t i = 0; i < kWheelCount; ++i) { - const double constraint_radius = - joint_target.valid ? joint_target.radius[i] : vehicle_radius_[i]; - const double cos_alpha_minus_gamma = - steering_status.cos_angle[i] * translational_acceleration_direction.x() - + steering_status.sin_angle[i] * translational_acceleration_direction.y(); - const double sin_alpha_minus_varphi = steering_status.sin_angle_minus_phi[i]; - const double double_k1_torque_base_plus_wheel_velocity = - 2 * k1_ * wheel_torque_base[i] + wheel_velocities[i]; - - params.a += ellipse_coeff_quadratic_translational_ * cos_alpha_minus_gamma - * cos_alpha_minus_gamma; - params.b += ellipse_coeff_cross_term_ * angular_acceleration_direction - * cos_alpha_minus_gamma * sin_alpha_minus_varphi / constraint_radius; - params.c += ellipse_coeff_quadratic_angular_ * sin_alpha_minus_varphi - * sin_alpha_minus_varphi / (constraint_radius * constraint_radius); - params.d += ellipse_coeff_linear_translational_ - * double_k1_torque_base_plus_wheel_velocity * cos_alpha_minus_gamma; - params.e += ellipse_coeff_linear_angular_ * angular_acceleration_direction - * double_k1_torque_base_plus_wheel_velocity * sin_alpha_minus_varphi - / constraint_radius; - params.f += wheel_torque_base[i] * (k1_ * wheel_torque_base[i] + wheel_velocities[i]); - } - - return params; - } - - [[nodiscard]] Eigen::Vector4d calculate_steering_control_torques( - const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, - const JointTargetStates& joint_target, const JointFeedbackStates& joint_feedback, - const Eigen::Vector3d& chassis_acceleration) { - const double vx = chassis_status_expected.velocity.x(); - const double vy = chassis_status_expected.velocity.y(); - const double vz = chassis_status_expected.velocity.z(); - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - const auto joint = select_joint_state(joint_target, joint_feedback); - if (!joint.valid) [[unlikely]] - return Eigen::Vector4d::Zero(); - - Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() - + chassis_status_expected.wheel_velocity_y.array().square(); - - Eigen::Vector4d steering_control_velocity = - vx * ay - vy * ax - vz * (vx * vx + vy * vy) - + joint.radius.array() * (az * vx - vz * (ax + vz * vy)) * phi_cos_vec_.array() - + joint.radius.array() * (az * vy - vz * (ay - vz * vx)) * phi_sin_vec_.array(); - Eigen::Vector4d steering_control_angle; - - for (size_t i = 0; i < kWheelCount; ++i) { - if (dot_r_squared[i] > 1e-2) { - steering_control_velocity[i] /= dot_r_squared[i]; - steering_control_angle[i] = std::atan2( - chassis_status_expected.wheel_velocity_y[i], - chassis_status_expected.wheel_velocity_x[i]); - } else { - const double x = - ax - joint.radius[i] * (az * phi_sin_vec_[i] + vz * vz * phi_cos_vec_[i]); - const double y = - ay + joint.radius[i] * (az * phi_cos_vec_[i] - vz * vz * phi_sin_vec_[i]); - if (x * x + y * y > 1e-6) { - steering_control_velocity[i] = 0.0; - steering_control_angle[i] = std::atan2(y, x); - } else { - steering_control_velocity[i] = nan_; - steering_control_angle[i] = nan_; - } - } - } - - Eigen::Vector4d steering_torque = steering_velocity_pid_.update( - steering_control_velocity - + steering_angle_pid_.update( - (steering_control_angle - steering_status.angle).unaryExpr([](double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - })) - - steering_status.velocity); - - return steering_torque.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); - } - - [[nodiscard]] Eigen::Vector4d calculate_wheel_control_torques( - const SteeringStatus& steering_status, const JointTargetStates& joint_target, - const JointFeedbackStates& joint_feedback, const Eigen::Vector3d& chassis_acceleration, - const Eigen::Vector4d& wheel_pid_torques) const { - const auto joint = select_joint_state(joint_target, joint_feedback); - - const double ax = chassis_acceleration.x(); - const double ay = chassis_acceleration.y(); - const double az = chassis_acceleration.z(); - - Eigen::Vector4d wheel_torque = - wheel_radius_ - * (ax * mass_ * steering_status.cos_angle.array() - + ay * mass_ * steering_status.sin_angle.array() - + az * moment_of_inertia_ * steering_status.sin_angle_minus_phi.array() - / joint.radius.array()) - / 4.0; - - wheel_torque += wheel_pid_torques; - return wheel_torque; - } - - void update_control_torques( - const Eigen::Vector4d& steering_torque, const Eigen::Vector4d& wheel_torque) { - for (size_t i = 0; i < kWheelCount; ++i) { - *steering_control_torque_[i] = steering_torque[i]; - *wheel_control_torque_[i] = wheel_torque[i]; - } - } - - void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { - chassis_velocity_expected_ += dt_ * base_link_to_odom_vector(chassis_acceleration); - } - - Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { - vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); - return vector; - } - - [[nodiscard]] double calculate_chassis_energy(const Eigen::Vector3d& velocity) const { - return mass_ * velocity.head<2>().squaredNorm() - + moment_of_inertia_ * velocity.z() * velocity.z(); - } - - static Eigen::Vector2d radial_unit_(double phi) { return {std::cos(phi), std::sin(phi)}; } - - static Eigen::Vector2d tangential_unit_(double phi) { return {-std::sin(phi), std::cos(phi)}; } - - [[nodiscard]] Eigen::Vector2d radial_unit_fast_(size_t wheel_index) const { - return {phi_cos_[wheel_index], phi_sin_[wheel_index]}; - } - - [[nodiscard]] Eigen::Vector2d tangential_unit_fast_(size_t wheel_index) const { - return {-phi_sin_[wheel_index], phi_cos_[wheel_index]}; - } - - static double wrap_to_half_pi_(double diff) { - diff = std::fmod(diff, std::numbers::pi); - if (diff < -std::numbers::pi / 2) - diff += std::numbers::pi; - else if (diff > std::numbers::pi / 2) - diff -= std::numbers::pi; - return diff; - } - - static constexpr std::array phi_ = { - 0.0, - std::numbers::pi / 2, - std::numbers::pi, - -std::numbers::pi / 2, - }; - - static constexpr std::array phi_cos_ = { - 1.0, - 0.0, - -1.0, - 0.0, - }; - - static constexpr std::array phi_sin_ = { - 0.0, - 1.0, - 0.0, - -1.0, - }; - - static constexpr std::array kJointNames = { - "left_front", "left_back", "right_back", "right_front"}; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double dt_ = 1e-3; - static constexpr double g_ = 9.81; - - const double mass_; - const double moment_of_inertia_; - const double chassis_radius_; - const double rod_length_; - const double wheel_radius_; - const double friction_coefficient_; - const double k1_; - const double k2_; - const double no_load_power_; - - // Precomputed constants for calculate_ellipse_parameters - const double ellipse_coeff_quadratic_translational_; // k1 * mass^2 * wheel_radius^2 / 16 - const double ellipse_coeff_cross_term_; // k1 * mass * moment_of_inertia * wheel_radius^2 / 8 - const double ellipse_coeff_quadratic_angular_; // k1 * moment_of_inertia^2 * wheel_radius^2 / 16 - const double ellipse_coeff_linear_translational_; // mass * wheel_radius / 4 - const double ellipse_coeff_linear_angular_; // moment_of_inertia * wheel_radius / 4 - - Eigen::Vector4d vehicle_radius_; - const Eigen::Vector4d phi_cos_vec_{1.0, 0.0, -1.0, 0.0}; - const Eigen::Vector4d phi_sin_vec_{0.0, 1.0, 0.0, -1.0}; - Eigen::Vector4d last_joint_velocity_ = Eigen::Vector4d::Zero(); - bool last_joint_velocity_valid_ = false; - Eigen::Vector4d last_joint_target_angle_ = Eigen::Vector4d::Zero(); - bool last_joint_target_angle_valid_ = false; - - InputInterface joystick_right_; - InputInterface joystick_left_; - - std::array, kWheelCount> steering_angle_; - std::array, kWheelCount> steering_velocity_; - std::array, kWheelCount> wheel_velocity_; - - std::array, kWheelCount> joint_angle_; - std::array, kWheelCount> joint_velocity_; - - std::array, kWheelCount> joint_target_physical_angle_; - std::array, kWheelCount> joint_target_physical_velocity_; - std::array, kWheelCount> joint_target_physical_acceleration_; - - InputInterface chassis_yaw_velocity_imu_; - InputInterface chassis_control_velocity_; - InputInterface power_limit_; - - std::array, kWheelCount> steering_control_torque_; - std::array, kWheelCount> wheel_control_torque_; - - QcpSolver qcp_solver_; - filter::LowPassFilter<3> control_acceleration_filter_; - - double chassis_yaw_angle_imu_ = 0.0; - Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); - - pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; - pid::PidCalculator chassis_angular_velocity_pid_; - pid::MatrixPidCalculator<4> steering_velocity_pid_; - pid::MatrixPidCalculator<4> steering_angle_pid_; - pid::MatrixPidCalculator<4> wheel_velocity_pid_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableChassisController, rmcs_executor::Component) From 074a373cf46306353c56601656e7022595d85a52 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Mon, 6 Jul 2026 22:33:52 +0800 Subject: [PATCH 70/90] feat(chassis): update active suspension parameters and integrate mouse wheel control --- .../config/deformable-infantry-omni-b.yaml | 5 +- .../config/deformable-infantry-omni.yaml | 3 - .../controller/chassis/deformable_chassis.cpp | 149 ++++++++++++------ .../shooting/friction_wheel_controller.cpp | 50 ------ .../hardware/deformable-infantry-omni-b.cpp | 5 +- .../src/hardware/deformable-infantry-omni.cpp | 5 +- .../referee/app/ui/deformable_infantry_ui.cpp | 5 +- .../ui/widget/deformable_chassis_top_view.hpp | 16 +- 8 files changed, 112 insertions(+), 126 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 21cbb86d..73e1bb7a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -59,7 +59,6 @@ chassis_controller: # Deploy geometry / chassis-owned joint intent min_angle: 8.0 max_angle: 58.0 - launch_ramp_shortcut_enabled: false active_suspension_enable: true spin_ratio: 1.0 @@ -145,8 +144,6 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 - friction_velocity_min: 550.0 - friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -196,7 +193,7 @@ deformable_chassis_controller: ros__parameters: mass: 25.5 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 + chassis_radius: 0.26848 rod_length: 0.140 wheel_radius: 0.075 friction_coefficient: 6.6 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 4ac8e02c..3b0c80a2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -57,7 +57,6 @@ chassis_controller: # Deploy geometry / chassis-owned joint intent min_angle: 12.0 max_angle: 50.0 - launch_ramp_shortcut_enabled: false active_suspension_enable: true spin_ratio: 1.0 @@ -143,8 +142,6 @@ friction_wheel_controller: friction_velocities: - 580.0 - 580.0 - friction_velocity_min: 550.0 - friction_velocity_max: 600.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 4b25a86c..43aa6a45 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -40,10 +40,15 @@ class DeformableChassis rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) - , launch_ramp_shortcut_enabled_(get_parameter_or("launch_ramp_shortcut_enabled", true)) , min_angle_(get_parameter_or("min_angle", 7.0)) , max_angle_(get_parameter_or("max_angle", 58.0)) + , active_suspension_base_angle_deg_(std::clamp( + get_parameter_or("active_suspension_base_angle", max_angle_), min_angle_ - 5.0, + max_angle_)) + , active_suspension_base_angle_scroll_step_deg_(std::max( + std::abs(get_parameter_or("active_suspension_base_angle_scroll_step_deg", 5.0)), + 0.0)) , target_physical_velocity_limit_( std::max( deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), @@ -102,6 +107,7 @@ class DeformableChassis register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); @@ -130,6 +136,7 @@ class DeformableChassis register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); + register_output("/chassis/active_suspension/active", active_suspension_active_, false); register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); @@ -222,10 +229,12 @@ class DeformableChassis } update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right); + update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); *ctrl_hold_active_ = ctrl_hold_requested_by_input_(); + *active_suspension_active_ = suspension_requested_by_input_(); update_velocity_control(); update_lift_target_toggle(keyboard); + update_active_suspension_base_angle_from_mouse_wheel_(); run_joint_intent_pipeline_(); } while (false); @@ -275,7 +284,6 @@ class DeformableChassis const bool last_c_pressed = last_keyboard_.c; const bool qe_combo_pressed = q_pressed && e_pressed; const bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; - const bool e_rising = !last_e_pressed && e_pressed; const bool c_rising = !last_c_pressed && keyboard.c; const bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; if (switch_left == rmcs_msgs::Switch::DOWN) { @@ -297,14 +305,6 @@ class DeformableChassis } else if (qe_combo_rising) { deactivate_complex_spin_(); activate_qe_complex_spin_(mode); - } else if (e_rising && !q_pressed) { - if (complex_spin_active_) { - deactivate_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - activate_complex_spin_(mode); - } } else if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); @@ -324,12 +324,6 @@ class DeformableChassis mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } - } else if (launch_ramp_shortcut_enabled_ && !last_keyboard_.x && keyboard.x) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; } else if (!last_keyboard_.z && keyboard.z) { deactivate_complex_spin_(); deactivate_qe_complex_spin_(); @@ -434,7 +428,9 @@ class DeformableChassis return current_physical_angles; } - bool prone_override_requested_by_keyboard() const { return keyboard_.ready() && keyboard_->ctrl; } + bool suspension_toggle_requested_by_keyboard_(const rmcs_msgs::Keyboard& keyboard) const { + return !last_keyboard_.e && keyboard.e && !keyboard.q; + } bool suspension_toggle_requested_by_switch_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { @@ -443,14 +439,27 @@ class DeformableChassis } void update_suspension_toggle_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) { - suspension_on_by_switch = !suspension_on_by_switch; + active_suspension_on_by_remote_ = !active_suspension_on_by_remote_; + } + if (suspension_toggle_requested_by_keyboard_(keyboard)) { + active_suspension_on_by_keyboard_ = !active_suspension_on_by_keyboard_; } } + bool prone_override_requested_by_keyboard() const { + return keyboard_.ready() && keyboard_->ctrl; + } + bool ctrl_hold_requested_by_input_() const { - return prone_override_requested_by_keyboard() || suspension_on_by_switch; + return prone_override_requested_by_keyboard() || active_suspension_on_by_remote_ + || active_suspension_on_by_keyboard_; + } + + bool low_prone_override_active_() const { + return prone_override_requested_by_keyboard(); } bool suspension_requested_by_input_() const { @@ -566,8 +575,33 @@ class DeformableChassis }); } + double active_suspension_min_angle_deg_() const { return min_angle_ - 5.0; } + double active_suspension_min_angle_rad_() const { - return deg_to_rad(min_angle_ - 5.0); + return deg_to_rad(active_suspension_min_angle_deg_()); + } + + double active_suspension_base_angle_rad_() const { + return deg_to_rad(active_suspension_base_angle_deg_); + } + + bool active_suspension_correction_inverted_() const { + const double midpoint = (active_suspension_min_angle_deg_() + max_angle_) / 2.0; + return active_suspension_base_angle_deg_ > midpoint; + } + + void update_active_suspension_base_angle_from_mouse_wheel_() { + if (!suspension_requested_by_input_() || low_prone_override_active_() || !mouse_wheel_.ready()) + return; + + const double mouse_wheel = *mouse_wheel_; + if (!std::isfinite(mouse_wheel) || std::abs(mouse_wheel) <= 1e-9) + return; + + active_suspension_base_angle_deg_ = std::clamp( + active_suspension_base_angle_deg_ + - mouse_wheel * active_suspension_base_angle_scroll_step_deg_, + active_suspension_min_angle_deg_(), max_angle_); } void update_active_suspension_() { @@ -593,18 +627,29 @@ class DeformableChassis return; } - // Positive pitch_angle_diff raises the rear pair. Positive roll_angle_diff raises the left - // pair. Every leg starts from the active-suspension minimum and only receives additive - // corrections so at least one leg always stays at that minimum. - const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); - const double back_pitch_add = std::max(pitch_angle_diff, 0.0); - const double left_roll_add = std::max(roll_angle_diff, 0.0); - const double right_roll_add = std::max(-roll_angle_diff, 0.0); - - suspension_correction_target_rad_[kLeftFront] = front_pitch_add + left_roll_add; - suspension_correction_target_rad_[kLeftBack] = back_pitch_add + left_roll_add; - suspension_correction_target_rad_[kRightBack] = back_pitch_add + right_roll_add; - suspension_correction_target_rad_[kRightFront] = front_pitch_add + right_roll_add; + if (active_suspension_correction_inverted_()) { + const double front_pitch_drop = std::max(pitch_angle_diff, 0.0); + const double back_pitch_drop = std::max(-pitch_angle_diff, 0.0); + const double left_roll_drop = std::max(-roll_angle_diff, 0.0); + const double right_roll_drop = std::max(roll_angle_diff, 0.0); + + suspension_correction_target_rad_[kLeftFront] = + -(front_pitch_drop + left_roll_drop); + suspension_correction_target_rad_[kLeftBack] = -(back_pitch_drop + left_roll_drop); + suspension_correction_target_rad_[kRightBack] = -(back_pitch_drop + right_roll_drop); + suspension_correction_target_rad_[kRightFront] = + -(front_pitch_drop + right_roll_drop); + } else { + const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); + const double back_pitch_add = std::max(pitch_angle_diff, 0.0); + const double left_roll_add = std::max(roll_angle_diff, 0.0); + const double right_roll_add = std::max(-roll_angle_diff, 0.0); + + suspension_correction_target_rad_[kLeftFront] = front_pitch_add + left_roll_add; + suspension_correction_target_rad_[kLeftBack] = back_pitch_add + left_roll_add; + suspension_correction_target_rad_[kRightBack] = back_pitch_add + right_roll_add; + suspension_correction_target_rad_[kRightFront] = front_pitch_add + right_roll_add; + } joint_suspension_active_.fill(true); } @@ -617,12 +662,14 @@ class DeformableChassis suspension_correction_target_rad_[i] = 0.0; } - const double base_angle = std::isfinite(joint_target_physical_angle_state_rad_[i]) + const double base_angle = std::isfinite(joint_target_physical_angle_state_rad_[i]) ? joint_target_physical_angle_state_rad_[i] - : active_suspension_min_angle_rad_(); - const double correction_max = std::max(max_target_angle - base_angle, 0.0); + : (low_prone_override_active_() ? active_suspension_min_angle_rad_() + : active_suspension_base_angle_rad_()); + const double correction_min = active_suspension_min_angle_rad_() - base_angle; + const double correction_max = max_target_angle - base_angle; const double target = std::clamp( - suspension_correction_target_rad_[i], 0.0, correction_max); + suspension_correction_target_rad_[i], correction_min, correction_max); double& angle_state = suspension_correction_state_rad_[i]; double& velocity_state = suspension_correction_velocity_state_rad_[i]; @@ -669,6 +716,7 @@ class DeformableChassis *chassis_control_angle_ = nan_; current_target_angle_ = max_angle_; + active_suspension_base_angle_deg_ = max_angle_; lf_current_target_angle_ = current_target_angle_; lb_current_target_angle_ = current_target_angle_; rb_current_target_angle_ = current_target_angle_; @@ -679,7 +727,8 @@ class DeformableChassis joint_target_physical_velocity_state_rad_.fill(0.0); joint_target_physical_acceleration_state_rad_.fill(0.0); reset_active_suspension_correction_state_(); - suspension_on_by_switch = false; + active_suspension_on_by_keyboard_ = false; + active_suspension_on_by_remote_ = false; deactivate_complex_spin_(); deactivate_qe_complex_spin_(); @@ -760,16 +809,6 @@ class DeformableChassis angular_velocity = following_velocity_controller_.update(err); } break; - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - constexpr double alignment = 2 * std::numbers::pi; - if (err > alignment / 2) - err -= alignment; - - angular_velocity = following_velocity_controller_.update(err); - } break; - default: break; } @@ -859,7 +898,9 @@ class DeformableChassis current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); if (suspension_requested) { - current_target_physical_angles_rad_.fill(active_suspension_min_angle_rad_()); + current_target_physical_angles_rad_.fill( + low_prone_override_active_() ? active_suspension_min_angle_rad_() + : active_suspension_base_angle_rad_()); } update_chassis_imu_calibration_(); @@ -1015,6 +1056,7 @@ class DeformableChassis InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; + InputInterface mouse_wheel_; InputInterface rotary_knob_; InputInterface update_rate_; @@ -1030,6 +1072,7 @@ class DeformableChassis OutputInterface mode_; OutputInterface chassis_control_velocity_; OutputInterface ctrl_hold_active_; + OutputInterface active_suspension_active_; bool spinning_forward_ = true; bool apply_symmetric_target = true; @@ -1040,7 +1083,6 @@ class DeformableChassis double qe_last_toggle_elapsed_ = 0.0; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; - const bool launch_ramp_shortcut_enabled_; InputInterface left_front_joint_physical_angle_; InputInterface left_back_joint_physical_angle_; @@ -1073,6 +1115,8 @@ class DeformableChassis double min_angle_; double max_angle_; + double active_suspension_base_angle_deg_; + double active_suspension_base_angle_scroll_step_deg_; double current_target_angle_; double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, @@ -1090,7 +1134,8 @@ class DeformableChassis double target_physical_velocity_limit_; double target_physical_acceleration_limit_; bool active_suspension_enable_; - bool suspension_on_by_switch = false; + bool active_suspension_on_by_keyboard_ = false; + bool active_suspension_on_by_remote_ = false; double suspension_velocity_limit_; double suspension_acceleration_limit_; double suspension_correction_velocity_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index 3018b3f8..f0b277f1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -31,7 +31,6 @@ class FrictionWheelController register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/mouse/mouse_wheel", mouse_wheel_); auto friction_wheels = get_parameter("friction_wheels").as_string_array(); auto friction_working_velocities = get_parameter("friction_velocities").as_double_array(); @@ -59,36 +58,6 @@ class FrictionWheelController friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); } - const bool has_adjustable_min = has_parameter("friction_velocity_min"); - const bool has_adjustable_max = has_parameter("friction_velocity_max"); - if (has_adjustable_min != has_adjustable_max) - throw std::runtime_error( - "Parameter mismatch: 'friction_velocity_min' and 'friction_velocity_max' must be " - "provided together."); - - friction_velocity_adjustment_enabled_ = has_adjustable_min; - if (friction_velocity_adjustment_enabled_) { - friction_velocity_min_ = get_parameter("friction_velocity_min").as_double(); - friction_velocity_max_ = get_parameter("friction_velocity_max").as_double(); - if (friction_velocity_min_ > friction_velocity_max_) - throw std::runtime_error( - "Invalid friction velocity range: 'friction_velocity_min' must be less than " - "or equal to 'friction_velocity_max'."); - - friction_adjustable_velocity_ = friction_working_velocities_[0]; - for (size_t i = 1; i < friction_count_; i++) { - if (std::abs(friction_working_velocities_[i] - friction_adjustable_velocity_) > 1e-6) - throw std::runtime_error( - "Adjustable friction velocity requires all 'friction_velocities' to be " - "identical."); - } - - if (friction_adjustable_velocity_ < friction_velocity_min_ - || friction_adjustable_velocity_ > friction_velocity_max_) - throw std::runtime_error( - "Initial friction velocity is outside the configured adjustable range."); - } - friction_soft_start_stop_step_ = (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); @@ -101,7 +70,6 @@ class FrictionWheelController const auto switch_right = *switch_right_; const auto switch_left = *switch_left_; const auto keyboard = *keyboard_; - const auto mouse_wheel = *mouse_wheel_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -111,7 +79,6 @@ class FrictionWheelController } if (switch_right != Switch::DOWN) { - update_adjustable_friction_velocity(keyboard, mouse_wheel); update_friction_working_velocity_outputs(); if ((!last_keyboard_.v && keyboard.v) @@ -148,18 +115,6 @@ class FrictionWheelController *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; } - void update_adjustable_friction_velocity(const rmcs_msgs::Keyboard& keyboard, double mouse_wheel) { - if (!friction_velocity_adjustment_enabled_ || !keyboard.x) - return; - - friction_adjustable_velocity_ -= mouse_wheel * 5.0 * 10; - friction_adjustable_velocity_ = - std::clamp(friction_adjustable_velocity_, friction_velocity_min_, friction_velocity_max_); - - for (size_t i = 0; i < friction_count_; i++) - friction_working_velocities_[i] = friction_adjustable_velocity_; - } - void update_friction_working_velocity_outputs() { for (size_t i = 0; i < friction_count_; i++) *friction_working_velocity_outputs_[i] = friction_working_velocities_[i]; @@ -245,7 +200,6 @@ class FrictionWheelController InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; - InputInterface mouse_wheel_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -259,10 +213,6 @@ class FrictionWheelController std::unique_ptr[]> friction_working_velocity_outputs_; bool friction_enabled_ = false; - bool friction_velocity_adjustment_enabled_ = false; - double friction_adjustable_velocity_ = 0.0; - double friction_velocity_min_ = 0.0; - double friction_velocity_max_ = 0.0; double friction_soft_start_stop_step_; double friction_soft_start_stop_percentage_ = nan_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index a6596b03..5b9dcea3 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -137,9 +137,8 @@ class DeformableInfantryOmniB for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()); + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point for (auto& motor : chassis_joint_motors_) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 3560c5b6..d9d9e00c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -124,9 +124,8 @@ class DeformableInfantryOmni for (auto& motor : chassis_wheel_motors_) motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()); + .set_reduction_ratio(19.0) + .enable_multi_turn_angle()); for (auto& motor : chassis_joint_motors_) motor.configure( diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index aea13c51..78387530 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -54,6 +54,7 @@ class DeformableInfantry register_input("/predefined/timestamp", timestamp_); register_input("/chassis/control_mode", chassis_mode_); + register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/supercap/voltage", supercap_voltage_); @@ -165,7 +166,8 @@ class DeformableInfantry *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, }; - deformable_chassis_leg_arcs_.update(*chassis_angle_, leg_angles); + deformable_chassis_leg_arcs_.update( + *chassis_angle_, leg_angles, active_suspension_active_.ready() && *active_suspension_active_); } static constexpr uint16_t screen_width = 1920, screen_height = 1080; @@ -189,6 +191,7 @@ class DeformableInfantry InputInterface timestamp_; InputInterface chassis_mode_; + InputInterface active_suspension_active_; InputInterface chassis_angle_; InputInterface supercap_voltage_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index 72c81891..9c08074d 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -21,7 +21,7 @@ class DeformableChassisLegArcs { std::swap(min_angle_rad_, max_angle_rad_); } - void update(double chassis_angle, const std::array& leg_angles) { + void update(double chassis_angle, const std::array& leg_angles, bool active_suspension) { if (!valid_angle_range_()) { set_visible(false); return; @@ -36,7 +36,7 @@ class DeformableChassisLegArcs { last_leg_angles_[i] = leg_angles[i]; update_leg_( legs_[i], last_chassis_angle_ + leg_base_mid_angles_[i], leg_radii_near_[i], - leg_radii_far_[i], last_leg_angles_[i]); + leg_radii_far_[i], last_leg_angles_[i], active_suspension); } } @@ -109,17 +109,13 @@ class DeformableChassisLegArcs { (leg_angle - min_angle_rad_) / (max_angle_rad_ - min_angle_rad_), 0.0, 1.0); } - static Shape::Color leg_color_(double normalized_extension) { - if (normalized_extension < 1.0 / 3.0) - return Shape::Color::ORANGE; - if (normalized_extension < 2.0 / 3.0) - return Shape::Color::YELLOW; - return Shape::Color::WHITE; + static Shape::Color leg_color_(bool active_suspension) { + return active_suspension ? Shape::Color::YELLOW : Shape::Color::WHITE; } void update_leg_( Arc& leg, double body_angle, uint16_t near_radius, uint16_t far_radius, - double leg_angle) const { + double leg_angle, bool active_suspension) const { const double normalized_extension = normalized_leg_extension_(leg_angle); // Min angle looks like a thin leg stretching radially outward from the center ring. const uint16_t radius = static_cast(std::lround( @@ -135,7 +131,7 @@ class DeformableChassisLegArcs { leg.set_y(center_y_); leg.set_r(radius); leg.set_width(width); - leg.set_color(leg_color_(normalized_extension)); + leg.set_color(leg_color_(active_suspension)); leg.set_angle(to_referee_angle_(body_angle), half_angle); } From 0abad5f03968b88b8d580deb03a96016fa0f0b1d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 00:45:50 +0800 Subject: [PATCH 71/90] Refactor Deformable Infantry Code and UI Components - Removed the entire deformable infantry steering implementation as it is no longer needed. - Updated the deformable infantry UI to use a loop for registering wheel velocities and joint physical angles, improving code maintainability. - Introduced `kWheelName` static array to avoid repetition and enhance clarity in the UI input registration. - Enhanced the update logic for deformable chassis leg arcs to use a more concise approach for checking readiness of joint angles. --- .../config/deformable-infantry-omni-b.yaml | 8 +- .../config/deformable-infantry-omni.yaml | 8 +- .../controller/chassis/deformable_chassis.cpp | 1181 ++++------------- .../deformable_chassis_joint_manager.hpp | 161 +++ .../deformable_chassis_mode_manager.hpp | 385 ++++++ .../chassis/deformable_chassis_suspension.hpp | 324 +++++ .../deformable_omni_wheel_controller.cpp | 259 ++-- .../chassis/omni_wheel_controller.cpp | 58 +- .../hardware/deformable-infantry-omni-b.cpp | 208 +-- .../src/hardware/deformable-infantry-omni.cpp | 193 ++- .../hardware/deformable-infantry-steering.cpp | 860 ------------ .../referee/app/ui/deformable_infantry_ui.cpp | 48 +- .../rmcs_core/src/referee/app/ui/infantry.cpp | 18 +- 13 files changed, 1543 insertions(+), 2168 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 73e1bb7a..402a2297 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -203,12 +203,10 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - dt: 0.001 b0: -1.0 kt: 1.0 @@ -225,9 +223,9 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity @@ -248,9 +246,9 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity @@ -271,9 +269,9 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 3b0c80a2..03a46e23 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -201,12 +201,10 @@ deformable_chassis_controller: lf_joint_controller: ros__parameters: - # Joint-local servo inputs produced by chassis intent generation measurement_angle: /chassis/left_front_joint/physical_angle setpoint_angle: /chassis/left_front_joint/target_physical_angle setpoint_velocity: /chassis/left_front_joint/target_physical_velocity control: /chassis/left_front_joint/control_torque - dt: 0.001 b0: -1.0 kt: 1.0 @@ -223,9 +221,9 @@ lf_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + lb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle setpoint_angle: /chassis/left_back_joint/target_physical_angle setpoint_velocity: /chassis/left_back_joint/target_physical_velocity @@ -246,9 +244,9 @@ lb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rb_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle setpoint_angle: /chassis/right_back_joint/target_physical_angle setpoint_velocity: /chassis/right_back_joint/target_physical_velocity @@ -269,9 +267,9 @@ rb_joint_controller: u_max: 200.0 output_min: -200.0 output_max: 200.0 + rf_joint_controller: ros__parameters: - # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle setpoint_angle: /chassis/right_front_joint/target_physical_angle setpoint_velocity: /chassis/right_front_joint/target_physical_velocity diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 43aa6a45..e12e684d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -20,6 +21,10 @@ #include "controller/pid/pid_calculator.hpp" +#include "deformable_chassis_joint_manager.hpp" +#include "deformable_chassis_mode_manager.hpp" +#include "deformable_chassis_suspension.hpp" + namespace rmcs_core::controller::chassis { class DeformableChassis @@ -27,9 +32,9 @@ class DeformableChassis , public rclcpp::Node { public: enum JointIndex : size_t { - kLeftFront = 0, - kLeftBack = 1, - kRightBack = 2, + kLeftFront = 0, + kLeftBack = 1, + kRightBack = 2, kRightFront = 3, kJointCount = 4, }; @@ -37,169 +42,107 @@ class DeformableChassis DeformableChassis() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) - - , min_angle_(get_parameter_or("min_angle", 7.0)) - , max_angle_(get_parameter_or("max_angle", 58.0)) - , active_suspension_base_angle_deg_(std::clamp( - get_parameter_or("active_suspension_base_angle", max_angle_), min_angle_ - 5.0, - max_angle_)) - , active_suspension_base_angle_scroll_step_deg_(std::max( - std::abs(get_parameter_or("active_suspension_base_angle_scroll_step_deg", 5.0)), - 0.0)) - , target_physical_velocity_limit_( - std::max( - deg_to_rad(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), - 1e-6)) - , target_physical_acceleration_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), - 1e-6)) - , active_suspension_enable_(get_parameter_or("active_suspension_enable", false)) - , suspension_velocity_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_velocity_limit_deg", - get_parameter_or("target_physical_velocity_limit", 180.0)))), - 1e-6)) - , suspension_acceleration_limit_( - std::max( - deg_to_rad( - std::abs(get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - get_parameter_or("target_physical_acceleration_limit", 720.0)))), - 1e-6)) - , suspension_correction_velocity_limit_( - std::max( - deg_to_rad(std::abs(get_parameter_or( - "active_suspension_correction_velocity_limit_deg", 720.0))), - 1e-6)) - , suspension_correction_acceleration_limit_( - std::max( - deg_to_rad(std::abs(get_parameter_or( - "active_suspension_correction_acceleration_limit_deg", 3600.0))), - 1e-6)) - , chassis_imu_calibration_wait_time_( - std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0)) - , chassis_imu_calibration_sample_time_( - std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6)) { + , spin_ratio_( + std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; - load_active_suspension_pid_( - "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, - -2.0, 2.0, -3.0, 3.0); - load_active_suspension_pid_( - "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, - -1.0, 1.0, -0.785, 0.785); - load_active_suspension_pid_( - "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, - -2.0, 2.0, -3.0, 3.0); - load_active_suspension_pid_( - "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, - -1.0, 1.0, -0.785, 0.785); + + suspension_.configure(*this); + joint_mgr_.configure(*this); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/rotary_knob", rotary_knob_); register_input("/predefined/update_rate", update_rate_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); - register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/manual_yaw_velocity_override", manual_yaw_velocity_override_, false); - - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); + "/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, + false); register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); + "/chassis/manual_yaw_velocity_override", + manual_yaw_velocity_override_, false); + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + register_input( + "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input( + "/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + fmt::format( + "/chassis/{}_joint/physical_angle", kJointName[i]), + joint_physical_angle_[i], false); + + register_output( + fmt::format( + "/chassis/{}_joint/target_physical_angle", + kJointName[i]), + joint_target_angle_[i], nan_); + + register_output( + fmt::format( + "/chassis/{}_joint/target_physical_velocity", + kJointName[i]), + joint_target_velocity_[i], nan_); + + register_output( + fmt::format( + "/chassis/{}_joint/target_physical_acceleration", + kJointName[i]), + joint_target_acceleration_[i], nan_); + + register_output( + fmt::format( + "/chassis/{}_joint/control_angle_error", + kJointName[i]), + joint_angle_error_[i], nan_); + } + + register_output( + "/gimbal/scope/control_torque", scope_motor_control_torque, + nan_); register_output("/chassis/angle", chassis_angle_, nan_); - register_output("/chassis/control_angle", chassis_control_angle_, nan_); + register_output( + "/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); - register_output("/chassis/control_velocity", chassis_control_velocity_); - register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); - register_output("/chassis/active_suspension/active", active_suspension_active_, false); - - register_output("/chassis/left_front_joint/control_angle_error", lf_angle_error_, nan_); - register_output("/chassis/left_back_joint/control_angle_error", lb_angle_error_, nan_); - register_output("/chassis/right_front_joint/control_angle_error", rf_angle_error_, nan_); - register_output("/chassis/right_back_joint/control_angle_error", rb_angle_error_, nan_); - - register_output( - "/chassis/left_front_joint/target_physical_angle", - left_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_angle", - left_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_angle", - right_back_joint_target_physical_angle_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_angle", - right_front_joint_target_physical_angle_, nan_); - register_output( - "/chassis/left_front_joint/target_physical_velocity", - left_front_joint_target_physical_velocity_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_velocity", - left_back_joint_target_physical_velocity_, nan_); register_output( - "/chassis/right_back_joint/target_physical_velocity", - right_back_joint_target_physical_velocity_, nan_); + "/chassis/control_velocity", chassis_control_velocity_); register_output( - "/chassis/right_front_joint/target_physical_velocity", - right_front_joint_target_physical_velocity_, nan_); + "/chassis/ctrl_hold_active", ctrl_hold_active_, false); register_output( - "/chassis/left_front_joint/target_physical_acceleration", - left_front_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/left_back_joint/target_physical_acceleration", - left_back_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/right_back_joint/target_physical_acceleration", - right_back_joint_target_physical_acceleration_, nan_); - register_output( - "/chassis/right_front_joint/target_physical_acceleration", - right_front_joint_target_physical_acceleration_, nan_); + "/chassis/active_suspension/active", + active_suspension_active_, false); - *mode_ = rmcs_msgs::ChassisMode::AUTO; + *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; chassis_control_velocity_->vector << nan_, nan_, nan_; - - current_target_angle_ = max_angle_; - lf_current_target_angle_ = max_angle_; - lb_current_target_angle_ = max_angle_; - rf_current_target_angle_ = max_angle_; - rb_current_target_angle_ = max_angle_; } void before_updating() override { if (!gimbal_yaw_angle_.ready()) { gimbal_yaw_angle_.make_and_bind_directly(0.0); - RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + RCLCPP_WARN( + get_logger(), + "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + get_logger(), + "Failed to fetch \"/gimbal/yaw/control_angle_error\". " + "Set to 0.0."); } if (!manual_yaw_velocity_override_.ready()) manual_yaw_velocity_override_.make_and_bind_directly(nan_); @@ -218,560 +161,95 @@ class DeformableChassis using rmcs_msgs::Switch; const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; do { - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + if ((switch_left == Switch::UNKNOWN + || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN + && switch_right == Switch::DOWN)) { reset_all_controls(); break; } - update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); - *ctrl_hold_active_ = ctrl_hold_requested_by_input_(); - *active_suspension_active_ = suspension_requested_by_input_(); + double rotary_knob = + rotary_knob_.ready() ? *rotary_knob_ : 0.0; + + mode_mgr_.update( + switch_left, switch_right, keyboard, + rotary_knob, update_dt()); + + const auto& mm = mode_mgr_.result(); + *mode_ = mm.mode; + *ctrl_hold_active_ = mm.ctrl_hold_active; + *active_suspension_active_ = mm.suspension_active; + update_velocity_control(); - update_lift_target_toggle(keyboard); - update_active_suspension_base_angle_from_mouse_wheel_(); run_joint_intent_pipeline_(); } while (false); - - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; } private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr const char* kJointName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; - static constexpr double angular_velocity_max_ = 30.0; - static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; - static constexpr double imu_calibration_offset_limit_rad_ = - 1.0 * std::numbers::pi / 180.0; - - void load_active_suspension_pid_( - const std::string& prefix, pid::PidCalculator& pid, double kp_default, double ki_default, - double kd_default, double integral_min_default, double integral_max_default, - double output_min_default, double output_max_default) { - pid.kp = get_parameter_or(prefix + "kp", kp_default); - pid.ki = get_parameter_or(prefix + "ki", ki_default); - pid.kd = get_parameter_or(prefix + "kd", kd_default); - pid.integral_min = get_parameter_or(prefix + "integral_min", integral_min_default); - pid.integral_max = get_parameter_or(prefix + "integral_max", integral_max_default); - pid.output_min = get_parameter_or(prefix + "output_min", output_min_default); - pid.output_max = get_parameter_or(prefix + "output_max", output_max_default); - } + static constexpr double angular_velocity_max_ = 30.0; + static constexpr double default_dt_ = 1e-3; void validate_joint_feedback_inputs() const { - if (left_front_joint_physical_angle_.ready() && left_back_joint_physical_angle_.ready() - && right_front_joint_physical_angle_.ready() && right_back_joint_physical_angle_.ready()) - return; - - throw std::runtime_error( - "missing deformable chassis feedback interfaces: expected /chassis/*_joint/physical_angle"); - } - - void update_mode_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard) { - auto mode = *mode_; - const bool q_pressed = keyboard.q; - const bool e_pressed = keyboard.e; - const bool last_q_pressed = last_keyboard_.q; - const bool last_e_pressed = last_keyboard_.e; - const bool last_c_pressed = last_keyboard_.c; - const bool qe_combo_pressed = q_pressed && e_pressed; - const bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; - const bool c_rising = !last_c_pressed && keyboard.c; - const bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; - if (switch_left == rmcs_msgs::Switch::DOWN) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - return; - } - - if (qe_complex_spin_active_) { - if (c_rising) { - deactivate_qe_complex_spin_(); - apply_symmetric_target = true; - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - mode = rmcs_msgs::ChassisMode::SPIN; - } - } else if (qe_combo_rising) { - deactivate_complex_spin_(); - activate_qe_complex_spin_(mode); - } else if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE - && switch_right == rmcs_msgs::Switch::DOWN) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::STEP_DOWN; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } else if (!last_keyboard_.c && keyboard.c) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::AUTO; - } else { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } else if (!last_keyboard_.z && keyboard.z) { - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; - } - - if (complex_spin_active_ || qe_complex_spin_active_) - mode = rmcs_msgs::ChassisMode::SPIN; - - *mode_ = mode; - } - - void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { - complex_spin_active_ = true; - complex_spin_elapsed_ = 0.0; - apply_symmetric_target = true; - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } - - void deactivate_complex_spin_() { - complex_spin_active_ = false; - complex_spin_elapsed_ = 0.0; - } - - void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { - qe_complex_spin_active_ = true; - qe_last_toggle_elapsed_ = 0.0; - qe_front_high_rear_low_ = true; - apply_front_high_rear_low_target_(); - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - spinning_forward_ = !spinning_forward_; - } - } - - void deactivate_qe_complex_spin_() { - qe_complex_spin_active_ = false; - qe_last_toggle_elapsed_ = 0.0; - } - - void apply_front_high_rear_low_target_() { - lf_current_target_angle_ = max_angle_; - rf_current_target_angle_ = max_angle_; - lb_current_target_angle_ = min_angle_; - rb_current_target_angle_ = min_angle_; - apply_symmetric_target = false; - qe_front_high_rear_low_ = true; - } - - void apply_front_low_rear_high_target_() { - lf_current_target_angle_ = min_angle_; - rf_current_target_angle_ = min_angle_; - lb_current_target_angle_ = max_angle_; - rb_current_target_angle_ = max_angle_; - apply_symmetric_target = false; - qe_front_high_rear_low_ = false; - } - - void toggle_bg_target_() { - if (qe_front_high_rear_low_) { - apply_front_low_rear_high_target_(); - } else { - apply_front_high_rear_low_target_(); - } - } - - void toggle_qe_complex_spin_target_() { - toggle_bg_target_(); - } - - void update_qe_complex_spin_toggle_() { - constexpr double qe_complex_spin_toggle_period = 1.0; - - qe_last_toggle_elapsed_ += update_dt(); - size_t qe_complex_spin_toggle_count = 0; - while (qe_last_toggle_elapsed_ >= qe_complex_spin_toggle_period) { - qe_last_toggle_elapsed_ -= qe_complex_spin_toggle_period; - ++qe_complex_spin_toggle_count; - } - - if ((qe_complex_spin_toggle_count % 2) == 1) - toggle_qe_complex_spin_target_(); - } - - std::array read_current_joint_physical_angles_() const { - const std::array*, kJointCount> physical_angle_inputs{ - &left_front_joint_physical_angle_, &left_back_joint_physical_angle_, - &right_back_joint_physical_angle_, &right_front_joint_physical_angle_}; - - std::array current_physical_angles{}; - current_physical_angles.fill(nan_); - for (size_t i = 0; i < kJointCount; ++i) { - if (physical_angle_inputs[i]->ready() && std::isfinite(*(*physical_angle_inputs[i]))) { - current_physical_angles[i] = *(*physical_angle_inputs[i]); - } - } - - return current_physical_angles; - } - - bool suspension_toggle_requested_by_keyboard_(const rmcs_msgs::Keyboard& keyboard) const { - return !last_keyboard_.e && keyboard.e && !keyboard.q; - } - - bool suspension_toggle_requested_by_switch_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { - return switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP - && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; - } - - void update_suspension_toggle_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard) { - if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) { - active_suspension_on_by_remote_ = !active_suspension_on_by_remote_; - } - if (suspension_toggle_requested_by_keyboard_(keyboard)) { - active_suspension_on_by_keyboard_ = !active_suspension_on_by_keyboard_; - } - } - - bool prone_override_requested_by_keyboard() const { - return keyboard_.ready() && keyboard_->ctrl; - } - - bool ctrl_hold_requested_by_input_() const { - return prone_override_requested_by_keyboard() || active_suspension_on_by_remote_ - || active_suspension_on_by_keyboard_; - } - - bool low_prone_override_active_() const { - return prone_override_requested_by_keyboard(); - } - - bool suspension_requested_by_input_() const { - return active_suspension_enable_ && ctrl_hold_requested_by_input_(); - } - - bool symmetric_joint_target_requested_() const { - constexpr double epsilon = 1e-6; - return std::abs(lf_current_target_angle_ - lb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rb_current_target_angle_) <= epsilon - && std::abs(lf_current_target_angle_ - rf_current_target_angle_) <= epsilon; - } - - void reset_attitude_correction_state_() { - pitch_outer_pid_.reset(); - pitch_inner_pid_.reset(); - roll_outer_pid_.reset(); - roll_inner_pid_.reset(); - joint_suspension_active_.fill(false); - suspension_correction_target_rad_.fill(0.0); - } - - void reset_active_suspension_correction_state_() { - suspension_correction_target_rad_.fill(0.0); - suspension_correction_state_rad_.fill(0.0); - suspension_correction_velocity_state_rad_.fill(0.0); - suspension_correction_acceleration_state_rad_.fill(0.0); - } - - void reset_chassis_imu_calibration_window_() { - chassis_imu_calibration_hold_elapsed_ = 0.0; - chassis_imu_calibration_sample_count_ = 0; - chassis_imu_pitch_sum_ = 0.0; - chassis_imu_roll_sum_ = 0.0; - chassis_imu_calibration_completed_for_window_ = false; - } - - void update_chassis_imu_calibration_() { - if (chassis_imu_calibrated_once_) - return; - - if (!symmetric_joint_target_requested_()) { - reset_chassis_imu_calibration_window_(); - return; - } - - const double raw_pitch = *chassis_imu_pitch_; - const double raw_roll = *chassis_imu_roll_; - if (!std::isfinite(raw_pitch) || !std::isfinite(raw_roll)) - return; - - chassis_imu_calibration_hold_elapsed_ += update_dt(); - if (chassis_imu_calibration_hold_elapsed_ < chassis_imu_calibration_wait_time_) - return; - - const double calibration_end_time = - chassis_imu_calibration_wait_time_ + chassis_imu_calibration_sample_time_; - if (chassis_imu_calibration_hold_elapsed_ < calibration_end_time) { - chassis_imu_pitch_sum_ += raw_pitch; - chassis_imu_roll_sum_ += raw_roll; - ++chassis_imu_calibration_sample_count_; - return; - } - - if (chassis_imu_calibration_completed_for_window_) - return; - - chassis_imu_calibration_completed_for_window_ = true; - if (chassis_imu_calibration_sample_count_ == 0) { - RCLCPP_WARN( - get_logger(), - "[chassis imu calibration] skipped because no valid samples were collected"); - return; - } - - chassis_imu_pitch_offset_ = std::clamp( - chassis_imu_pitch_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_roll_offset_ = std::clamp( - chassis_imu_roll_sum_ / static_cast(chassis_imu_calibration_sample_count_), - -imu_calibration_offset_limit_rad_, imu_calibration_offset_limit_rad_); - chassis_imu_calibrated_once_ = true; - RCLCPP_INFO( - get_logger(), - "[chassis imu calibration] pitch_offset=% .3f deg roll_offset=% .3f deg " - "(samples=%zu)", - chassis_imu_pitch_offset_ * rad_to_deg_, chassis_imu_roll_offset_ * rad_to_deg_, - chassis_imu_calibration_sample_count_); - } - - bool ensure_joint_target_states_from_feedback( - const std::array& current_physical_angles) { - bool any_active = false; - for (size_t i = 0; i < kJointCount; ++i) { - if (std::isfinite(current_physical_angles[i]) && !joint_target_active_[i]) { - joint_target_physical_angle_state_rad_[i] = current_physical_angles[i]; - joint_target_physical_velocity_state_rad_[i] = 0.0; - joint_target_physical_acceleration_state_rad_[i] = 0.0; - current_target_physical_angles_rad_[i] = current_physical_angles[i]; - joint_target_active_[i] = true; - } - - any_active = any_active || joint_target_active_[i]; - } - - return any_active; - } - - bool any_joint_target_active_() const { - return std::any_of( - joint_target_active_.begin(), joint_target_active_.end(), [](bool active) { - return active; - }); - } - - double active_suspension_min_angle_deg_() const { return min_angle_ - 5.0; } - - double active_suspension_min_angle_rad_() const { - return deg_to_rad(active_suspension_min_angle_deg_()); - } - - double active_suspension_base_angle_rad_() const { - return deg_to_rad(active_suspension_base_angle_deg_); - } - - bool active_suspension_correction_inverted_() const { - const double midpoint = (active_suspension_min_angle_deg_() + max_angle_) / 2.0; - return active_suspension_base_angle_deg_ > midpoint; - } - - void update_active_suspension_base_angle_from_mouse_wheel_() { - if (!suspension_requested_by_input_() || low_prone_override_active_() || !mouse_wheel_.ready()) - return; - - const double mouse_wheel = *mouse_wheel_; - if (!std::isfinite(mouse_wheel) || std::abs(mouse_wheel) <= 1e-9) - return; - - active_suspension_base_angle_deg_ = std::clamp( - active_suspension_base_angle_deg_ - - mouse_wheel * active_suspension_base_angle_scroll_step_deg_, - active_suspension_min_angle_deg_(), max_angle_); - } - - void update_active_suspension_() { - if (!suspension_requested_by_input_()) { - reset_attitude_correction_state_(); - return; - } - - constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - const double corrected_pitch = - std::clamp(*chassis_imu_pitch_ - chassis_imu_pitch_offset_, -max_attitude, max_attitude); - const double corrected_roll = - std::clamp(*chassis_imu_roll_ - chassis_imu_roll_offset_, -max_attitude, max_attitude); - const double corrected_pitch_rate = *chassis_imu_pitch_rate_; - const double corrected_roll_rate = *chassis_imu_roll_rate_; - - const double pitch_outer = pitch_outer_pid_.update(-corrected_pitch); - const double roll_outer = roll_outer_pid_.update(corrected_roll); - const double pitch_angle_diff = pitch_inner_pid_.update(pitch_outer - corrected_pitch_rate); - const double roll_angle_diff = roll_inner_pid_.update(roll_outer + corrected_roll_rate); - if (!std::isfinite(pitch_angle_diff) || !std::isfinite(roll_angle_diff)) { - reset_attitude_correction_state_(); - return; - } - - if (active_suspension_correction_inverted_()) { - const double front_pitch_drop = std::max(pitch_angle_diff, 0.0); - const double back_pitch_drop = std::max(-pitch_angle_diff, 0.0); - const double left_roll_drop = std::max(-roll_angle_diff, 0.0); - const double right_roll_drop = std::max(roll_angle_diff, 0.0); - - suspension_correction_target_rad_[kLeftFront] = - -(front_pitch_drop + left_roll_drop); - suspension_correction_target_rad_[kLeftBack] = -(back_pitch_drop + left_roll_drop); - suspension_correction_target_rad_[kRightBack] = -(back_pitch_drop + right_roll_drop); - suspension_correction_target_rad_[kRightFront] = - -(front_pitch_drop + right_roll_drop); - } else { - const double front_pitch_add = std::max(-pitch_angle_diff, 0.0); - const double back_pitch_add = std::max(pitch_angle_diff, 0.0); - const double left_roll_add = std::max(roll_angle_diff, 0.0); - const double right_roll_add = std::max(-roll_angle_diff, 0.0); - - suspension_correction_target_rad_[kLeftFront] = front_pitch_add + left_roll_add; - suspension_correction_target_rad_[kLeftBack] = back_pitch_add + left_roll_add; - suspension_correction_target_rad_[kRightBack] = back_pitch_add + right_roll_add; - suspension_correction_target_rad_[kRightFront] = front_pitch_add + right_roll_add; - } - - joint_suspension_active_.fill(true); - } - - void update_active_suspension_correction_trajectory() { - const double dt = update_dt(); - const double max_target_angle = deg_to_rad(max_angle_); - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_suspension_active_[i]) { - suspension_correction_target_rad_[i] = 0.0; - } - - const double base_angle = std::isfinite(joint_target_physical_angle_state_rad_[i]) - ? joint_target_physical_angle_state_rad_[i] - : (low_prone_override_active_() ? active_suspension_min_angle_rad_() - : active_suspension_base_angle_rad_()); - const double correction_min = active_suspension_min_angle_rad_() - base_angle; - const double correction_max = max_target_angle - base_angle; - const double target = std::clamp( - suspension_correction_target_rad_[i], correction_min, correction_max); - - double& angle_state = suspension_correction_state_rad_[i]; - double& velocity_state = suspension_correction_velocity_state_rad_[i]; - double& acceleration_state = suspension_correction_acceleration_state_rad_[i]; - const double position_error = target - angle_state; - const double stopping_distance = - velocity_state * velocity_state / (2.0 * suspension_correction_acceleration_limit_); - - double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(suspension_correction_velocity_limit_, position_error); - } - - const double velocity_error = desired_velocity - velocity_state; - acceleration_state = std::clamp( - velocity_error / dt, -suspension_correction_acceleration_limit_, - suspension_correction_acceleration_limit_); - - velocity_state += acceleration_state * dt; - velocity_state = std::clamp( - velocity_state, -suspension_correction_velocity_limit_, - suspension_correction_velocity_limit_); - angle_state += velocity_state * dt; - - const double next_error = target - angle_state; - if ((position_error > 0.0 && next_error < 0.0) - || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { - angle_state = target; - velocity_state = 0.0; - acceleration_state = 0.0; - } - } + for (size_t i = 0; i < kJointCount; ++i) + if (!joint_physical_angle_[i].ready()) + throw std::runtime_error( + "missing deformable chassis feedback " + "interfaces: expected " + "/chassis/*_joint/physical_angle"); } void reset_all_controls() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; + *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; - reset_attitude_correction_state_(); - reset_chassis_imu_calibration_window_(); + + mode_mgr_.reset(); + suspension_.reset(); + joint_mgr_.reset(); chassis_control_velocity_->vector << nan_, nan_, nan_; - *chassis_angle_ = nan_; + *chassis_angle_ = nan_; *chassis_control_angle_ = nan_; - current_target_angle_ = max_angle_; - active_suspension_base_angle_deg_ = max_angle_; - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - joint_target_active_.fill(false); - current_target_physical_angles_rad_.fill(nan_); - joint_target_physical_angle_state_rad_.fill(nan_); - joint_target_physical_velocity_state_rad_.fill(0.0); - joint_target_physical_acceleration_state_rad_.fill(0.0); - reset_active_suspension_correction_state_(); - active_suspension_on_by_keyboard_ = false; - active_suspension_on_by_remote_ = false; - deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - *scope_motor_control_torque = nan_; - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rf_angle_error_ = nan_; - *rb_angle_error_ = nan_; - - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; - - } - - void update_velocity_control() { - const Eigen::Vector2d translational_velocity = update_translational_velocity_control(); - const double angular_velocity = update_angular_velocity_control(); - chassis_control_velocity_->vector << translational_velocity, angular_velocity; + publish_nan_joint_targets(); } double update_dt() const { - if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + if (update_rate_.ready() && std::isfinite(*update_rate_) + && *update_rate_ > 1e-6) return 1.0 / *update_rate_; return default_dt_; } + void update_velocity_control() { + Eigen::Vector2d translational_velocity = + update_translational_velocity_control(); + double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, + angular_velocity; + } + Eigen::Vector2d update_translational_velocity_control() { const auto keyboard = *keyboard_; - const Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + Eigen::Vector2d keyboard_move{ + keyboard.w - keyboard.s, keyboard.a - keyboard.d}; Eigen::Vector2d translational_velocity = - Eigen::Rotation2Dd{*gimbal_yaw_angle_} * ((*joystick_right_) + keyboard_move); + Eigen::Rotation2Dd{*gimbal_yaw_angle_} + * (*joystick_right_ + keyboard_move); if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); @@ -781,23 +259,27 @@ class DeformableChassis } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan_; switch (*mode_) { case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { + bool forward = mode_mgr_.result().spinning_forward; angular_velocity = - spin_ratio_ * (spinning_forward_ ? angular_velocity_max_ : -angular_velocity_max_); - angular_velocity = - std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); + spin_ratio_ + * (forward ? angular_velocity_max_ + : -angular_velocity_max_); + angular_velocity = std::clamp( + angular_velocity, -angular_velocity_max_, + angular_velocity_max_); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + double err = calculate_unsigned_chassis_angle_error( + chassis_control_angle); - // In step-down mode, front/back can both be used for alignment. constexpr double alignment = std::numbers::pi; while (err > alignment / 2) { chassis_control_angle -= alignment; @@ -806,7 +288,8 @@ class DeformableChassis err -= alignment; } - angular_velocity = following_velocity_controller_.update(err); + angular_velocity = + following_velocity_controller_.update(err); } break; default: break; @@ -818,7 +301,8 @@ class DeformableChassis return angular_velocity; } - double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + double calculate_unsigned_chassis_angle_error( + double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; @@ -830,264 +314,142 @@ class DeformableChassis return err; } - void update_lift_target_toggle(rmcs_msgs::Keyboard keyboard) { - constexpr double rotary_knob_symmetric_edge_threshold = 0.7; - constexpr double rotary_knob_bg_edge_threshold = -0.9; - constexpr double complex_spin_toggle_period = 0.5; - - const bool keyboard_toggle_condition = - !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q && !keyboard.e; - const bool rotary_knob_toggle_condition = - last_rotary_knob_ < rotary_knob_symmetric_edge_threshold - && *rotary_knob_ >= rotary_knob_symmetric_edge_threshold; - const bool rotary_knob_bg_toggle_condition = - !qe_complex_spin_active_ && last_rotary_knob_ > rotary_knob_bg_edge_threshold - && *rotary_knob_ <= rotary_knob_bg_edge_threshold; - const bool front_high_rear_low = !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; - bool complex_spin_toggle_condition = false; - - if (complex_spin_active_) { - complex_spin_elapsed_ += update_dt(); - size_t complex_spin_toggle_count = 0; - while (complex_spin_elapsed_ >= complex_spin_toggle_period) { - complex_spin_elapsed_ -= complex_spin_toggle_period; - ++complex_spin_toggle_count; - } - complex_spin_toggle_condition = (complex_spin_toggle_count % 2) == 1; - } + static double deg_to_rad(double deg) { + return deg * std::numbers::pi / 180.0; + } - if (qe_complex_spin_active_) - update_qe_complex_spin_toggle_(); + std::array read_feedback_() const { + std::array angles; + angles.fill(nan_); - if (apply_symmetric_target) { - lf_current_target_angle_ = current_target_angle_; - lb_current_target_angle_ = current_target_angle_; - rb_current_target_angle_ = current_target_angle_; - rf_current_target_angle_ = current_target_angle_; - } + for (size_t i = 0; i < kJointCount; ++i) + if (joint_physical_angle_[i].ready() + && std::isfinite(*joint_physical_angle_[i])) + angles[i] = *joint_physical_angle_[i]; - if (rotary_knob_toggle_condition || keyboard_toggle_condition || complex_spin_toggle_condition) { - current_target_angle_ = - (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; - apply_symmetric_target = true; - } else if (rotary_knob_bg_toggle_condition) { - toggle_bg_target_(); - } else if (front_high_rear_low) { - apply_front_high_rear_low_target_(); - } else if (front_low_rear_high) { - apply_front_low_rear_high_target_(); + return angles; + } + + std::array compute_target_angles_rad_() const { + const auto& mm = mode_mgr_.result(); + std::array targets_deg = mm.joint_target_deg; + + std::array targets_rad; + for (size_t i = 0; i < kJointCount; ++i) + targets_rad[i] = deg_to_rad(targets_deg[i]); + + if (mm.suspension_active) { + double base = mm.low_prone + ? mode_mgr_.active_suspension_min_angle_rad() + : deg_to_rad(mm.base_angle_deg); + targets_rad.fill(base); } - last_rotary_knob_ = *rotary_knob_; + return targets_rad; } - // Chassis owns the high-level joint intent pipeline: read feedback, generate deploy targets, - // coordinate suspension overrides, then publish the resulting joint intent for the servo layer. void run_joint_intent_pipeline_() { - const auto current_physical_angles = read_current_joint_physical_angles_(); - const bool suspension_requested = suspension_requested_by_input_(); + const auto current_physical_angles = read_feedback_(); + const auto& mm = mode_mgr_.result(); - if (!ensure_joint_target_states_from_feedback(current_physical_angles)) { + if (!joint_mgr_.init_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); return; } - current_target_physical_angles_rad_[kLeftFront] = deg_to_rad(lf_current_target_angle_); - current_target_physical_angles_rad_[kLeftBack] = deg_to_rad(lb_current_target_angle_); - current_target_physical_angles_rad_[kRightBack] = deg_to_rad(rb_current_target_angle_); - current_target_physical_angles_rad_[kRightFront] = deg_to_rad(rf_current_target_angle_); - if (suspension_requested) { - current_target_physical_angles_rad_.fill( - low_prone_override_active_() ? active_suspension_min_angle_rad_() - : active_suspension_base_angle_rad_()); - } - - update_chassis_imu_calibration_(); - scope_motor_control(suspension_requested); - update_active_suspension_(); - - update_joint_target_trajectory(); - update_active_suspension_correction_trajectory(); - publish_joint_target_angles(current_physical_angles); - } + std::array target_angles_rad = + compute_target_angles_rad_(); - static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } + suspension_.calibrate( + *chassis_imu_pitch_, *chassis_imu_roll_, + mm.symmetric_target, update_dt()); - void scope_motor_control(bool suspension_requested = false) { - const bool prone_target_active = suspension_requested; - if (prone_target_active && *mode_ != rmcs_msgs::ChassisMode::SPIN) { - *scope_motor_control_torque = -0.3; - } else { - *scope_motor_control_torque = 0.3; - } - } + *scope_motor_control_torque = suspension_.scope_torque( + mm.suspension_active, + mm.mode == rmcs_msgs::ChassisMode::SPIN); - void update_joint_target_trajectory() { - const double dt = update_dt(); - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_target_active_[i]) - continue; + auto corrections = suspension_.update( + *chassis_imu_pitch_ - suspension_.pitch_offset(), + *chassis_imu_roll_ - suspension_.roll_offset(), + *chassis_imu_pitch_rate_, *chassis_imu_roll_rate_, + mm.suspension_active, mm.low_prone, + mode_mgr_.min_angle(), mode_mgr_.max_angle(), + mm.base_angle_deg, mode_mgr_.correction_inverted(), + joint_mgr_.angle_states(), update_dt()); - double& angle_state = joint_target_physical_angle_state_rad_[i]; - double& velocity_state = joint_target_physical_velocity_state_rad_[i]; - double& acceleration_state = joint_target_physical_acceleration_state_rad_[i]; - const double target_angle = current_target_physical_angles_rad_[i]; - const double velocity_limit = joint_suspension_active_[i] - ? suspension_velocity_limit_ - : target_physical_velocity_limit_; - const double acceleration_limit = - joint_suspension_active_[i] ? suspension_acceleration_limit_ - : target_physical_acceleration_limit_; - - if (!std::isfinite(target_angle) || !std::isfinite(angle_state)) { - continue; - } + joint_mgr_.run_trajectory( + target_angles_rad, mm.suspension_active, update_dt()); - const double position_error = target_angle - angle_state; - const double stopping_distance = - velocity_state * velocity_state / (2.0 * acceleration_limit); + publish_joint_targets_(current_physical_angles, corrections); + } - double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(velocity_limit, position_error); - } + void publish_joint_targets_( + const std::array& feedback_angles, + const DeformableChassisActiveSuspension::Corrections& corr) { - const double velocity_error = desired_velocity - velocity_state; - acceleration_state = - std::clamp(velocity_error / dt, -acceleration_limit, acceleration_limit); - - velocity_state += acceleration_state * dt; - velocity_state = std::clamp(velocity_state, -velocity_limit, velocity_limit); - angle_state += velocity_state * dt; - - const double next_error = target_angle - angle_state; - if ((position_error > 0.0 && next_error < 0.0) - || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { - angle_state = target_angle; - velocity_state = 0.0; - acceleration_state = 0.0; - } - } - } + double min_angle_rad = + mode_mgr_.active_suspension_min_angle_rad(); + double max_angle_rad = mode_mgr_.max_angle_rad(); - void publish_joint_target_angles( - const std::array& current_physical_angles) { - if (!any_joint_target_active_()) { + if (!joint_mgr_.any_active()) { publish_nan_joint_targets(); return; } - const auto publish_joint = [this, ¤t_physical_angles]( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output, - OutputInterface& acceleration_output, - OutputInterface& angle_error_output) { - if (!joint_target_active_[index]) { - *angle_output = nan_; - *velocity_output = nan_; - *acceleration_output = nan_; - *angle_error_output = nan_; - return; + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_mgr_.joint_active(i)) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + continue; } - const double target_angle = joint_target_physical_angle_state_rad_[index] - + suspension_correction_state_rad_[index]; - *angle_output = std::clamp( - target_angle, active_suspension_min_angle_rad_(), deg_to_rad(max_angle_)); - *velocity_output = joint_target_physical_velocity_state_rad_[index] - + suspension_correction_velocity_state_rad_[index]; - *acceleration_output = joint_target_physical_acceleration_state_rad_[index] - + suspension_correction_acceleration_state_rad_[index]; - *angle_error_output = std::isfinite(current_physical_angles[index]) - ? current_physical_angles[index] - - *angle_output - : nan_; - }; - - publish_joint( - kLeftFront, left_front_joint_target_physical_angle_, - left_front_joint_target_physical_velocity_, - left_front_joint_target_physical_acceleration_, lf_angle_error_); - publish_joint( - kLeftBack, left_back_joint_target_physical_angle_, - left_back_joint_target_physical_velocity_, - left_back_joint_target_physical_acceleration_, lb_angle_error_); - publish_joint( - kRightBack, right_back_joint_target_physical_angle_, - right_back_joint_target_physical_velocity_, - right_back_joint_target_physical_acceleration_, rb_angle_error_); - publish_joint( - kRightFront, right_front_joint_target_physical_angle_, - right_front_joint_target_physical_velocity_, - right_front_joint_target_physical_acceleration_, rf_angle_error_); + double target = + joint_mgr_.angle_state(i) + corr.angle[i]; + *joint_target_angle_[i] = std::clamp( + target, min_angle_rad, max_angle_rad); + *joint_target_velocity_[i] = + joint_mgr_.velocity_state(i) + corr.velocity[i]; + *joint_target_acceleration_[i] = + joint_mgr_.acceleration_state(i) + + corr.acceleration[i]; + *joint_angle_error_[i] = + std::isfinite(feedback_angles[i]) + ? feedback_angles[i] - *joint_target_angle_[i] + : nan_; + } } void publish_nan_joint_targets() { - reset_attitude_correction_state_(); - reset_active_suspension_correction_state_(); - joint_target_active_.fill(false); - - *left_front_joint_target_physical_angle_ = nan_; - *left_back_joint_target_physical_angle_ = nan_; - *right_back_joint_target_physical_angle_ = nan_; - *right_front_joint_target_physical_angle_ = nan_; - - *left_front_joint_target_physical_velocity_ = nan_; - *left_back_joint_target_physical_velocity_ = nan_; - *right_back_joint_target_physical_velocity_ = nan_; - *right_front_joint_target_physical_velocity_ = nan_; - - *left_front_joint_target_physical_acceleration_ = nan_; - *left_back_joint_target_physical_acceleration_ = nan_; - *right_back_joint_target_physical_acceleration_ = nan_; - *right_front_joint_target_physical_acceleration_ = nan_; - - *lf_angle_error_ = nan_; - *lb_angle_error_ = nan_; - *rb_angle_error_ = nan_; - *rf_angle_error_ = nan_; + suspension_.reset(); + joint_mgr_.reset(); + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } } -private: InputInterface joystick_right_; InputInterface switch_right_; InputInterface switch_left_; InputInterface keyboard_; - InputInterface mouse_wheel_; InputInterface rotary_knob_; InputInterface update_rate_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - double last_rotary_knob_ = 0.0; - InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; InputInterface manual_yaw_velocity_override_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; - OutputInterface chassis_control_velocity_; + OutputInterface + chassis_control_velocity_; OutputInterface ctrl_hold_active_; OutputInterface active_suspension_active_; - bool spinning_forward_ = true; - bool apply_symmetric_target = true; - bool complex_spin_active_ = false; - double complex_spin_elapsed_ = 0.0; - bool qe_complex_spin_active_ = false; - bool qe_front_high_rear_low_ = true; - double qe_last_toggle_elapsed_ = 0.0; - pid::PidCalculator following_velocity_controller_; - const double spin_ratio_; - - InputInterface left_front_joint_physical_angle_; - InputInterface left_back_joint_physical_angle_; - InputInterface right_front_joint_physical_angle_; - InputInterface right_back_joint_physical_angle_; InputInterface chassis_imu_pitch_; InputInterface chassis_imu_roll_; InputInterface chassis_imu_pitch_rate_; @@ -1095,76 +457,29 @@ class DeformableChassis OutputInterface scope_motor_control_torque; - OutputInterface lf_angle_error_; - OutputInterface lb_angle_error_; - OutputInterface rf_angle_error_; - OutputInterface rb_angle_error_; - - OutputInterface left_front_joint_target_physical_angle_; - OutputInterface left_back_joint_target_physical_angle_; - OutputInterface right_back_joint_target_physical_angle_; - OutputInterface right_front_joint_target_physical_angle_; - OutputInterface left_front_joint_target_physical_velocity_; - OutputInterface left_back_joint_target_physical_velocity_; - OutputInterface right_back_joint_target_physical_velocity_; - OutputInterface right_front_joint_target_physical_velocity_; - OutputInterface left_front_joint_target_physical_acceleration_; - OutputInterface left_back_joint_target_physical_acceleration_; - OutputInterface right_back_joint_target_physical_acceleration_; - OutputInterface right_front_joint_target_physical_acceleration_; - - double min_angle_; - double max_angle_; - double active_suspension_base_angle_deg_; - double active_suspension_base_angle_scroll_step_deg_; - - double current_target_angle_; - double lf_current_target_angle_, lb_current_target_angle_, rb_current_target_angle_, - rf_current_target_angle_; - - std::array current_target_physical_angles_rad_ = {0.0, 0.0, 0.0, 0.0}; - - std::array joint_target_active_ = {false, false, false, false}; - std::array joint_target_physical_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array joint_target_physical_velocity_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array joint_target_physical_acceleration_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - - double target_physical_velocity_limit_; - double target_physical_acceleration_limit_; - bool active_suspension_enable_; - bool active_suspension_on_by_keyboard_ = false; - bool active_suspension_on_by_remote_ = false; - double suspension_velocity_limit_; - double suspension_acceleration_limit_; - double suspension_correction_velocity_limit_; - double suspension_correction_acceleration_limit_; - std::array joint_suspension_active_ = {false, false, false, false}; - std::array suspension_correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array suspension_correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array suspension_correction_velocity_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array suspension_correction_acceleration_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - pid::PidCalculator pitch_outer_pid_{}; - pid::PidCalculator pitch_inner_pid_{}; - pid::PidCalculator roll_outer_pid_{}; - pid::PidCalculator roll_inner_pid_{}; - double chassis_imu_pitch_offset_ = 0.0; - double chassis_imu_roll_offset_ = 0.0; - double chassis_imu_calibration_wait_time_; - double chassis_imu_calibration_sample_time_; - double chassis_imu_calibration_hold_elapsed_ = 0.0; - size_t chassis_imu_calibration_sample_count_ = 0; - double chassis_imu_pitch_sum_ = 0.0; - double chassis_imu_roll_sum_ = 0.0; - bool chassis_imu_calibration_completed_for_window_ = false; - bool chassis_imu_calibrated_once_ = false; - static constexpr double default_dt_ = 1e-3; + std::array, kJointCount> + joint_physical_angle_; + std::array, kJointCount> + joint_target_angle_; + std::array, kJointCount> + joint_target_velocity_; + std::array, kJointCount> + joint_target_acceleration_; + std::array, kJointCount> + joint_angle_error_; + + pid::PidCalculator following_velocity_controller_; + const double spin_ratio_; + + DeformableChassisModeManager mode_mgr_; + DeformableChassisActiveSuspension suspension_; + DeformableChassisJointManager joint_mgr_; }; } // namespace rmcs_core::controller::chassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableChassis, + rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp new file mode 100644 index 00000000..75c8b077 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp @@ -0,0 +1,161 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::controller::chassis { + +class DeformableChassisJointManager { +public: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + DeformableChassisJointManager() = default; + + void configure(rclcpp::Node& node) { + target_vel_limit_ = std::max( + deg_to_rad_(std::abs( + node.get_parameter_or("target_physical_velocity_limit", 180.0))), + 1e-6); + target_acc_limit_ = std::max( + deg_to_rad_(std::abs( + node.get_parameter_or("target_physical_acceleration_limit", 720.0))), + 1e-6); + suspension_vel_limit_ = std::max( + deg_to_rad_(std::abs(node.get_parameter_or( + "active_suspension_target_velocity_limit_deg", + node.get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6); + suspension_acc_limit_ = std::max( + deg_to_rad_(std::abs(node.get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + node.get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6); + } + + void reset() { + joint_target_active_.fill(false); + target_angle_state_rad_.fill(nan_); + target_velocity_state_rad_.fill(0.0); + target_acceleration_state_rad_.fill(0.0); + } + + bool init_from_feedback( + const std::array& physical_angles) { + + bool any_active = false; + for (size_t i = 0; i < kJointCount; ++i) { + if (std::isfinite(physical_angles[i]) && !joint_target_active_[i]) { + target_angle_state_rad_[i] = physical_angles[i]; + target_velocity_state_rad_[i] = 0.0; + target_acceleration_state_rad_[i] = 0.0; + joint_target_active_[i] = true; + } + any_active = any_active || joint_target_active_[i]; + } + return any_active; + } + + bool any_active() const { + for (size_t i = 0; i < kJointCount; ++i) + if (joint_target_active_[i]) + return true; + return false; + } + + void run_trajectory( + const std::array& target_angles_rad, + bool suspension_active, + double dt) { + + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) + continue; + + double& angle_st = target_angle_state_rad_[i]; + double& vel_st = target_velocity_state_rad_[i]; + double& acc_st = target_acceleration_state_rad_[i]; + double target = target_angles_rad[i]; + + double vel_limit = suspension_active + ? suspension_vel_limit_ + : target_vel_limit_; + double acc_limit = suspension_active + ? suspension_acc_limit_ + : target_acc_limit_; + + if (!std::isfinite(target) || !std::isfinite(angle_st)) + continue; + + double position_error = target - angle_st; + double stopping_distance = + vel_st * vel_st / (2.0 * acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 + && std::abs(position_error) > stopping_distance) { + desired_velocity = + std::copysign(vel_limit, position_error); + } + + double velocity_error = desired_velocity - vel_st; + acc_st = std::clamp( + velocity_error / dt, -acc_limit, acc_limit); + + vel_st += acc_st * dt; + vel_st = std::clamp(vel_st, -vel_limit, vel_limit); + angle_st += vel_st * dt; + + double next_error = target - angle_st; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 + && std::abs(vel_st) < 1e-3)) { + angle_st = target; + vel_st = 0.0; + acc_st = 0.0; + } + } + } + + double angle_state(size_t i) const { + return target_angle_state_rad_[i]; + } + double velocity_state(size_t i) const { + return target_velocity_state_rad_[i]; + } + double acceleration_state(size_t i) const { + return target_acceleration_state_rad_[i]; + } + bool joint_active(size_t i) const { return joint_target_active_[i]; } + + std::array angle_states() const { + return target_angle_state_rad_; + } + +private: + static double deg_to_rad_(double deg) { + return deg * std::numbers::pi / 180.0; + } + + std::array joint_target_active_ = { + false, false, false, false}; + std::array target_angle_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array target_velocity_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array target_acceleration_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + + double target_vel_limit_ = 0.0; + double target_acc_limit_ = 0.0; + double suspension_vel_limit_ = 0.0; + double suspension_acc_limit_ = 0.0; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp new file mode 100644 index 00000000..3df2f7d6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp @@ -0,0 +1,385 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class DeformableChassisModeManager { +public: + static constexpr size_t kJointCount = 4; + + struct Result { + rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; + bool ctrl_hold_active = false; + bool suspension_active = false; + bool low_prone = false; + bool symmetric_target = true; + bool spinning_forward = true; + std::array joint_target_deg = {58.0, 58.0, 58.0, 58.0}; + double base_angle_deg = 58.0; + }; + + explicit DeformableChassisModeManager(rclcpp::Node& node) + : min_angle_(node.get_parameter_or("min_angle", 7.0)) + , max_angle_(node.get_parameter_or("max_angle", 58.0)) + , base_angle_deg_(std::clamp( + node.get_parameter_or("active_suspension_base_angle", max_angle_), + min_angle_ - 5.0, max_angle_)) + , suspension_enable_( + node.get_parameter_or("active_suspension_enable", false)) { + result_.base_angle_deg = base_angle_deg_; + result_.joint_target_deg.fill(max_angle_); + current_target_angle_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + } + + void reset() { + result_.mode = rmcs_msgs::ChassisMode::AUTO; + result_.ctrl_hold_active = false; + result_.suspension_active = false; + result_.low_prone = false; + result_.symmetric_target = true; + result_.spinning_forward = true; + result_.joint_target_deg.fill(max_angle_); + result_.base_angle_deg = max_angle_; + + current_target_angle_ = max_angle_; + base_angle_deg_ = max_angle_; + joint_current_target_angle_.fill(max_angle_); + apply_symmetric_target_ = true; + complex_spin_active_ = false; + complex_spin_elapsed_ = 0.0; + qe_complex_spin_active_ = false; + qe_front_high_rear_low_ = true; + qe_last_toggle_elapsed_ = 0.0; + suspension_on_by_keyboard_ = false; + suspension_on_by_remote_ = false; + + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_rotary_knob_ = 0.0; + } + + void update( + rmcs_msgs::Switch switch_left, + rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, + double rotary_knob, + double dt) { + + update_mode_from_inputs_(switch_left, switch_right, keyboard); + update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); + + result_.low_prone = keyboard.ctrl; + result_.ctrl_hold_active = + keyboard.ctrl || suspension_on_by_remote_ || suspension_on_by_keyboard_; + result_.suspension_active = + suspension_enable_ && result_.ctrl_hold_active; + + result_.symmetric_target = symmetric_joint_target_requested_(); + + update_lift_target_toggle_(keyboard, rotary_knob, dt); + + result_.joint_target_deg = joint_current_target_angle_; + result_.base_angle_deg = base_angle_deg_; + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + const Result& result() const { return result_; } + + double min_angle() const { return min_angle_; } + double max_angle() const { return max_angle_; } + double max_angle_rad() const { return deg_to_rad_(max_angle_); } + + double active_suspension_min_angle_rad() const { + return deg_to_rad_(min_angle_ - 5.0); + } + + bool correction_inverted() const { + double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; + return base_angle_deg_ > midpoint; + } + +private: + static double deg_to_rad_(double deg) { + return deg * std::numbers::pi / 180.0; + } + + void update_mode_from_inputs_( + rmcs_msgs::Switch switch_left, + rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + + auto mode = result_.mode; + bool q_pressed = keyboard.q; + bool e_pressed = keyboard.e; + bool last_q_pressed = last_keyboard_.q; + bool last_e_pressed = last_keyboard_.e; + bool last_c_pressed = last_keyboard_.c; + bool qe_combo_pressed = q_pressed && e_pressed; + bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; + bool c_rising = !last_c_pressed && keyboard.c; + bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; + + if (switch_left == rmcs_msgs::Switch::DOWN) { + deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); + result_.mode = mode; + return; + } + + if (qe_complex_spin_active_) { + if (c_rising) { + deactivate_qe_complex_spin_(); + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + mode = rmcs_msgs::ChassisMode::SPIN; + } + } else if (qe_combo_rising) { + deactivate_complex_spin_(); + activate_qe_complex_spin_(mode); + } else if ( + last_switch_right_ == rmcs_msgs::Switch::MIDDLE + && switch_right == rmcs_msgs::Switch::DOWN) { + deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + result_.spinning_forward = !result_.spinning_forward; + } + } else if (!last_keyboard_.c && keyboard.c) { + deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + result_.spinning_forward = !result_.spinning_forward; + } + } else if (!last_keyboard_.z && keyboard.z) { + deactivate_complex_spin_(); + deactivate_qe_complex_spin_(); + mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; + } + + if (complex_spin_active_ || qe_complex_spin_active_) + mode = rmcs_msgs::ChassisMode::SPIN; + + result_.mode = mode; + } + + void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { + complex_spin_active_ = true; + complex_spin_elapsed_ = 0.0; + apply_symmetric_target_ = true; + if (mode != rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::SPIN; + result_.spinning_forward = !result_.spinning_forward; + } + } + + void deactivate_complex_spin_() { + complex_spin_active_ = false; + complex_spin_elapsed_ = 0.0; + } + + void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { + qe_complex_spin_active_ = true; + qe_last_toggle_elapsed_ = 0.0; + qe_front_high_rear_low_ = true; + apply_front_high_rear_low_target_(); + if (mode != rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::SPIN; + result_.spinning_forward = !result_.spinning_forward; + } + } + + void deactivate_qe_complex_spin_() { + qe_complex_spin_active_ = false; + qe_last_toggle_elapsed_ = 0.0; + } + + void apply_front_high_rear_low_target_() { + joint_current_target_angle_[0] = max_angle_; + joint_current_target_angle_[3] = max_angle_; + joint_current_target_angle_[1] = min_angle_; + joint_current_target_angle_[2] = min_angle_; + apply_symmetric_target_ = false; + qe_front_high_rear_low_ = true; + } + + void apply_front_low_rear_high_target_() { + joint_current_target_angle_[0] = min_angle_; + joint_current_target_angle_[3] = min_angle_; + joint_current_target_angle_[1] = max_angle_; + joint_current_target_angle_[2] = max_angle_; + apply_symmetric_target_ = false; + qe_front_high_rear_low_ = false; + } + + void toggle_bg_target_() { + if (qe_front_high_rear_low_) + apply_front_low_rear_high_target_(); + else + apply_front_high_rear_low_target_(); + } + + void toggle_qe_complex_spin_target_() { + toggle_bg_target_(); + } + + void update_qe_complex_spin_toggle_(double dt) { + constexpr double qe_complex_spin_toggle_period = 1.0; + qe_last_toggle_elapsed_ += dt; + size_t toggle_count = 0; + while (qe_last_toggle_elapsed_ >= qe_complex_spin_toggle_period) { + qe_last_toggle_elapsed_ -= qe_complex_spin_toggle_period; + ++toggle_count; + } + if ((toggle_count % 2) == 1) + toggle_qe_complex_spin_target_(); + } + + void update_suspension_toggle_from_inputs_( + rmcs_msgs::Switch switch_left, + rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) + suspension_on_by_remote_ = !suspension_on_by_remote_; + if (suspension_toggle_requested_by_keyboard_(keyboard)) + suspension_on_by_keyboard_ = !suspension_on_by_keyboard_; + } + + bool suspension_toggle_requested_by_keyboard_( + const rmcs_msgs::Keyboard& keyboard) const { + return !last_keyboard_.e && keyboard.e && !keyboard.q; + } + + bool suspension_toggle_requested_by_switch_( + rmcs_msgs::Switch switch_left, + rmcs_msgs::Switch switch_right) const { + return switch_left == rmcs_msgs::Switch::DOWN + && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; + } + + bool symmetric_joint_target_requested_() const { + constexpr double epsilon = 1e-6; + return std::all_of( + joint_current_target_angle_.begin() + 1, joint_current_target_angle_.end(), + [&](double v) { + return std::abs(v - joint_current_target_angle_.front()) <= epsilon; + }); + } + + void update_lift_target_toggle_( + const rmcs_msgs::Keyboard& keyboard, + double rotary_knob, + double dt) { + + constexpr double rotary_knob_symmetric_edge_threshold = 0.7; + constexpr double rotary_knob_bg_edge_threshold = -0.9; + constexpr double complex_spin_toggle_period = 0.5; + + const bool keyboard_toggle_condition = + !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q + && !keyboard.e; + + const bool rotary_knob_toggle_condition = + last_rotary_knob_ < rotary_knob_symmetric_edge_threshold + && rotary_knob >= rotary_knob_symmetric_edge_threshold; + + const bool rotary_knob_bg_toggle_condition = + !qe_complex_spin_active_ + && last_rotary_knob_ > rotary_knob_bg_edge_threshold + && rotary_knob <= rotary_knob_bg_edge_threshold; + + const bool front_high_rear_low = + !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = + !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; + + bool complex_spin_toggle_condition = false; + if (complex_spin_active_) { + complex_spin_elapsed_ += dt; + size_t complex_spin_toggle_count = 0; + while (complex_spin_elapsed_ >= complex_spin_toggle_period) { + complex_spin_elapsed_ -= complex_spin_toggle_period; + ++complex_spin_toggle_count; + } + complex_spin_toggle_condition = + (complex_spin_toggle_count % 2) == 1; + } + + if (qe_complex_spin_active_) + update_qe_complex_spin_toggle_(dt); + + if (apply_symmetric_target_) + joint_current_target_angle_.fill(current_target_angle_); + + if (rotary_knob_toggle_condition || keyboard_toggle_condition + || complex_spin_toggle_condition) { + if (result_.suspension_active && keyboard_toggle_condition) { + base_angle_deg_ = + (std::abs(base_angle_deg_ - max_angle_) < 1e-6) + ? min_angle_ + : max_angle_; + } else { + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) + ? min_angle_ + : max_angle_; + apply_symmetric_target_ = true; + } + } else if (rotary_knob_bg_toggle_condition) { + toggle_bg_target_(); + } else if (front_high_rear_low) { + apply_front_high_rear_low_target_(); + } else if (front_low_rear_high) { + apply_front_low_rear_high_target_(); + } + + last_rotary_knob_ = rotary_knob; + } + + Result result_; + + double min_angle_; + double max_angle_; + double base_angle_deg_; + bool suspension_enable_; + + double current_target_angle_; + std::array joint_current_target_angle_; + bool apply_symmetric_target_ = true; + bool complex_spin_active_ = false; + double complex_spin_elapsed_ = 0.0; + bool qe_complex_spin_active_ = false; + bool qe_front_high_rear_low_ = true; + double qe_last_toggle_elapsed_ = 0.0; + bool suspension_on_by_keyboard_ = false; + bool suspension_on_by_remote_ = false; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + double last_rotary_knob_ = 0.0; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp new file mode 100644 index 00000000..b72139a3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp @@ -0,0 +1,324 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class DeformableChassisActiveSuspension { +public: + static constexpr size_t kJointCount = 4; + static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + + struct Corrections { + std::array angle = {0.0, 0.0, 0.0, 0.0}; + std::array velocity = {0.0, 0.0, 0.0, 0.0}; + std::array acceleration = {0.0, 0.0, 0.0, 0.0}; + std::array active = {false, false, false, false}; + }; + + void configure(rclcpp::Node& node) { + load_pid_( + node, "active_suspension_pitch_outer_", pitch_outer_pid_, // + 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, 3.0); + load_pid_( + node, "active_suspension_pitch_inner_", pitch_inner_pid_, // + 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); + load_pid_( + node, "active_suspension_roll_outer_", roll_outer_pid_, // + 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, 3.0); + load_pid_( + node, "active_suspension_roll_inner_", roll_inner_pid_, // + 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); + + correction_vel_limit_ = std::max( + deg_to_rad_(std::abs(node.get_parameter_or( + "active_suspension_correction_velocity_limit_deg", 720.0))), + 1e-6); + correction_acc_limit_ = std::max( + deg_to_rad_(std::abs(node.get_parameter_or( + "active_suspension_correction_acceleration_limit_deg", 3600.0))), + 1e-6); + + calibration_wait_time_ = std::max( + node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_sample_time_ = std::max( + node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + } + + void calibrate( + double pitch, double roll, + bool symmetric_target, double dt) { + + if (calibrated_once_) + return; + + if (!symmetric_target) { + reset_calibration_window_(); + return; + } + + if (!std::isfinite(pitch) || !std::isfinite(roll)) + return; + + calibration_hold_elapsed_ += dt; + if (calibration_hold_elapsed_ < calibration_wait_time_) + return; + + double calibration_end = + calibration_wait_time_ + calibration_sample_time_; + if (calibration_hold_elapsed_ < calibration_end) { + pitch_sum_ += pitch; + roll_sum_ += roll; + ++sample_count_; + return; + } + + if (calibration_completed_for_window_) + return; + + calibration_completed_for_window_ = true; + if (sample_count_ == 0) + return; + + pitch_offset_ = std::clamp( + pitch_sum_ / static_cast(sample_count_), + -offset_limit_rad_, offset_limit_rad_); + roll_offset_ = std::clamp( + roll_sum_ / static_cast(sample_count_), + -offset_limit_rad_, offset_limit_rad_); + calibrated_once_ = true; + } + + bool calibrated() const { return calibrated_once_; } + double pitch_offset() const { return pitch_offset_; } + double roll_offset() const { return roll_offset_; } + + double scope_torque(bool suspension_active, bool is_spin) const { + if (suspension_active && !is_spin) + return -0.3; + return 0.3; + } + + Corrections update( + double pitch, double roll, + double pitch_rate, double roll_rate, + bool suspension_active, bool low_prone, + double min_angle_deg, double max_angle_deg, + double base_angle_deg, bool correction_inverted, + const std::array& base_joint_angles, + double dt) { + + Corrections corr; + + if (!suspension_active) { + reset_attitude_(); + run_correction_trajectory_( + low_prone, min_angle_deg, max_angle_deg, + base_angle_deg, base_joint_angles, dt, corr); + return corr; + } + + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + double cp = std::clamp(pitch, -max_attitude, max_attitude); + double cr = std::clamp(roll, -max_attitude, max_attitude); + + double pitch_outer = pitch_outer_pid_.update(-cp); + double roll_outer = roll_outer_pid_.update(cr); + double pitch_diff = pitch_inner_pid_.update( + pitch_outer - pitch_rate); + double roll_diff = roll_inner_pid_.update( + roll_outer + roll_rate); + + if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { + reset_attitude_(); + return corr; + } + + compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); + run_correction_trajectory_( + low_prone, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, dt, corr); + return corr; + } + + void reset() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + correction_target_rad_.fill(0.0); + correction_state_rad_.fill(0.0); + correction_velocity_state_rad_.fill(0.0); + correction_acceleration_state_rad_.fill(0.0); + reset_calibration_window_(); + } + +private: + static double deg_to_rad_(double deg) { + return deg * std::numbers::pi / 180.0; + } + + void load_pid_( + rclcpp::Node& node, const std::string& prefix, + pid::PidCalculator& pid, + double kp_default, double ki_default, double kd_default, + double integral_min_default, double integral_max_default, + double output_min_default, double output_max_default) { + + pid.kp = node.get_parameter_or(prefix + "kp", kp_default); + pid.ki = node.get_parameter_or(prefix + "ki", ki_default); + pid.kd = node.get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = + node.get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = + node.get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = + node.get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = + node.get_parameter_or(prefix + "output_max", output_max_default); + } + + void reset_attitude_() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + correction_target_rad_.fill(0.0); + } + + void reset_calibration_window_() { + calibration_hold_elapsed_ = 0.0; + sample_count_ = 0; + pitch_sum_ = 0.0; + roll_sum_ = 0.0; + calibration_completed_for_window_ = false; + } + + void compute_correction_targets_( + double pitch_diff, double roll_diff, bool inverted) { + + if (inverted) { + double fp = std::max(pitch_diff, 0.0); + double bp = std::max(-pitch_diff, 0.0); + double lr = std::max(-roll_diff, 0.0); + double rr = std::max(roll_diff, 0.0); + correction_target_rad_[0] = -(fp + lr); + correction_target_rad_[1] = -(bp + lr); + correction_target_rad_[2] = -(bp + rr); + correction_target_rad_[3] = -(fp + rr); + } else { + double fp = std::max(-pitch_diff, 0.0); + double bp = std::max(pitch_diff, 0.0); + double lr = std::max(roll_diff, 0.0); + double rr = std::max(-roll_diff, 0.0); + correction_target_rad_[0] = fp + lr; + correction_target_rad_[1] = bp + lr; + correction_target_rad_[2] = bp + rr; + correction_target_rad_[3] = fp + rr; + } + } + + void run_correction_trajectory_( + bool low_prone, + double min_angle_deg, double max_angle_deg, + double base_angle_deg, + const std::array& base_joint_angles, + double dt, + Corrections& corr) { + + double max_target_rad = deg_to_rad_(max_angle_deg); + double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); + + for (size_t i = 0; i < kJointCount; ++i) { + double base_angle = std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone + ? min_susp_rad + : deg_to_rad_(base_angle_deg)); + + double correction_min = min_susp_rad - base_angle; + double correction_max = max_target_rad - base_angle; + double target = std::clamp( + correction_target_rad_[i], correction_min, correction_max); + + double& angle_st = correction_state_rad_[i]; + double& vel_st = correction_velocity_state_rad_[i]; + double& acc_st = correction_acceleration_state_rad_[i]; + + double position_error = target - angle_st; + double stopping_distance = + vel_st * vel_st / (2.0 * correction_acc_limit_); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 + && std::abs(position_error) > stopping_distance) { + desired_velocity = + std::copysign(correction_vel_limit_, position_error); + } + + double velocity_error = desired_velocity - vel_st; + acc_st = std::clamp( + velocity_error / dt, -correction_acc_limit_, + correction_acc_limit_); + + vel_st += acc_st * dt; + vel_st = std::clamp( + vel_st, -correction_vel_limit_, correction_vel_limit_); + angle_st += vel_st * dt; + + double next_error = target - angle_st; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(vel_st) < 1e-3)) { + angle_st = target; + vel_st = 0.0; + acc_st = 0.0; + } + + corr.active[i] = true; + } + + corr.angle = correction_state_rad_; + corr.velocity = correction_velocity_state_rad_; + corr.acceleration = correction_acceleration_state_rad_; + } + + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; + + double correction_vel_limit_ = 40.0; + double correction_acc_limit_ = 200.0; + + double calibration_wait_time_ = 2.0; + double calibration_sample_time_ = 3.0; + double calibration_hold_elapsed_ = 0.0; + size_t sample_count_ = 0; + double pitch_sum_ = 0.0; + double roll_sum_ = 0.0; + bool calibration_completed_for_window_ = false; + bool calibrated_once_ = false; + double pitch_offset_ = 0.0; + double roll_offset_ = 0.0; + + std::array correction_target_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array correction_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array correction_velocity_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; + std::array correction_acceleration_state_rad_ = { + 0.0, 0.0, 0.0, 0.0}; +}; + +} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index 7ae3c193..85220dd9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -1,11 +1,13 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -24,39 +26,45 @@ class DeformableOmniWheelController DeformableOmniWheelController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) , mass_(get_parameter("mass").as_double()) , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) , wheel_radius_(get_parameter("wheel_radius").as_double()) - , friction_coefficient_(get_parameter("friction_coefficient").as_double()) + , friction_coefficient_( + get_parameter("friction_coefficient").as_double()) , k1_(get_parameter("k1").as_double()) , k2_(get_parameter("k2").as_double()) , no_load_power_(get_parameter("no_load_power").as_double()) , com_height_(get_parameter_or("com_height", 0.0)) - , chassis_radius_x_(get_parameter_or("chassis_radius_x", 0.2341741 / std::numbers::sqrt2)) - , chassis_radius_y_(get_parameter_or("chassis_radius_y", 0.2341741 / std::numbers::sqrt2)) + , chassis_radius_x_(get_parameter_or( + "chassis_radius_x", 0.2341741 / std::numbers::sqrt2)) + , chassis_radius_y_(get_parameter_or( + "chassis_radius_y", 0.2341741 / std::numbers::sqrt2)) , translational_velocity_pid_calculator_(5.0, 0.0, 0.0) , angular_velocity_pid_calculator_(5.0, 0.0, 0.0) , wheel_velocity_pid_(0.6, 0.0, 0.0) { - register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); - - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + register_input( + "/chassis/left_front_wheel/max_torque", + wheel_motor_max_control_torque_); + + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format( + "/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + register_output( + fmt::format( + "/chassis/{}_wheel/control_torque", + kWheelName[i]), + wheel_control_torque_[i], nan_); + } - register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input( + "/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/radius", chassis_radius_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_control_torque_, nan_); - register_output("/chassis/left_back_wheel/control_torque", left_back_control_torque_, nan_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_control_torque_, nan_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_control_torque_, nan_); } void before_updating() override { @@ -71,73 +79,90 @@ class DeformableOmniWheelController return; } - if (!std::isfinite(*chassis_radius_) || *chassis_radius_ <= 1e-6) { + if (!std::isfinite(*chassis_radius_) + || *chassis_radius_ <= 1e-6) { reset_all_controls(); return; } - Eigen::Vector4d wheel_velocities = { - *left_front_velocity_, *left_back_velocity_, *right_back_velocity_, - *right_front_velocity_}; + Eigen::Vector4d wheel_velocities; + for (size_t i = 0; i < kWheelCount; ++i) + wheel_velocities[i] = *wheel_velocity_[i]; - const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); - auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); - const auto wheel_pid_torques = - calculate_wheel_pid_torques(wheel_velocities, chassis_velocity); + const auto chassis_velocity = + calculate_chassis_velocity(wheel_velocities); + auto chassis_control_torque = + calculate_chassis_control_torque(chassis_velocity); + const auto wheel_pid_torques = calculate_wheel_pid_torques( + wheel_velocities, chassis_velocity); chassis_control_torque.torque = constrain_chassis_control_torque( wheel_velocities, chassis_control_torque, wheel_pid_torques); - const auto wheel_control_torques = - calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); + const auto wheel_control_torques = calculate_wheel_control_torques( + chassis_control_torque, wheel_pid_torques); - *left_front_control_torque_ = wheel_control_torques[0]; - *left_back_control_torque_ = wheel_control_torques[1]; - *right_back_control_torque_ = wheel_control_torques[2]; - *right_front_control_torque_ = wheel_control_torques[3]; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = wheel_control_torques[i]; } private: + static constexpr size_t kWheelCount = 4; + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); + static constexpr double g_ = 9.81; + struct ChassisControlTorque { Eigen::Vector2d torque; Eigen::Vector2d lambda; }; void reset_all_controls() { - *left_front_control_torque_ = 0.0; - *left_back_control_torque_ = 0.0; - *right_back_control_torque_ = 0.0; - *right_front_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = 0.0; } - Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d calculate_chassis_velocity( + const Eigen::Vector4d& wheel_velocities) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = + std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector3d velocity; velocity.x() = -w1 - w2 + w3 + w4; velocity.y() = w1 - w2 - w3 + w4; velocity.z() = (w1 + w2 + w3 + w4) / a_plus_b; - velocity *= (-std::numbers::sqrt2 / 4 * wheel_radius_); + velocity *= + (-std::numbers::sqrt2 / 4 * wheel_radius_); return velocity; } - ChassisControlTorque calculate_chassis_control_torque(const Eigen::Vector3d& chassis_velocity) { + ChassisControlTorque calculate_chassis_control_torque( + const Eigen::Vector3d& chassis_velocity) { ChassisControlTorque result; - Eigen::Vector3d err = chassis_control_velocity_->vector - chassis_velocity; + Eigen::Vector3d err = + chassis_control_velocity_->vector - chassis_velocity; Eigen::Vector2d translational_torque = (-std::numbers::sqrt2 / 4 * wheel_radius_) * mass_ - * translational_velocity_pid_calculator_.update(err.head<2>()); + * translational_velocity_pid_calculator_.update( + err.head<2>()); result.torque.x() = translational_torque.norm(); - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); - result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) - * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(err[2]); + const double a_plus_b = + std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + result.torque.y() = + (-std::numbers::sqrt2 / 4 * wheel_radius_) + * (moment_of_inertia_ / a_plus_b) + * angular_velocity_pid_calculator_.update(err[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) - translational_torque_direction = translational_torque / result.torque.x(); + translational_torque_direction = + translational_torque / result.torque.x(); else - translational_torque_direction = Eigen::Vector2d::UnitX(); + translational_torque_direction = + Eigen::Vector2d::UnitX(); auto& [x, y] = translational_torque_direction; result.lambda = {-x + y, -x - y}; @@ -145,71 +170,104 @@ class DeformableOmniWheelController } Eigen::Vector4d calculate_wheel_pid_torques( - const Eigen::Vector4d& wheel_velocities, const Eigen::Vector3d& chassis_velocity) { + const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_velocity) { const auto& [x, y, z] = chassis_velocity; - const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = + std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector4d wheel_control_velocity = { -x + y + a_plus_b * z, -x - y + a_plus_b * z, +x - y + a_plus_b * z, +x + y + a_plus_b * z, }; - wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); - return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); + wheel_control_velocity *= + -1 / (std::numbers::sqrt2 * wheel_radius_); + return wheel_velocity_pid_.update( + wheel_control_velocity - wheel_velocities); } Eigen::Vector2d constrain_chassis_control_torque( - const Eigen::Vector4d& wheel_velocities, const ChassisControlTorque& chassis_control_torque, + const Eigen::Vector4d& wheel_velocities, + const ChassisControlTorque& chassis_control_torque, const Eigen::Vector4d& wheel_pid_torques) const { const auto& [w1, w2, w3, w4] = wheel_velocities; const auto& [x_max, y_max] = chassis_control_torque.torque; - const double y_sign = y_max > 0 ? 1.0 : -1.0; - const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; + const double y_sign = y_max > 0 ? 1.0 : -1.0; + const auto& [lambda_1, lambda_2] = + chassis_control_torque.lambda; const auto& [t1, t2, t3, t4] = wheel_pid_torques; - const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; - const double rhombus_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); + const double rhombus_top = + (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; + const double rhombus_right = + rhombus_top + / std::max(std::abs(lambda_1), std::abs(lambda_2)); const double a = 4 * k1_; const double b = 0; const double c = 4 * k1_; - const double d = - lambda_1 * (w1 - w3 + 2 * k1_ * (t1 - t3)) + lambda_2 * (w2 - w4 + 2 * k1_ * (t2 - t4)); - const double e = y_sign * (2 * k1_ * wheel_pid_torques.sum() + wheel_velocities.sum()); - const double f = k1_ * wheel_pid_torques.array().pow(2).sum() - + (wheel_pid_torques.array() * wheel_velocities.array()).sum() - + k2_ * wheel_velocities.array().pow(2).sum() - no_load_power_ - - *power_limit_; + const double d = lambda_1 * (w1 - w3 + 2 * k1_ * (t1 - t3)) + + lambda_2 * (w2 - w4 + 2 * k1_ * (t2 - t4)); + const double e = y_sign * (2 * k1_ * wheel_pid_torques.sum() + + wheel_velocities.sum()); + const double f = + k1_ * wheel_pid_torques.array().pow(2).sum() + + (wheel_pid_torques.array() * wheel_velocities.array()) + .sum() + + k2_ * wheel_velocities.array().pow(2).sum() + - no_load_power_ - *power_limit_; Eigen::Vector2d result = Eigen::Vector2d::Constant(nan_); if (com_height_ > 1e-6) { const double dir_x = -(lambda_1 + lambda_2) / 2.0; const double dir_y = (lambda_1 - lambda_2) / 2.0; - const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); - const double gamma_1 = coeff * (+dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); - const double gamma_2 = coeff * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); - - const double force_to_torque = friction_coefficient_ * wheel_radius_; - const double rhs = force_to_torque * mass_ * g_ / 4.0; - const std::vector half_planes = { - { lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, - {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, - { lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, - {-lambda_2 - force_to_torque * gamma_2, -y_sign, rhs}, - {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, - { lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, - {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, - { lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, - }; + const double coeff = + -com_height_ + / (std::numbers::sqrt2 * wheel_radius_); + const double gamma_1 = + coeff + * (+dir_x / chassis_radius_x_ + + dir_y / chassis_radius_y_); + const double gamma_2 = + coeff + * (-dir_x / chassis_radius_x_ + + dir_y / chassis_radius_y_); + + const double force_to_torque = + friction_coefficient_ * wheel_radius_; + const double rhs = + force_to_torque * mass_ * g_ / 4.0; + const std::vector + half_planes = { + {lambda_1 - force_to_torque * gamma_1, y_sign, + rhs}, + {-lambda_1 - force_to_torque * gamma_1, + -y_sign, rhs}, + {lambda_2 - force_to_torque * gamma_2, y_sign, + rhs}, + {-lambda_2 - force_to_torque * gamma_2, + -y_sign, rhs}, + {-lambda_1 + force_to_torque * gamma_1, y_sign, + rhs}, + {lambda_1 + force_to_torque * gamma_1, + -y_sign, rhs}, + {-lambda_2 + force_to_torque * gamma_2, y_sign, + rhs}, + {lambda_2 + force_to_torque * gamma_2, + -y_sign, rhs}, + }; result = qcp_solver_.solve( - {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); + {1.0, 1.0}, {x_max, std::abs(y_max)}, + half_planes, {a, b, c, d, e, f}); } if (!result.array().isFinite().all()) { result = qcp_solver_.solve( - {1.0, 1.0}, {x_max, std::abs(y_max)}, {rhombus_right, rhombus_top}, + {1.0, 1.0}, {x_max, std::abs(y_max)}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f}); } result.y() *= y_sign; @@ -217,23 +275,22 @@ class DeformableOmniWheelController } static Eigen::Vector4d calculate_wheel_control_torques( - ChassisControlTorque chassis_control_torque, Eigen::Vector4d wheel_pid_torques) { - const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; + ChassisControlTorque chassis_control_torque, + Eigen::Vector4d wheel_pid_torques) { + const auto& [lambda_1, lambda_2] = + chassis_control_torque.lambda; Eigen::Vector4d wheel_torques = { +lambda_1 * chassis_control_torque.torque.x(), +lambda_2 * chassis_control_torque.torque.x(), -lambda_1 * chassis_control_torque.torque.x(), -lambda_2 * chassis_control_torque.torque.x(), }; - wheel_torques.array() += chassis_control_torque.torque.y(); + wheel_torques.array() += + chassis_control_torque.torque.y(); wheel_torques.array() += wheel_pid_torques.array(); return wheel_torques; } - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - static constexpr double g_ = 9.81; - const double mass_; const double moment_of_inertia_; const double wheel_radius_; @@ -245,26 +302,23 @@ class DeformableOmniWheelController InputInterface wheel_motor_max_control_torque_; - InputInterface left_front_velocity_; - InputInterface left_back_velocity_; - InputInterface right_back_velocity_; - InputInterface right_front_velocity_; + std::array, kWheelCount> + wheel_velocity_; + std::array, kWheelCount> + wheel_control_torque_; - InputInterface chassis_control_velocity_; + InputInterface + chassis_control_velocity_; InputInterface power_limit_; InputInterface chassis_radius_; - pid::MatrixPidCalculator<2> translational_velocity_pid_calculator_; + pid::MatrixPidCalculator<2> + translational_velocity_pid_calculator_; pid::PidCalculator angular_velocity_pid_calculator_; pid::MatrixPidCalculator<4> wheel_velocity_pid_; QcpSolver qcp_solver_; - - OutputInterface left_front_control_torque_; - OutputInterface left_back_control_torque_; - OutputInterface right_back_control_torque_; - OutputInterface right_front_control_torque_; }; } // namespace rmcs_core::controller::chassis @@ -272,4 +326,5 @@ class DeformableOmniWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableOmniWheelController, rmcs_executor::Component) + rmcs_core::controller::chassis::DeformableOmniWheelController, + rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index a6e181da..5512981d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -1,10 +1,12 @@ #include #include +#include #include #include #include +#include #include #include #include @@ -31,21 +33,17 @@ class OmniWheelController register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < kWheelCount; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + register_output( + fmt::format("/chassis/{}_wheel/control_torque", kWheelName[i]), + wheel_control_torque_[i], nan_); + } register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); - - register_output( - "/chassis/left_front_wheel/control_torque", left_front_control_torque_, nan_); - register_output("/chassis/left_back_wheel/control_torque", left_back_control_torque_, nan_); - register_output( - "/chassis/right_back_wheel/control_torque", right_back_control_torque_, nan_); - register_output( - "/chassis/right_front_wheel/control_torque", right_front_control_torque_, nan_); } void before_updating() override { @@ -60,10 +58,9 @@ class OmniWheelController return; } - Eigen::Vector4d wheel_velocities = { - *left_front_velocity_, *left_back_velocity_, // - *right_back_velocity_, *right_front_velocity_ // - }; + Eigen::Vector4d wheel_velocities; + for (size_t i = 0; i < kWheelCount; ++i) + wheel_velocities[i] = *wheel_velocity_[i]; const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); @@ -74,23 +71,24 @@ class OmniWheelController const auto wheel_control_torques = calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); - *left_front_control_torque_ = wheel_control_torques[0]; - *left_back_control_torque_ = wheel_control_torques[1]; - *right_back_control_torque_ = wheel_control_torques[2]; - *right_front_control_torque_ = wheel_control_torques[3]; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = wheel_control_torques[i]; } private: + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + static constexpr size_t kWheelCount = 4; + struct ChassisControlTorque { Eigen::Vector2d torque; Eigen::Vector2d lambda; }; void reset_all_controls() { - *left_front_control_torque_ = 0.0; - *left_back_control_torque_ = 0.0; - *right_back_control_torque_ = 0.0; - *right_front_control_torque_ = 0.0; + for (size_t i = 0; i < kWheelCount; ++i) + *wheel_control_torque_[i] = 0.0; } static Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) { @@ -198,10 +196,8 @@ class OmniWheelController InputInterface wheel_motor_max_control_torque_; - InputInterface left_front_velocity_; - InputInterface left_back_velocity_; - InputInterface right_back_velocity_; - InputInterface right_front_velocity_; + std::array, kWheelCount> + wheel_velocity_; InputInterface chassis_control_velocity_; InputInterface power_limit_; @@ -213,10 +209,8 @@ class OmniWheelController QcpSolver qcp_solver_; - OutputInterface left_front_control_torque_; - OutputInterface left_back_control_torque_; - OutputInterface right_back_control_torque_; - OutputInterface right_front_control_torque_; + std::array, kWheelCount> + wheel_control_torque_; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 5b9dcea3..f93aaca2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -6,11 +7,13 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include @@ -87,6 +90,10 @@ class DeformableInfantryOmniB } private: + static constexpr const char* kJointName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + class DeformableInfantryOmniBCommand; class BottomBoard; class TopBoard; @@ -164,27 +171,14 @@ class DeformableInfantryOmniB "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); deformableInfantry.register_output( "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); + for (size_t i = 0; i < 4; ++i) { + deformableInfantry.register_output( + fmt::format("/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), + joint_physical_angle_[i], nan_); + deformableInfantry.register_output( + fmt::format("/chassis/{}_joint/physical_velocity", DeformableInfantryOmniB::kJointName[i]), + joint_physical_velocity_[i], nan_); + } deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); deformableInfantry.register_output( "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); @@ -228,14 +222,10 @@ class DeformableInfantryOmniB for (auto& motor : chassis_joint_motors_) motor.update_status(); - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], + joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -316,22 +306,42 @@ class DeformableInfantryOmniB .as_bytes(), }); } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case 0: + builder.can0_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 1: + builder.can1_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 2: + builder.can2_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 3: + builder.can3_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + } + } } } @@ -365,11 +375,11 @@ class DeformableInfantryOmniB void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + *joint_physical_angle_[0], *joint_physical_angle_[1], + *joint_physical_angle_[2], *joint_physical_angle_[3]}; const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + *joint_physical_velocity_[0], *joint_physical_velocity_[1], + *joint_physical_velocity_[2], *joint_physical_velocity_[3]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = nan_; @@ -400,29 +410,39 @@ class DeformableInfantryOmniB }; if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) + fmt::format_to( + std::back_inserter(wheel_rx_str), "{}%c", + i > 0 ? " " : "", wheel_rx(i)); RCLCPP_INFO( deformable_infantry_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", + "rx=[%s]", chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), + wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) + fmt::format_to( + std::back_inserter(joint_rx_str), "{}%c", + i > 0 ? " " : "", joint_rx(i)); RCLCPP_INFO( deformable_infantry_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + "rx=[%s]", + *joint_physical_angle_[0], *joint_physical_angle_[1], + *joint_physical_angle_[2], *joint_physical_angle_[3], + *joint_physical_velocity_[0], *joint_physical_velocity_[1], + *joint_physical_velocity_[2], *joint_physical_velocity_[3], + joint_rx_str.c_str()); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -462,28 +482,27 @@ class DeformableInfantryOmniB dr16_.store_status(data.uart_data); } - void can0_receive_callback(const librmcs::data::CanDataView& data) override { + void process_chassis_can_receive_( + size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); } } + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(0, data); + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -493,31 +512,17 @@ class DeformableInfantryOmniB } void can2_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { + if (data.can_id == 0x142) gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { + else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); - } } void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } + process_chassis_can_receive_(3, data); } void uart0_receive_callback(const librmcs::data::UartDataView& data) override { @@ -587,14 +592,8 @@ class DeformableInfantryOmniB OutputInterface chassis_imu_roll_; OutputInterface chassis_imu_pitch_rate_; OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; OutputInterface encoder_alpha_; OutputInterface encoder_alpha_dot_; OutputInterface radius_; @@ -758,10 +757,15 @@ class DeformableInfantryOmniB text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); text("Chassis Status"); - text("- left front: {}", bottom_board_->chassis_joint_motors_[0].last_raw_angle()); - text("- left back: {}", bottom_board_->chassis_joint_motors_[1].last_raw_angle()); - text("- right back: {}", bottom_board_->chassis_joint_motors_[2].last_raw_angle()); - text("- right front: {}", bottom_board_->chassis_joint_motors_[3].last_raw_angle()); + constexpr auto kPosition = + std::array{"left front", "left back", "right back", "right front"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_joint_motors_)) { + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + } response->message = feedback_message.str(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index d9d9e00c..e2058665 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -8,6 +8,7 @@ #include "hardware/device/vt13.hpp" #include +#include #include #include #include @@ -21,6 +22,7 @@ #include #include +#include #include #include #include @@ -82,6 +84,9 @@ class DeformableInfantryOmni private: static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr const char* kJointName[] = { + "left_front", "left_back", "right_back", "right_front", + }; class Command : public rmcs_executor::Component { public: @@ -147,27 +152,14 @@ class DeformableInfantryOmni status.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); status.register_output("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - status.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, kNaN); - status.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - kNaN); - status.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - kNaN); - status.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, kNaN); + for (size_t i = 0; i < 4; ++i) { + status.register_output( + fmt::format("/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), + joint_physical_angle_[i], kNaN); + status.register_output( + fmt::format("/chassis/{}_joint/physical_velocity", DeformableInfantryOmni::kJointName[i]), + joint_physical_velocity_[i], kNaN); + } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); status.register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_, kNaN); status.register_output("/chassis/radius", radius_, default_radius_); @@ -209,14 +201,10 @@ class DeformableInfantryOmni for (auto& motor : chassis_joint_motors_) motor.update_status(); - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); + for (size_t i = 0; i < 4; ++i) + update_joint_physical_feedback_( + i, joint_physical_angle_[i], + joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -297,22 +285,42 @@ class DeformableInfantryOmni .as_bytes(), }); } else { - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); + for (size_t i = 0; i < 4; ++i) { + switch (i) { + case 0: + builder.can0_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 1: + builder.can1_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 2: + builder.can2_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + case 3: + builder.can3_transmit({ + .can_id = 0x141, + .can_data = + chassis_joint_motors_[i].generate_command() + .as_bytes(), + }); + break; + } + } } } @@ -361,14 +369,8 @@ class DeformableInfantryOmni OutputInterface chassis_imu_roll_; OutputInterface chassis_imu_pitch_rate_; OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; + std::array, 4> joint_physical_angle_; + std::array, 4> joint_physical_velocity_; OutputInterface encoder_alpha_; OutputInterface encoder_alpha_dot_; OutputInterface radius_; @@ -397,11 +399,11 @@ class DeformableInfantryOmni void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; + *joint_physical_angle_[0], *joint_physical_angle_[1], + *joint_physical_angle_[2], *joint_physical_angle_[3]}; const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; + *joint_physical_velocity_[0], *joint_physical_velocity_[1], + *joint_physical_velocity_[2], *joint_physical_velocity_[3]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { *encoder_alpha_ = kNaN; @@ -432,29 +434,39 @@ class DeformableInfantryOmni }; if (debug_log_wheel_motor_) { + std::string wheel_rx_str; + for (size_t i = 0; i < 4; ++i) + fmt::format_to( + std::back_inserter(wheel_rx_str), "{}%c", + i > 0 ? " " : "", wheel_rx(i)); RCLCPP_INFO( status_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", + "rx=[%s]", chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), + wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { + std::string joint_rx_str; + for (size_t i = 0; i < 4; ++i) + fmt::format_to( + std::back_inserter(joint_rx_str), "{}%c", + i > 0 ? " " : "", joint_rx(i)); RCLCPP_INFO( status_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + "rx=[%s]", + *joint_physical_angle_[0], *joint_physical_angle_[1], + *joint_physical_angle_[2], *joint_physical_angle_[3], + *joint_physical_velocity_[0], *joint_physical_velocity_[1], + *joint_physical_velocity_[2], *joint_physical_velocity_[3], + joint_rx_str.c_str()); } next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); @@ -494,28 +506,27 @@ class DeformableInfantryOmni dr16_.store_status(data.uart_data); } - void can0_receive_callback(const librmcs::data::CanDataView& data) override { + void process_chassis_can_receive_( + size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); + chassis_wheel_motors_[index].store_status(data.can_data); + wheel_status_received_[index].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); + chassis_joint_motors_[index].store_status(data.can_data); + joint_status_received_[index].store(true, std::memory_order_relaxed); } } + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(0, data); + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x300) { + process_chassis_can_receive_(1, data); + if (!data.is_extended_can_id && !data.is_remote_transmission + && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -525,31 +536,17 @@ class DeformableInfantryOmni } void can2_receive_callback(const librmcs::data::CanDataView& data) override { + process_chassis_can_receive_(2, data); if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { + if (data.can_id == 0x142) gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { + else if (data.can_id == 0x203) gimbal_bullet_feeder_.store_status(data.can_data); - } } void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } + process_chassis_can_receive_(3, data); } void uart0_receive_callback(const librmcs::data::UartDataView& data) override { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp deleted file mode 100644 index bdac3e64..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-steering.cpp +++ /dev/null @@ -1,860 +0,0 @@ -#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/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/remote_control.hpp" -#include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" - -namespace rmcs_core::hardware { - -using Clock = std::chrono::steady_clock; - -class DeformableInfantryV2 - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DeformableInfantryV2() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , deformable_infantry_command_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_output("/tf", tf_); - - tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); - - // For command: remote-status - using Srv = std_srvs::srv::Trigger; - status_service_ = create_service( - "/rmcs/service/robot_status", - [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { - status_service_callback(response); - }); - - bottom_board_ = std::make_unique( - *this, *deformable_infantry_command_, - get_parameter("serial_filter_rmcs_board").as_string()); - top_board_ = std::make_unique( - *this, *deformable_infantry_command_, vt13_, - get_parameter("serial_filter_top_board").as_string()); - remote_control_ = - std::make_unique(*this, bottom_board_->dr16_, vt13_); - } - - ~DeformableInfantryV2() override = default; - - void update() override { - bottom_board_->update(); - top_board_->update(); - vt13_.update_status(); - remote_control_->update(); - } - - void command_update() { - const bool even = ((cmd_tick_++ & 1u) == 0u); - bottom_board_->command_update(even); - top_board_->command_update(); - } - -private: - class DeformableInfantryV2Command; - class BottomBoard; - class TopBoard; - - class DeformableInfantryV2Command : public rmcs_executor::Component { - public: - explicit DeformableInfantryV2Command(DeformableInfantryV2& deformableInfantry) - : deformableInfantry(deformableInfantry) {} - - void update() override { deformableInfantry.command_update(); } - - DeformableInfantryV2& deformableInfantry; - }; - - class BottomBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - explicit BottomBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, - const std::string& serial_filter = {}) - : RmcsBoardLite{ - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}} - , deformable_infantry_{deformableInfantry} - , command_{deformableInfantry_command} - , tf_{deformableInfantry.tf_} { - - deformableInfantry.register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart0_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(10.0) - .enable_multi_turn_angle() - .set_reversed()); - - // V2: LK MG5010 i36 direct-drive joint motors, built-in encoder zero point - for (auto& motor : chassis_joint_motors_) - motor.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei36} - .set_reversed() - .enable_multi_turn_angle()); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU - // installation is re-validated on hardware. - return std::make_tuple(-y, x, z); - }); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); - - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - - deformableInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); - deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, - nan_); - deformableInfantry.register_output( - "/chassis/left_front_joint/physical_velocity", left_front_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/left_back_joint/physical_velocity", left_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_back_joint/physical_velocity", right_back_joint_physical_velocity_, - nan_); - deformableInfantry.register_output( - "/chassis/right_front_joint/physical_velocity", - right_front_joint_physical_velocity_, nan_); - deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); - deformableInfantry.register_output( - "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); - deformableInfantry.register_output("/chassis/radius", radius_, nan_); - - deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); - deformableInfantry.get_parameter_or( - "debug_log_wheel_motor", debug_log_wheel_motor_, false); - deformableInfantry.get_parameter_or( - "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); - } - - ~BottomBoard() override = default; - - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gz(); - { - const double q0 = imu_.q0(); - const double q1 = imu_.q1(); - const double q2 = imu_.q2(); - const double q3 = imu_.q3(); - - double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); - - const double standard_pitch = std::asin(sin_pitch); - const double standard_roll = - std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); - - // Export chassis attitude using the requested convention: - // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; - *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); - } - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - for (auto& motor : chassis_joint_motors_) - motor.update_status(); - - update_joint_physical_feedback_( - 0, left_front_joint_physical_angle_, left_front_joint_physical_velocity_); - update_joint_physical_feedback_( - 1, left_back_joint_physical_angle_, left_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 2, right_back_joint_physical_angle_, right_back_joint_physical_velocity_); - update_joint_physical_feedback_( - 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); - - update_geometry_feedback_(); - if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) - log_chassis_feedback_once_per_second_(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - if (supercap_status_received_.load(std::memory_order_relaxed)) - supercap_.update_status(); - if (debug_log_supercap_) - log_supercap_feedback_once_per_second_(); - gimbal_bullet_feeder_.update_status(); - - tf_->set_state( - gimbal_yaw_motor_.angle()); - } - - void command_update(bool even) { - auto builder = start_transmit(); - if (even) { - // Steer motors: same as V1 - builder.can0_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } else { - // V2: Wheel DJI frames (wheel only, no joint packed in) - builder.can0_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - // V2: Joint LK motors - individual CAN frames - builder.can0_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[1].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[2].generate_command().as_bytes(), - }); - builder.can3_transmit({ - .can_id = 0x141, - .can_data = chassis_joint_motors_[3].generate_command().as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x142, - .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), - }); - } - } - - private: - DeformableInfantryV2& deformable_infantry_; - - static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - - static double to_physical_angle_(double motor_angle) { - return joint_zero_physical_angle_rad_ - motor_angle; - } - - static double to_physical_velocity_(double motor_velocity) { return -motor_velocity; } - - void update_joint_physical_feedback_( - size_t index, OutputInterface& angle_output, - OutputInterface& velocity_output) { - if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; - *velocity_output = nan_; - return; - } - - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); - *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); - } - - void update_geometry_feedback_() { - const Eigen::Vector4d alpha_rad{ - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_}; - const Eigen::Vector4d alpha_dot_rad{ - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_}; - - if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = nan_; - return; - } - - *encoder_alpha_ = alpha_rad.mean(); - *encoder_alpha_dot_ = alpha_dot_rad.mean(); - *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); - } - - void log_chassis_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_chassis_feedback_log_time_) - return; - - const auto wheel_rx = [this](size_t index) { - return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - const auto joint_rx = [this](size_t index) { - return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; - }; - - if (debug_log_wheel_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " - "rx=[%c %c %c %c]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), wheel_rx(0), - wheel_rx(1), wheel_rx(2), wheel_rx(3)); - } - - if (debug_log_deformable_joint_motor_) { - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " - "rx=[%c %c %c %c]", - *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, - *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, - *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, - joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); - } - - next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void log_supercap_feedback_once_per_second_() { - const auto now = Clock::now(); - if (now < next_supercap_feedback_log_time_) - return; - - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); - auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); - const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); - - RCLCPP_INFO( - deformable_infantry_.get_logger(), - "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " - "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, - supercap_rx ? supercap_.supercap_voltage() : nan_, - supercap_rx ? supercap_.chassis_voltage() : nan_, - supercap_rx ? supercap_.chassis_power() : nan_, - std::to_integer(supercap_raw_bytes[0]), - std::to_integer(supercap_raw_bytes[1]), - std::to_integer(supercap_raw_bytes[2]), - std::to_integer(supercap_raw_bytes[3]), - std::to_integer(supercap_raw_bytes[4]), - std::to_integer(supercap_raw_bytes[5]), - std::to_integer(supercap_raw_bytes[6]), - std::to_integer(supercap_raw_bytes[7])); - - next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data); - } - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[0].store_status(data.can_data); - wheel_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[0].store_status(data.can_data); - joint_status_received_[0].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[0].store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[1].store_status(data.can_data); - wheel_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[1].store_status(data.can_data); - joint_status_received_[1].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[1].store_status(data.can_data); - else if (data.can_id == 0x300) { - if (data.can_data.size() == 8) - latest_supercap_status_.store( - device::CanPacket8{data.can_data}, std::memory_order_relaxed); - supercap_.store_status(data.can_data); - supercap_status_received_.store(true, std::memory_order_relaxed); - } - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[2].store_status(data.can_data); - wheel_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[2].store_status(data.can_data); - joint_status_received_[2].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x142) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x203) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (data.can_id == 0x205) - chassis_steer_motors_[2].store_status(data.can_data); - } - - void can3_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) - return; - if (data.can_id == 0x201) { - chassis_wheel_motors_[3].store_status(data.can_data); - wheel_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x141) { - chassis_joint_motors_[3].store_status(data.can_data); - joint_status_received_[3].store(true, std::memory_order_relaxed); - } else if (data.can_id == 0x205) - chassis_steer_motors_[3].store_status(data.can_data); - } - - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - const std::byte* ptr = data.uart_data.data(); - referee_ring_buffer_receive_.emplace_back_n( - [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - rmcs_executor::Component& command_; - - OutputInterface& tf_; - - device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; - device::Dr16 dr16_; - - device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, - }; - device::DjiMotor chassis_steer_motors_[4]{ - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_steering"}, - device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_steering"}, - }; - device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, - device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, - }; - - std::atomic wheel_status_received_[4] = {false, false, false, false}; - std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - device::Supercap supercap_{deformable_infantry_, command_}; - std::atomic latest_supercap_status_{ - device::CanPacket8{uint64_t{0}}}; - std::atomic supercap_status_received_{false}; - device::DjiMotor gimbal_bullet_feeder_{ - deformable_infantry_, command_, "/gimbal/bullet_feeder"}; - - rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - OutputInterface chassis_yaw_velocity_imu_; - OutputInterface chassis_imu_pitch_; - OutputInterface chassis_imu_roll_; - OutputInterface chassis_imu_pitch_rate_; - OutputInterface chassis_imu_roll_rate_; - OutputInterface left_front_joint_physical_angle_; - OutputInterface left_back_joint_physical_angle_; - OutputInterface right_back_joint_physical_angle_; - OutputInterface right_front_joint_physical_angle_; - OutputInterface left_front_joint_physical_velocity_; - OutputInterface left_back_joint_physical_velocity_; - OutputInterface right_back_joint_physical_velocity_; - OutputInterface right_front_joint_physical_velocity_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - }; - - class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: - friend class DeformableInfantryV2; - - explicit TopBoard( - DeformableInfantryV2& deformableInfantry, - DeformableInfantryV2Command& deformableInfantry_command, - device::Vt13& vt13, const std::string& serial_filter = {}) - : librmcs::agent::RmcsBoardLite( - serial_filter, - librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) - , tf_(deformableInfantry.tf_) - , vt13_{vt13} - , gimbal_imu_{1000, 0.2, 0.0} - , gimbal_pitch_motor_(deformableInfantry, deformableInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - deformableInfantry, deformableInfantry_command, "/gimbal/right_friction") { - - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10} - .set_reversed() - .set_encoder_zero_point( - static_cast( - deformableInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reduction_ratio(1.) - .set_reversed()); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); - - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - - gimbal_imu_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); - } - - ~TopBoard() override = default; - - void update() { - gimbal_imu_.update_status(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - Eigen::Quaterniond const gimbal_imu_pose{ - gimbal_imu_.q0(), gimbal_imu_.q1(), gimbal_imu_.q2(), gimbal_imu_.q3()}; - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); - *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); - - const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - tf_->set_state( - pitch_encoder_angle); - } - - void command_update() { - auto builder = start_transmit(); - builder.can0_transmit({ - .can_id = 0x141, - .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), - }); - builder.can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - gimbal_left_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - builder.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - } - - private: - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); - } - - void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - - void can0_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x141) - gimbal_pitch_motor_.store_status(data.can_data); - } - - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x201) - gimbal_left_friction_.store_status(data.can_data); - } - - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - if (data.can_id == 0x202) - gimbal_right_friction_.store_status(data.can_data); - } - - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { - gimbal_imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - gimbal_imu_.store_gyroscope_status(data.x, data.y, data.z); - } - - OutputInterface& tf_; - - OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface gimbal_yaw_velocity_imu_; - device::Vt13& vt13_; - - device::Bmi088 gimbal_imu_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - }; - - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Yaw: {}", bottom_board_->gimbal_yaw_motor_.last_raw_angle()); - text("- Pitch: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - text("- left front wheel: {}", bottom_board_->chassis_wheel_motors_[0].last_raw_angle()); - text("- left back wheel: {}", bottom_board_->chassis_wheel_motors_[1].last_raw_angle()); - text("- right back wheel: {}", bottom_board_->chassis_wheel_motors_[2].last_raw_angle()); - text("- right front wheel: {}", bottom_board_->chassis_wheel_motors_[3].last_raw_angle()); - text("- left front steer: {}", bottom_board_->chassis_steer_motors_[0].last_raw_angle()); - text("- left back steer: {}", bottom_board_->chassis_steer_motors_[1].last_raw_angle()); - text("- right back steer: {}", bottom_board_->chassis_steer_motors_[2].last_raw_angle()); - text("- right front steer: {}", bottom_board_->chassis_steer_motors_[3].last_raw_angle()); - text("- left front joint: {}", bottom_board_->chassis_joint_motors_[0].last_raw_angle()); - text("- left back joint: {}", bottom_board_->chassis_joint_motors_[1].last_raw_angle()); - text("- right back joint: {}", bottom_board_->chassis_joint_motors_[2].last_raw_angle()); - text("- right front joint: {}", bottom_board_->chassis_joint_motors_[3].last_raw_angle()); - - response->message = feedback_message.str(); - } - - OutputInterface tf_; - device::Vt13 vt13_; - - std::shared_ptr deformable_infantry_command_; - std::unique_ptr bottom_board_; - std::unique_ptr top_board_; - std::unique_ptr remote_control_; - - std::shared_ptr> status_service_; - uint32_t cmd_tick_ = 0; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DeformableInfantryV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 78387530..738c32a3 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -62,19 +63,17 @@ class DeformableInfantry register_input("/chassis/voltage", chassis_voltage_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < 4; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + } - register_input( - "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, false); - register_input( - "/chassis/left_back_joint/physical_angle", left_back_joint_physical_angle_, false); - register_input( - "/chassis/right_back_joint/physical_angle", right_back_joint_physical_angle_, false); - register_input( - "/chassis/right_front_joint/physical_angle", right_front_joint_physical_angle_, false); + for (size_t i = 0; i < 4; ++i) { + register_input( + fmt::format("/chassis/{}_joint/physical_angle", kWheelName[i]), + joint_physical_angle_[i], false); + } register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -153,19 +152,16 @@ class DeformableInfantry } void update_deformable_chassis_leg_arcs() { - if (!left_front_joint_physical_angle_.ready() || !left_back_joint_physical_angle_.ready() - || !right_back_joint_physical_angle_.ready() - || !right_front_joint_physical_angle_.ready()) { + if (!std::all_of( + joint_physical_angle_.begin(), joint_physical_angle_.end(), + [](const auto& j) { return j.ready(); })) { deformable_chassis_leg_arcs_.set_visible(false); return; } - const std::array leg_angles = { - *left_front_joint_physical_angle_, - *left_back_joint_physical_angle_, - *right_back_joint_physical_angle_, - *right_front_joint_physical_angle_, - }; + std::array leg_angles; + for (size_t i = 0; i < 4; ++i) + leg_angles[i] = *joint_physical_angle_[i]; deformable_chassis_leg_arcs_.update( *chassis_angle_, leg_angles, active_suspension_active_.ready() && *active_suspension_active_); } @@ -199,10 +195,12 @@ class DeformableInfantry InputInterface chassis_voltage_; - InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, - right_front_velocity_; - InputInterface left_front_joint_physical_angle_, left_back_joint_physical_angle_, - right_back_joint_physical_angle_, right_front_joint_physical_angle_; + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + + std::array, 4> wheel_velocity_; + std::array, 4> joint_physical_angle_; InputInterface robot_bullet_allowance_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index a1268855..9e2dd8a3 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -1,7 +1,9 @@ #include +#include #include #include +#include #include #include #include @@ -54,10 +56,11 @@ class Infantry register_input("/chassis/control_power_limit", chassis_control_power_limit_); register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); - register_input("/chassis/left_front_wheel/velocity", left_front_velocity_); - register_input("/chassis/left_back_wheel/velocity", left_back_velocity_); - register_input("/chassis/right_back_wheel/velocity", right_back_velocity_); - register_input("/chassis/right_front_wheel/velocity", right_front_velocity_); + for (size_t i = 0; i < 4; ++i) { + register_input( + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), + wheel_velocity_[i]); + } register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -144,8 +147,11 @@ class Infantry InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface left_front_velocity_, left_back_velocity_, right_back_velocity_, - right_front_velocity_; + static constexpr const char* kWheelName[] = { + "left_front", "left_back", "right_back", "right_front", + }; + + std::array, 4> wheel_velocity_; InputInterface robot_bullet_allowance_; From 1eb6bf9a2ee96498231dc36c2c2c744a45a0290e Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 15:44:46 +0800 Subject: [PATCH 72/90] Squashed commit of the following: commit 1e99fd9191b7cc74125234dff105878e6b9aeb70 Author: Zihan Qin Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab65867b13d54858f7d0938b309ad619d3fe02 Author: Zihan Qin Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect commit b5ae6cb717b64b300086ec6cbc434b48561d450c Author: Zihan Qin Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d36a9a4e1de2e54cb452352ef2d16263ff Author: qzhhhi Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files --- .../src/rmcs_bringup/config/mecanum-hero.yaml | 161 ---- .../rmcs_bringup/config/omni-infantry.yaml | 6 +- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 4 +- .../config/steering-hero-little.yaml | 417 +++++++++ .../rmcs_bringup/config/steering-hero.yaml | 399 ++++++++ .../config/steering-infantry.yaml | 80 +- rmcs_ws/src/rmcs_core/plugins.xml | 9 + .../src/broadcaster/tf_broadcaster.cpp | 2 + .../src/broadcaster/value_broadcaster.cpp | 19 +- .../src/rmcs_core/src/controller/adrc/eso.hpp | 4 +- .../rmcs_core/src/controller/adrc/nlesf.hpp | 14 +- .../src/rmcs_core/src/controller/adrc/td.hpp | 14 +- .../chassis/chassis_climber_controller.cpp | 722 +++++++++++++++ .../controller/chassis/chassis_controller.cpp | 24 +- .../chassis/chassis_power_controller.cpp | 13 +- .../deformable_omni_wheel_controller.cpp | 12 +- .../chassis/deformable_wheel_controller.cpp | 4 +- .../chassis/hero_chassis_controller.cpp | 303 ++++++ .../chassis/hero_chassis_power_controller.cpp | 232 +++++ .../hero_steering_wheel_controller.cpp | 518 +++++++++++ .../chassis/omni_wheel_controller.cpp | 6 +- .../src/controller/chassis/qcp_solver.hpp | 11 +- .../chassis/steering_wheel_status.cpp | 104 +++ .../controller/gimbal/dual_yaw_controller.cpp | 75 +- .../gimbal/hero_gimbal_controller.cpp | 48 +- .../src/controller/gimbal/player_viewer.cpp | 48 +- .../gimbal/precise_two_axis_gimbal_solver.hpp | 43 +- .../pid/friction_wheel_pid_recorder.cpp | 166 ++++ .../controller/pid/matrix_pid_calculator.hpp | 4 +- .../bullet_feeder_controller_42mm.cpp | 88 +- .../hero_friction_wheel_controller.cpp | 263 ++++++ .../shooting/hero_heat_controller.cpp | 63 ++ .../controller/shooting/putter_controller.cpp | 407 ++++++++ .../controller/shooting/shooting_recorder.cpp | 205 ++++- .../src/filter/alpha_beta_angle_filter.hpp | 8 +- rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp | 428 +++++++++ .../rmcs_core/src/filter/low_pass_filter.hpp | 2 +- .../src/hardware/device/benewake.hpp | 2 +- .../rmcs_core/src/hardware/device/bmi088.hpp | 8 +- .../src/hardware/device/bmi088_ekf.hpp | 172 ++++ .../hardware/device/board_clock_lifter.hpp | 53 ++ .../rmcs_core/src/hardware/device/gy614.hpp | 8 +- .../src/hardware/device/lk_motor.hpp | 7 +- .../rmcs_core/src/hardware/omni_infantry.cpp | 87 +- .../src/hardware/steering-hero-little.cpp | 865 ++++++++++++++++++ .../src/rmcs_core/src/referee/app/ui/hero.cpp | 397 +++++++- .../rmcs_core/src/referee/app/ui/infantry.cpp | 4 +- .../referee/app/ui/shape/cfs_scheduler.hpp | 12 +- .../referee/app/ui/shape/red_black_tree.hpp | 36 +- .../src/referee/app/ui/shape/remote_shape.hpp | 16 +- .../src/referee/app/ui/shape/shape.hpp | 165 ++-- .../app/ui/widget/crosshair_circle.hpp | 3 +- .../ui/widget/deformable_chassis_top_view.hpp | 18 +- .../src/referee/app/ui/widget/rangefinder.hpp | 10 +- .../src/referee/app/ui/widget/status_ring.hpp | 18 +- rmcs_ws/src/rmcs_core/src/referee/command.cpp | 4 +- .../src/referee/command/interaction/ui.cpp | 14 +- rmcs_ws/src/rmcs_core/src/referee/frame.hpp | 2 +- .../rmcs_core/src/referee/status/field.hpp | 2 +- .../include/rmcs_executor/component.hpp | 402 +++++++- rmcs_ws/src/rmcs_executor/package.xml | 1 + rmcs_ws/src/rmcs_executor/src/component.cpp | 4 +- rmcs_ws/src/rmcs_executor/src/executor.hpp | 249 ++++- rmcs_ws/src/rmcs_executor/src/main.cpp | 4 +- .../include/rmcs_msgs/board_clock.hpp | 17 + .../include/rmcs_msgs/camera_frame_raw.hpp | 28 + .../include/rmcs_msgs/chassis_mode.hpp | 6 +- .../include/rmcs_msgs/full_robot_id.hpp | 64 +- .../include/rmcs_msgs/game_stage.hpp | 12 +- .../include/rmcs_msgs/imu_snapshot.hpp | 14 + .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 27 +- .../include/rmcs_msgs/robot_color.hpp | 4 +- .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 66 +- .../include/rmcs_msgs/serial_interface.hpp | 4 +- .../include/rmcs_msgs/shoot_condiction.hpp | 14 + .../include/rmcs_utility/atomic_futex.hpp | 160 ++++ .../include/rmcs_utility/crc/dji_crc.hpp | 8 +- .../include/rmcs_utility/double_buffer.hpp | 2 +- .../include/rmcs_utility/fps_counter.hpp | 6 +- .../include/rmcs_utility/memory_pool.hpp | 196 ++++ .../include/rmcs_utility/package_receive.hpp | 4 +- .../rmcs_utility/pooled_shared_factory.hpp | 76 ++ .../include/rmcs_utility/ring_buffer.hpp | 219 +++++ 83 files changed, 7629 insertions(+), 787 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml deleted file mode 100644 index 41d3572a..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml +++ /dev/null @@ -1,161 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::MecanumHero -> hero_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::Hero -> referee_ui_hero - - - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_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::BulletFeederController42mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_right_friction_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 - -hero_hardware: - ros__parameters: - 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 - -gimbal_controller: - ros__parameters: - upper_limit: -0.4598 - lower_limit: 0.4362 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - feedforward: -/chassis/yaw/velocity_imu - control: /gimbal/yaw/control_velocity - kp: 16.0 - ki: 0.0 - kd: 0.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 16.00 - ki: 0.0 - kd: 0.0 - output_max: 10.0 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/first_left_friction - - /gimbal/first_right_friction - - /gimbal/second_left_friction - - /gimbal/second_right_friction - friction_velocities: - - 450.00 - - 450.00 - - 535.00 - - 535.00 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 100000 - reserved_heat: 0 - -second_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_left_friction/velocity - setpoint: /gimbal/second_left_friction/control_velocity - control: /gimbal/second_left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -first_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -first_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_right_friction/velocity - setpoint: /gimbal/first_right_friction/control_velocity - control: /gimbal/first_right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -second_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_right_friction/velocity - setpoint: /gimbal/second_right_friction/control_velocity - control: /gimbal/second_right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - 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.0 - ki: 0.001 - kd: 0.0 - integral_min: -1000.0 - integral_max: 1000.0 - -left_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_front_wheel/velocity - setpoint: /chassis/left_front_wheel/control_velocity - control: /chassis/left_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -left_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_back_wheel/velocity - setpoint: /chassis/left_back_wheel/control_velocity - control: /chassis/left_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_back_wheel/velocity - setpoint: /chassis/right_back_wheel/control_velocity - control: /chassis/right_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_front_wheel/velocity - setpoint: /chassis/right_front_wheel/control_velocity - control: /chassis/right_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 diff --git a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml index 6364aec7..304407cf 100644 --- a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml @@ -53,9 +53,9 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 - ki: 0.001 - kd: 0.01 + kp: 4.4 + ki: 0.00011 + kd: 0.0011 pitch_angle_pid_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 99c03710..cec1cd16 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -79,7 +79,7 @@ gimbal_controller: bottom_yaw_angle_kp: 15.0 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 22.5 + bottom_yaw_velocity_kp: 2.8125 bottom_yaw_velocity_ki: 0.0 bottom_yaw_velocity_kd: 0.0 @@ -92,7 +92,7 @@ gimbal_controller: k_top_to_bottom: -1.0 - bottom_yaw_viscous_ff_gain: 0.002495 + bottom_yaw_viscous_ff_gain: 0.000311875 # bottom_yaw_coulomb_ff_gain: 0.457343 # bottom_yaw_coulomb_ff_tanh_gain: 100.0 top_yaw_viscous_ff_gain: 0.231 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml new file mode 100644 index 00000000..53970767 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml @@ -0,0 +1,417 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHeroLittle -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + +hero_hardware: + ros__parameters: + board_serial_top_board: "AF-BFF7-0B6F-46A5-2B4B-AA20-89C2-E180-64B9" + serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" + bottom_yaw_motor_zero_point: 35078 + pitch_motor_zero_point: 57241 + top_yaw_motor_zero_point: 48537 + viewer_motor_zero_point: 31940 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 60480 #39045 + left_front_zero_point: 5799 + right_front_zero_point: 3700 + left_back_zero_point: 7862 + right_back_zero_point: 1608 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/top_yaw/control_angle + - /chassis/climber/left_front_motor/torque + - /chassis/climber/right_front_motor/torque + - /chassis/left_front_steering/torque + - /chassis/left_back_steering/torque + - /chassis/right_front_steering/torque + - /chassis/right_back_steering/torque + # - /shoot/heat12707 + # - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + # - /chassis/control_power_limit + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque + # - /gimbal/bottom_yaw/torque + # - /gimbal/bottom_yaw/angle + # - /gimbal/top_yaw/angle + # - /gimbal/pitch/angle + # - /chassis/power + - /chassis/supercap/voltage + # - /chassis/control_power_limit + # - /referee/chassis/power_limit + # - /referee/chassis/buffer_energy + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + - /gimbal/bullet_feeder/torque + - /gimbal/bullet_feeder/control_torque + - /gimbal/putter/angle + - /gimbal/putter/velocity + - /gimbal/putter/torque + - /gimbal/putter/control_torque + - /gimbal/bullet_feeder/velocity + - /gimbal/bullet_feeder/angle + + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 50.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 130.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.668 + lower_limit: 0.320 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 14.0 #30.2 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 9.37 #11.0 + top_yaw_velocity_ki: 0.00033 #0.00029 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 10.0 #18.4 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.00 #2.81 + bottom_yaw_velocity_ki: 0.000071 #0.00028 + bottom_yaw_velocity_kd: 0.0 + bottom_yaw_velocity_integral_min: -2500.0 + bottom_yaw_velocity_integral_max: 2500.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 18.6 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 16.05 + ki: 0.00014 + kd: 0.0 + integral_min: -2500.0 + integral_max: 2500.0 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.415996 + lower_limit: -0.066441 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 4.00 + ki: 0.00 + kd: 0.50 + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.0 + bullet_feeder_velocity_ki: 0.1 #1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 6.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.6 + putter_return_velocity_kp: 0.0025 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/second_left_friction + - /gimbal/second_right_friction + - /gimbal/first_left_friction + - /gimbal/first_right_friction + friction_velocities_profile_0: + - 380.0 + - 380.0 + - 545.0 + - 545.0 + friction_velocities_profile_1: + - 535.0 + - 535.0 + - 595.0 + - 595.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 1: trigger, 2: timing + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00016 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00016 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.286378 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + wheel_radius: 0.055 + friction_coefficient: 0.6 + 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: 1 + 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. + pitch_error: 0. + 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: 2 + +hero_auto_aim_bridge: + ros__parameters: + config_file: "configs/standard3.yaml" + bullet_speed_fallback: 11.4 + result_timeout: 0.1 # 0.08 + debug: false + + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +pitch_static_torque_test_controller: + ros__parameters: + target: /gimbal/pitch + + interval_angle: 0.05 + wait_time: 1.5 + border_clip: 0.05 + + position_kp: 12.0 + position_ki: 0.0 + position_kd: 0.0 + + velocity_kp: 3.2 + velocity_ki: 0.0005 + velocity_kd: 0.0 + + velocity_integral_min: -2500.0 + velocity_integral_max: 2500.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 15.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml new file mode 100644 index 00000000..a6e01d35 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -0,0 +1,399 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHero -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + + + + +hero_hardware: + ros__parameters: + board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" + board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" + board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" + bottom_yaw_motor_zero_point: 37424 + pitch_motor_zero_point: 14916 + top_yaw_motor_zero_point: 33076 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 11865 + left_front_zero_point: 4398 + right_front_zero_point: 1081 + left_back_zero_point: 3794 + right_back_zero_point: 5839 + + + + +value_broadcaster: + ros__parameters: + forward_list: + # - /shoot/heat + - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + - /chassis/control_power_limit + - /chassis/climber/front/control_power_limit + - /chassis/climber/front/power_demand_estimate + - /chassis/climber/front/actual_power_estimate + - /chassis/steering_wheel/actual_power_estimate + # - /chassis/supercap/voltage + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque + # - /gimbal/bottom_yaw/torque + - /gimbal/bottom_yaw/control_angle_shift + - /gimbal/bottom_yaw/angle + - /gimbal/top_yaw/angle + - /gimbal/pitch/angle + + # - /gimbal/pitch/velocity_imu + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_torque + + - /gimbal/auto_aim/plan_yaw + - /gimbal/auto_aim/plan_pitch + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 60.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 150.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.688 + lower_limit: 0.357 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 5.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 10.0 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 13.9 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.0 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 0.0 + +# dual_yaw_controller: +# ros__parameters: +# top_yaw_angle_kp: 24.5 +# top_yaw_angle_ki: 0.0 +# top_yaw_angle_kd: 0.0 +# top_yaw_velocity_kp: 77.4 +# top_yaw_velocity_ki: 0.004 +# top_yaw_velocity_kd: 1.0 +# bottom_yaw_angle_kp: 8.6 +# bottom_yaw_angle_ki: 0.0 +# bottom_yaw_angle_kd: 0.0 +# bottom_yaw_velocity_kp: 25.85 +# bottom_yaw_velocity_ki: 0.0 +# bottom_yaw_velocity_kd: 50.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 12.00 #45.00 + ki: 0.00 + kd: 0.0 #1.00 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.68 + lower_limit: 1.17 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 17.00 + ki: 0.00 + kd: 2.00 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + friction_velocities_profile_0: + - 368.00 + - 368.00 + - 532.00 + - 532.00 + friction_velocities_profile_1: + - 525.0 + - 525.0 + - 585.0 + - 585.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 1: trigger, 2: timing + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.5 + bullet_feeder_velocity_ki: 1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 5.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.0 + putter_return_velocity_kp: 0.0015 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: false + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.0005 + ki: 0.00 + kd: 0.00004 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.0005 + ki: 0.00 + kd: 0.00004 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.0009 + ki: 0.00 + kd: 0.00008 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.0009 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.318198 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + wheel_radius: 0.055 + friction_coefficient: 0.6 + 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: 1 + 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. + pitch_error: 0. + 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: 2 + +# hero_auto_aim_bridge: +# ros__parameters: +# config_file: "configs/standard3.yaml" +# bullet_speed_fallback: 11.7 +# result_timeout: 0.1 # 0.08 +# debug: false + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 20.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 2.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml index d650d044..2e344d14 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml @@ -2,18 +2,18 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::SteeringInfantry -> steeringInfantry_hardware + - rmcs_core::hardware::SteeringInfantry -> 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::referee::app::ui::Infantry -> referee_ui_infantry + - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -26,37 +26,47 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller -steeringInfantry_hardware: +infantry_hardware: ros__parameters: - 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 - left_back_zero_point: 5770 - right_back_zero_point: 2380 - right_front_zero_point: 1705 + board_serial_top_board: "AF-B4E5-CE0E-4342-FF2C-F9E2-DE47-2D85-9B75" + board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" + yaw_motor_zero_point: 20993 + pitch_motor_zero_point: 18874 + left_front_zero_point: 1695 + right_front_zero_point: 5068 + left_back_zero_point: 7870 + right_back_zero_point: 3141 + gimbal_controller: ros__parameters: - upper_limit: -0.39518 - lower_limit: 0.36 + upper_limit: -0.54 + lower_limit: 0.274 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 15.0 + ki: 0.0 + kd: 1.5 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 16.0 + kp: 20.0 ki: 0.0 - kd: 0.0 + kd: 0.9 -pitch_angle_pid_controller: +yaw_velocity_pid_controller: ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 10.00 - ki: 0.0 - kd: 0.0 + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 2.5 + ki: 0.00 + kd: 0.6 friction_wheel_controller: ros__parameters: @@ -64,9 +74,9 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 - friction_soft_start_stop_time: 1.0 + - 590.0 + - 590.0 + friction_soft_start_stop_time: 0.3 heat_controller: ros__parameters: @@ -75,8 +85,8 @@ heat_controller: bullet_feeder_controller: ros__parameters: - bullets_per_feeder_turn: 10.0 - shot_frequency: 24.0 + bullets_per_feeder_turn: 9.0 + shot_frequency: 27.0 safe_shot_frequency: 10.0 eject_frequency: 15.0 eject_time: 0.15 @@ -86,8 +96,8 @@ bullet_feeder_controller: shooting_recorder: ros__parameters: - friction_wheel_count: 4 - log_mode: 2 # 1: trigger, 2: timing + friction_wheel_count: 2 + log_mode: 1 left_friction_velocity_pid_controller: ros__parameters: @@ -112,15 +122,15 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 0.283 + kp: 0.7 ki: 0.0 kd: 0.0 steering_wheel_controller: ros__parameters: - mess: 19.0 - moment_of_inertia: 1.0 - vehicle_radius: 0.24678 + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 wheel_radius: 0.055 friction_coefficient: 0.6 k1: 2.958580e+00 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1438f8a8..8d30c5cf 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,6 +1,7 @@ + @@ -9,9 +10,14 @@ + + + + + @@ -24,9 +30,12 @@ + + + diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp index 40dfe60a..f3d13c70 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp @@ -26,6 +26,8 @@ class TfBroadcaster } ~TfBroadcaster() = default; + void before_updating() override { fast_tf::rcl::broadcast_all(*tf_); } + void update() override { using namespace std::chrono_literals; if (*update_count_ == 0) diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp index c9b9a584..ec612065 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -14,15 +14,16 @@ class ValueBroadcaster : Node{get_component_name()} { declare_parameter>("forward_list", std::vector{}); parameter_subscriber_ = std::make_unique(this); - parameter_callback_ = parameter_subscriber_->add_parameter_callback( + parameter_callback_ = parameter_subscriber_->add_parameter_callback( "forward_list", [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); } - void before_pairing( - const std::map& output_map) override { - for (const auto& [name, type] : output_map) { - if (type == typeid(double)) { + void before_pairing(const OutputInfoMap& output_map) override { + for (const auto& [name, output] : output_map) { + if (output.kind != rmcs_executor::InterfaceKind::Normal) + continue; + if (output.type.get() == typeid(double)) { forward_units_.emplace( name, std::make_unique>(this, name)); @@ -52,7 +53,7 @@ class ValueBroadcaster "unsupported or the output does not exist.", name.c_str()); } else { - auto& unit = iter->second; + auto& unit = iter->second; unit->active = true; } } @@ -70,8 +71,8 @@ class ValueBroadcaster virtual ~BasicForwarderUnit() {} virtual void activate(rclcpp::Node* node) = 0; - virtual void update() = 0; - virtual void deactivate() = 0; + virtual void update() = 0; + virtual void deactivate() = 0; bool active; }; @@ -118,4 +119,4 @@ class ValueBroadcaster #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp index e7d9d6ee..0d79b300 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp @@ -66,9 +66,7 @@ class ESO { } private: - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp index c3bd6725..ad21b4fb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp @@ -47,22 +47,22 @@ class NLESF { constexpr double kEps = 1e-9; const double safe_b0 = (std::fabs(b0) < kEps) ? ((b0 >= 0.0) ? kEps : -kEps) : b0; - const double u0 = cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) - + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); + const double u0 = + cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); const double u = clamp((u0 - z3) / safe_b0, cfg_.u_min, cfg_.u_max); return Output{u0, u}; } private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp index 6b14ce5f..390d5711 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp @@ -57,14 +57,14 @@ class TD { private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } static double fhan(double x1_minus_v, double x2, double r, double h) { const double d = r * h * h; @@ -72,9 +72,7 @@ class TD { const double y = x1_minus_v + a0; const double a1 = std::sqrt(d * (d + 8.0 * std::fabs(y))); - const double a = (std::fabs(y) > d) - ? (a0 + sign(y) * (a1 - d) * 0.5) - : (a0 + y); + const double a = (std::fabs(y) > d) ? (a0 + sign(y) * (a1 - d) * 0.5) : (a0 + y); if (std::fabs(a) <= d) { return -r * a / d; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp new file mode 100644 index 00000000..cfd1a669 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,722 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +namespace { +double estimate_front_power( + double left_torque, double right_torque, double left_velocity, double right_velocity, + double bias, double k_tau2, double k_mech) { + + if (!std::isfinite(left_torque) || !std::isfinite(right_torque)) + return 0.0; + + return bias + k_tau2 * (std::pow(left_torque, 2) + std::pow(right_torque, 2)) + + k_mech + * (std::abs(left_torque * left_velocity) + std::abs(right_torque * right_velocity)); +} +} // namespace + +enum class AutoClimbState { IDLE, ALIGN, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; + +class ChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_support_retract_velocity_fast_abs_ = + get_parameter("auto_climb_support_retract_velocity_fast").as_double(); + auto_climb_support_retract_velocity_slow_abs_ = + get_parameter("auto_climb_support_retract_velocity_slow").as_double(); + auto_climb_approach_chassis_velocity_ = + get_parameter("auto_climb_approach_chassis_velocity").as_double(); + auto_climb_support_deploy_chassis_velocity_ = + get_parameter("auto_climb_support_deploy_chassis_velocity").as_double(); + auto_climb_support_retract_chassis_velocity_ = + get_parameter("auto_climb_support_retract_chassis_velocity").as_double(); + auto_climb_dash_chassis_velocity_ = + get_parameter("auto_climb_dash_chassis_velocity").as_double(); + first_stair_dash_leveled_pitch_threshold_ = + get_parameter("first_stair_dash_leveled_pitch_threshold").as_double(); + second_stair_dash_leveled_pitch_threshold_ = + get_parameter("second_stair_dash_leveled_pitch_threshold").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + front_power_estimate_bias_ = get_parameter("front_power_estimate_bias").as_double(); + front_power_estimate_k_tau2_ = get_parameter("front_power_estimate_k_tau2").as_double(); + front_power_estimate_k_mech_ = get_parameter("front_power_estimate_k_mech").as_double(); + + register_output( + "/chassis/climber/left_front_motor/requested_control_torque", + climber_front_left_requested_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/requested_control_torque", + climber_front_right_requested_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + register_output("/chassis/climber/auto_climb_active", auto_climb_active_, false); + register_output( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_output( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, 0.0); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + front_power_limiter_ = create_partner_component( + get_component_name() + "_front_power_limiter", front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void update() override { + using namespace rmcs_msgs; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_climb_control(update_auto_climb_control()); + } else { + apply_climb_control(update_manual_support_control(keyboard)); + } + } + + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + class ChassisClimberFrontPowerLimiter : public rmcs_executor::Component { + public: + ChassisClimberFrontPowerLimiter(double bias, double k_tau2, double k_mech) + : front_power_estimate_bias_(bias) + , front_power_estimate_k_tau2_(k_tau2) + , front_power_estimate_k_mech_(k_mech) { + register_input( + "/chassis/climber/left_front_motor/requested_control_torque", + left_requested_control_torque_); + register_input( + "/chassis/climber/right_front_motor/requested_control_torque", + right_requested_control_torque_); + register_input("/chassis/climber/left_front_motor/velocity", left_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_velocity_); + register_input("/chassis/climber/left_front_motor/max_torque", left_max_torque_); + register_input("/chassis/climber/right_front_motor/max_torque", right_max_torque_); + register_input("/chassis/climber/front/control_power_limit", control_power_limit_); + + register_output( + "/chassis/climber/left_front_motor/control_torque", left_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", right_control_torque_, nan_); + register_output( + "/chassis/climber/front/actual_power_estimate", actual_power_estimate_, 0.0); + } + + void update() override { + const double left_requested = *left_requested_control_torque_; + const double right_requested = *right_requested_control_torque_; + + if (!std::isfinite(left_requested) || !std::isfinite(right_requested)) { + *left_control_torque_ = nan_; + *right_control_torque_ = nan_; + *actual_power_estimate_ = 0.0; + return; + } + + if (*control_power_limit_ <= 0.0) { + *left_control_torque_ = 0.0; + *right_control_torque_ = 0.0; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, + *right_velocity_, front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + return; + } + + const double left_torque = + std::clamp(left_requested, -*left_max_torque_, *left_max_torque_); + const double right_torque = + std::clamp(right_requested, -*right_max_torque_, *right_max_torque_); + + const double estimated_power = estimate_front_power( + left_torque, right_torque, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + + if (estimated_power <= *control_power_limit_ || estimated_power <= 0.0) { + *left_control_torque_ = left_torque; + *right_control_torque_ = right_torque; + *actual_power_estimate_ = estimated_power; + return; + } + + const double scale = std::clamp(*control_power_limit_ / estimated_power, 0.0, 1.0); + *left_control_torque_ = left_torque * scale; + *right_control_torque_ = right_torque * scale; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + } + + private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + InputInterface left_requested_control_torque_; + InputInterface right_requested_control_torque_; + InputInterface left_velocity_; + InputInterface right_velocity_; + InputInterface left_max_torque_; + InputInterface right_max_torque_; + InputInterface control_power_limit_; + + OutputInterface left_control_torque_; + OutputInterface right_control_torque_; + OutputInterface actual_power_estimate_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = false; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + stop_auto_climb(); + start_back_climber_retract("Auto climb exit"); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + if (auto_climb_state_ == AutoClimbState::IDLE) + return {}; + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + back_climber_zero_velocity_hold_ = false; + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + start_back_climber_retract("Manual support"); + } + + if (!manual_support_retracting_) { + if (back_climber_zero_velocity_hold_) + control.back_climber_velocity = 0.0; + return control; + } + + if (back_climber_recover_count > 1200) { + control.back_climber_velocity = -auto_climb_support_retract_velocity_slow_abs_; + } else { + control.back_climber_velocity = -auto_climb_support_retract_velocity_fast_abs_; + } + + if (is_back_climber_blocked()) + manual_support_retract_block_count_++; + else + manual_support_retract_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = true; + control.back_climber_velocity = 0.0; + RCLCPP_INFO(logger_, "Manual support retract completed."); + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -5.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) + auto_climb_align_stable_count_++; + else + auto_climb_align_stable_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -5.0, + .override_chassis_vx = auto_climb_approach_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_support_deploy_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double leveled_pitch_threshold = auto_climb_stair_index_ == 0 + ? first_stair_dash_leveled_pitch_threshold_ + : second_stair_dash_leveled_pitch_threshold_; + bool is_leveled = + pitch < leveled_pitch_threshold && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, threshold=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, leveled_pitch_threshold, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = back_climber_recover_count <= 1200 + ? -auto_climb_support_retract_velocity_fast_abs_ + : -auto_climb_support_retract_velocity_slow_abs_, + .override_chassis_vx = auto_climb_support_retract_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportRetractConfirmTicks) { + bool has_next_stair = auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs; + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + back_climber_zero_velocity_hold_ = true; + control.front_track_velocity = nan_; + control.back_climber_velocity = 0.0; + control.override_chassis_vx = nan_; + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + void apply_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + *auto_climb_active_ = auto_climb_state_ != AutoClimbState::IDLE; + if (back_climber_recover_count != 0) { + back_climber_recover_count--; + } + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + if (back_climber_recover_count > 1200) { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_first_torque_); + } else { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_second_torque_); + } + + *front_power_budget_active_ = is_front_power_budget_active(); + *front_power_demand_estimate_ = estimate_front_power( + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void reset_all_controls() { + *climber_front_left_requested_control_torque_ = nan_; + *climber_front_right_requested_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + *auto_climb_active_ = false; + *front_power_budget_active_ = false; + *front_power_demand_estimate_ = 0.0; + stop_manual_support(); + stop_auto_climb(); + back_climber_zero_velocity_hold_ = false; + } + + void stop_manual_support() { + back_climber_recover_count = 1500; + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + } + + void start_back_climber_retract(const char* source) { + if (!back_climber_recover_count) { + back_climber_recover_count = 1500; + } + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + back_climber_zero_velocity_hold_ = false; + RCLCPP_INFO(logger_, "%s back climber retract started.", source); + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) + return; + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + bool is_front_power_budget_active() const { + return auto_climb_state_ == AutoClimbState::APPROACH + || auto_climb_state_ == AutoClimbState::SUPPORT_RETRACT; + } + + void dual_motor_sync_control( + double setpoint, double left_velocity, double right_velocity, + pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, + double& right_torque_out) { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + void limit_back_climber_retract_torque( + double back_climber_velocity_setpoint, double& left_torque, double& right_torque, + double max_torque) const { + + if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) + return; + if (!(max_torque > 0.0)) + return; + + const double peak = std::max(std::abs(left_torque), std::abs(right_torque)); + if (peak <= max_torque) + return; + + const double scale = max_torque / peak; + left_torque *= scale; + right_torque *= scale; + } + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 100; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractConfirmTicks = 50; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_support_retract_velocity_fast_abs_; + double auto_climb_support_retract_velocity_slow_abs_; + double auto_climb_approach_chassis_velocity_; + double auto_climb_support_deploy_chassis_velocity_; + double auto_climb_support_retract_chassis_velocity_; + double auto_climb_dash_chassis_velocity_; + double first_stair_dash_leveled_pitch_threshold_; + double second_stair_dash_leveled_pitch_threshold_; + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool back_climber_zero_velocity_hold_ = false; + + OutputInterface climber_front_left_requested_control_torque_; + OutputInterface climber_front_right_requested_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + OutputInterface auto_climb_active_; + OutputInterface front_power_budget_active_; + OutputInterface front_power_demand_estimate_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; + + std::shared_ptr front_power_limiter_; + + double back_climber_retract_first_torque_ = 8.0; + double back_climber_retract_second_torque_ = 0.5; + int back_climber_recover_count = 0; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index da70a96f..5b7d9bca 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -58,8 +58,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +74,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -100,8 +100,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,7 +112,7 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } @@ -133,7 +133,7 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { @@ -171,7 +171,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -202,7 +202,7 @@ class ChassisController // Maximum control velocities static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double angular_velocity_max = 16.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,8 +214,8 @@ class ChassisController InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 1eee280c..8c4b38e1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -52,8 +52,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -75,8 +76,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -117,8 +118,8 @@ class ChassisPowerController // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index 7ae3c193..ec5922b8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -194,14 +194,14 @@ class DeformableOmniWheelController const double force_to_torque = friction_coefficient_ * wheel_radius_; const double rhs = force_to_torque * mass_ * g_ / 4.0; const std::vector half_planes = { - { lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, + {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, - { lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, + {lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, {-lambda_2 - force_to_torque * gamma_2, -y_sign, rhs}, - {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, - { lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, - {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, - { lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, + {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, + {lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, + {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, + {lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, }; result = qcp_solver_.solve( {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp index ba3b68c0..e25d9cdb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_wheel_controller.cpp @@ -153,7 +153,6 @@ class DeformableChassisController "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); register_output( "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); - } void update() override { @@ -567,7 +566,8 @@ class DeformableChassisController {rhombus_right, rhombus_top}, quadratic_constraint); const double min_translational = 0.3 * rhombus_right; - if (best_point.x() < min_translational && translational_acceleration_max > min_translational) + if (best_point.x() < min_translational + && translational_acceleration_max > min_translational) best_point.x() = min_translational; Eigen::Vector3d best_acceleration; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp new file mode 100644 index 00000000..dd12e09e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -0,0 +1,303 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 2.4) { + following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_min = -angular_velocity_max; + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + + register_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto mode = *mode_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::BACK)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::FRONT; + } + } else if (!last_keyboard_.z && keyboard.z) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::FRONT)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + } + } + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(translational_velocity); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + + auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: { + angular_velocity = + update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle); + + // Keep AUTO rear-following gentle at low translation speed and fully enabled at max. + const double measured_translational_speed = + chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm() + : translational_velocity.norm(); + angular_velocity *= + std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + + } break; + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + } break; + case rmcs_msgs::ChassisMode::STEP_DOWN: { + angular_velocity = + update_following_angular_velocity(step_down_facing_, chassis_control_angle); + } break; + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> signed + // In launch ramp mode, only one direction can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + } break; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); + + return err; + } + +private: + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + // Maximum control velocities + static constexpr double translational_velocity_max = 10.0; + static constexpr double angular_velocity_max = 16.0; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + bool spinning_forward_ = true; + bool chassis_velocity_feedback_ready_ = false; + StepDownFacing step_down_facing_ = StepDownFacing::BACK; + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp new file mode 100644 index 00000000..ee59e1ce --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::controller::chassis { + +using namespace referee::app; + +class HeroChassisPowerController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisPowerController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + get_parameter("front_climber_power_limit_max", front_climber_power_limit_max_); + get_parameter("drive_power_limit_floor", drive_power_limit_floor_); + get_parameter("auto_climb_min_control_power_limit", auto_climb_min_control_power_limit_); + register_input("/chassis/control_mode", mode_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/chassis/power", chassis_power_); + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); + register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_input( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, false); + register_input("/chassis/climber/auto_climb_active", auto_climb_active_, false); + + register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); + register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); + register_output( + "/chassis/climber/front/control_power_limit", front_climber_control_power_limit_, 0.0); + + register_output( + "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, 12.5); + register_output("/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, 12.0); + register_output("/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, 11.0); + } + + void before_updating() override { + if (!front_power_budget_active_.ready()) + front_power_budget_active_.make_and_bind_directly(false); + if (!front_power_demand_estimate_.ready()) + front_power_demand_estimate_.make_and_bind_directly(0.0); + if (!auto_climb_active_.ready()) + auto_climb_active_.make_and_bind_directly(false); + } + + void update() override { + update_charging_power_limit(); + + update_ui(); + + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_power_control(); + return; + } + + update_virtual_buffer_energy(); + + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || *auto_climb_active_; + update_control_power_limit(); + } + +private: + void update_charging_power_limit() { + // Maximum excess power when buffer energy is sufficient. + constexpr double excess_power_limit = 35; + + // charging_power_limit = + constexpr double buffer_energy_control_line = 120; // = referee + excess + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 + + *supercap_charge_power_limit_ = + *chassis_power_limit_referee_ + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_dead_line) + / (buffer_energy_base_line - buffer_energy_dead_line), + 0.0, 1.0) + + excess_power_limit + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_base_line) + / (buffer_energy_control_line - buffer_energy_base_line), + 0.0, 1.0); + } + + void reset_power_control() { + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; + *chassis_control_power_limit_ = 0.0; + *front_climber_control_power_limit_ = 0.0; + } + + void update_virtual_buffer_energy() { + constexpr double dt = 1e-3; + virtual_buffer_energy_ += dt * (chassis_power_limit_expected_ - *chassis_power_); + virtual_buffer_energy_ = std::clamp( + virtual_buffer_energy_, 0.0, + std::min(*chassis_buffer_energy_referee_, virtual_buffer_energy_limit_)); + } + + void update_control_power_limit() { + double total_power_limit; + + if (boost_mode_ && *supercap_enabled_) + total_power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? inf_ + : *chassis_power_limit_referee_ + 80.0; + else + total_power_limit = *chassis_power_limit_referee_; + chassis_power_limit_expected_ = total_power_limit; + + // chassis_control_power_limit = + constexpr double supercap_voltage_control_line = 12.5; // = supercap + constexpr double supercap_voltage_base_line = 12.0; // = referee + total_power_limit = + *chassis_power_limit_referee_ + + (total_power_limit - *chassis_power_limit_referee_) + * std::clamp( + (*supercap_voltage_ - supercap_voltage_base_line) + / (supercap_voltage_control_line - supercap_voltage_base_line), + 0.0, 1.0); + + // Maximum excess power when virtual buffer energy is full. + constexpr double excess_power_limit = 0; + + total_power_limit += excess_power_limit; + total_power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; + + if (*auto_climb_active_) + total_power_limit = std::max(total_power_limit, auto_climb_min_control_power_limit_); + + const auto [drive_limit, front_limit] = split_control_power_limit(total_power_limit); + *chassis_control_power_limit_ = drive_limit; + *front_climber_control_power_limit_ = front_limit; + } + + std::pair split_control_power_limit(double total_power_limit) const { + if (!*front_power_budget_active_) + return {total_power_limit, 0.0}; + + if (total_power_limit <= drive_power_limit_floor_) + return {total_power_limit, 0.0}; + + const double front_limit = std::min( + {*front_power_demand_estimate_, front_climber_power_limit_max_, + total_power_limit - drive_power_limit_floor_}); + return {total_power_limit - front_limit, front_limit}; + } + + void update_ui() { + chassis_power_ui_.set_value(static_cast(std::round(*chassis_power_))); + chassis_control_power_limit_ui_.set_value( + static_cast(std::round(*chassis_control_power_limit_))); + } + + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface mode_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + InputInterface chassis_power_; + static constexpr double virtual_buffer_energy_limit_ = 30.0; + double virtual_buffer_energy_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_power_limit_referee_; + InputInterface chassis_buffer_energy_referee_; + InputInterface front_power_budget_active_; + InputInterface front_power_demand_estimate_; + InputInterface auto_climb_active_; + + bool boost_mode_ = false; + OutputInterface supercap_charge_power_limit_; + double chassis_power_limit_expected_; + OutputInterface chassis_control_power_limit_; + OutputInterface front_climber_control_power_limit_; + double front_climber_power_limit_max_; + double drive_power_limit_floor_; + double auto_climb_min_control_power_limit_ = 0.0; + + OutputInterface supercap_voltage_control_line_; + OutputInterface supercap_voltage_base_line_; + OutputInterface supercap_voltage_dead_line_; + + ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; + ui::Integer chassis_control_power_limit_ui_{ + ui::Shape::Color::WHITE, 15, 2, ui::x_center, 150, 0}; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisPowerController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp new file mode 100644 index 00000000..1db280c5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp @@ -0,0 +1,518 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/qcp_solver.hpp" +#include "controller/pid/matrix_pid_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroSteeringWheelController + : public rmcs_executor::Component + , public rclcpp::Node { + + using Formula = std::tuple; + +public: + explicit HeroSteeringWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , mess_(get_parameter("mess").as_double()) + , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) + , friction_coefficient_(get_parameter("friction_coefficient").as_double()) + , k1_(get_parameter("k1").as_double()) + , k2_(get_parameter("k2").as_double()) + , no_load_power_(get_parameter("no_load_power").as_double()) + , control_acceleration_filter_(5.0, 1000.0) + , chassis_velocity_expected_(Eigen::Vector3d::Zero()) + , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) + , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) + , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2 + , sin_varphi_(0, 1, 0, -1) + , steering_velocity_pid_(0.15, 0.0, 0.0) + , steering_angle_pid_(30.0, 0.0, 0.0) + , wheel_velocity_pid_(0.6, 0.0, 0.0) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + + register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); + register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); + register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); + register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); + + register_input("/chassis/left_front_steering/velocity", left_front_steering_velocity_); + register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); + register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); + register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); + + register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); + + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_power_limit", power_limit_); + + register_output( + "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); + register_output( + "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); + register_output( + "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); + register_output( + "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); + + register_output( + "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); + register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); + register_output( + "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); + register_output( + "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); + register_output( + "/chassis/steering_wheel/actual_power_estimate", steering_wheel_actual_power_estimate_, + 0.0); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + integral_yaw_angle_imu(); + + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + auto chassis_velocity = calculate_chassis_velocity(steering_status, wheel_velocities); + + auto chassis_status_expected = calculate_chassis_status_expected(chassis_velocity); + auto chassis_control_velocity = calculate_chassis_control_velocity(); + + auto chassis_acceleration = calculate_chassis_control_acceleration( + chassis_status_expected.velocity, chassis_control_velocity); + + double power_limit = + *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); + + auto wheel_pid_torques = + calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); + + auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( + steering_status, wheel_velocities, chassis_acceleration, wheel_pid_torques, + power_limit); + auto filtered_chassis_acceleration = + odom_to_base_link_vector(control_acceleration_filter_.update( + base_link_to_odom_vector(constrained_chassis_acceleration))); + + auto steering_torques = calculate_steering_control_torques( + steering_status, chassis_status_expected, filtered_chassis_acceleration); + auto wheel_torques = calculate_wheel_control_torques( + steering_status, filtered_chassis_acceleration, wheel_pid_torques); + + update_control_torques(steering_torques, wheel_torques); + *steering_wheel_actual_power_estimate_ = + calculate_actual_power_estimate(wheel_velocities, wheel_torques); + update_chassis_velocity_expected(filtered_chassis_acceleration); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + Eigen::Vector4d velocity; + }; + + struct ChassisStatus { + Eigen::Vector3d velocity; + Eigen::Vector4d wheel_velocity_x, wheel_velocity_y; + }; + + void reset_all_controls() { + control_acceleration_filter_.reset(); + + chassis_yaw_angle_imu_ = 0.0; + chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + *left_front_steering_control_torque_ = 0.0; + *left_back_steering_control_torque_ = 0.0; + *right_back_steering_control_torque_ = 0.0; + *right_front_steering_control_torque_ = 0.0; + + *left_front_wheel_control_torque_ = 0.0; + *left_back_wheel_control_torque_ = 0.0; + *right_back_wheel_control_torque_ = 0.0; + *right_front_wheel_control_torque_ = 0.0; + *steering_wheel_actual_power_estimate_ = 0.0; + } + + void integral_yaw_angle_imu() { + chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; + chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); + } + + SteeringStatus calculate_steering_status() { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + steering_status.velocity = { + *left_front_steering_velocity_, // + *left_back_steering_velocity_, // + *right_back_steering_velocity_, // + *right_front_steering_velocity_ // + }; + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + ChassisStatus calculate_chassis_status_expected(const Eigen::Vector3d& chassis_velocity) { + auto calculate_energy = [this](const Eigen::Vector3d& velocity) { + return mess_ * velocity.head<2>().squaredNorm() + + moment_of_inertia_ * velocity.z() * velocity.z(); + }; + auto chassis_energy = calculate_energy(chassis_velocity); + auto chassis_energy_expected = calculate_energy(chassis_velocity_expected_); + if (chassis_energy_expected > chassis_energy) { + double k = std::sqrt(chassis_energy / chassis_energy_expected); + chassis_velocity_expected_ *= k; + } + + ChassisStatus chassis_status_expected; + chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + chassis_status_expected.wheel_velocity_x = vx - vehicle_radius_ * vz * sin_varphi_.array(); + chassis_status_expected.wheel_velocity_y = vy + vehicle_radius_ * vz * cos_varphi_.array(); + + return chassis_status_expected; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; + chassis_control_velocity.head<2>() = + Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); + + return chassis_control_velocity; + } + + Eigen::Vector3d calculate_chassis_control_acceleration( + const Eigen::Vector3d& chassis_velocity_expected, + const Eigen::Vector3d& chassis_control_velocity) { + + Eigen::Vector2d translational_control_velocity = chassis_control_velocity.head<2>(); + Eigen::Vector2d translational_velocity = chassis_velocity_expected.head<2>(); + Eigen::Vector2d translational_control_acceleration = + chassis_translational_velocity_pid_.update( + translational_control_velocity - translational_velocity); + + const double& angular_control_velocity = chassis_control_velocity[2]; + const double& angular_velocity = chassis_velocity_expected[2]; + double angular_control_acceleration = + chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); + + Eigen::Vector3d chassis_control_acceleration; + chassis_control_acceleration << translational_control_acceleration, + angular_control_acceleration; + + if (chassis_control_acceleration.lpNorm<1>() < 1e-1) + chassis_control_acceleration.setZero(); + + return chassis_control_acceleration; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const ChassisStatus& chassis_status_expected) { + Eigen::Vector4d wheel_control_velocity = + chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() + + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); + return wheel_velocity_pid_.update( + wheel_control_velocity / wheel_radius_ - wheel_velocities); + } + + Eigen::Vector3d constrain_chassis_control_acceleration( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_acceleration, const Eigen::Vector4d& wheel_pid_torques, + const double& power_limit) { + + Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); + double translational_acceleration_max = translational_acceleration_direction.norm(); + if (translational_acceleration_max > 0.0) + translational_acceleration_direction /= translational_acceleration_max; + + double angular_acceleration_max = chassis_acceleration.z(); + double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; + angular_acceleration_max *= angular_acceleration_direction; + + const double rhombus_right = friction_coefficient_ * g_; + const double rhombus_top = rhombus_right * mess_ * vehicle_radius_ / moment_of_inertia_; + + auto [a, b, c, d, e, f] = calculate_ellipse_parameters( + steering_status, wheel_velocities, translational_acceleration_direction, + angular_acceleration_direction, wheel_pid_torques); + + Eigen::Vector2d best_point = qcp_solver_.solve( + {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f - power_limit}); + + Eigen::Vector3d best_acceleration; + best_acceleration << best_point.x() * translational_acceleration_direction, + best_point.y() * angular_acceleration_direction; + return best_acceleration; + } + + Eigen::Vector calculate_ellipse_parameters( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector2d& translational_acceleration_direction, + const double& angular_acceleration_direction, const Eigen::Vector4d& wheel_torque_base) { + Eigen::Vector4d cos_alpha_minus_gamma = + steering_status.cos_angle.array() * translational_acceleration_direction.x() + + steering_status.sin_angle.array() * translational_acceleration_direction.y(); + Eigen::Vector4d sin_alpha_minus_varphi = + cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array(); + Eigen::Vector4d double_k1_torque_base_plus_wheel_velocities = + 2 * k1_ * wheel_torque_base.array() + wheel_velocities.array(); + + Eigen::Vector formula; + auto& [a, b, c, d, e, f] = formula; + + a = (k1_ * mess_ * mess_ * wheel_radius_ * wheel_radius_ / 16.0) + * cos_alpha_minus_gamma.array().pow(2).sum(); + b = ((k1_ * mess_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (8.0 * vehicle_radius_)) + * angular_acceleration_direction + * (cos_alpha_minus_gamma.array() * sin_alpha_minus_varphi.array()).sum(); + c = ((k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (16.0 * vehicle_radius_ * vehicle_radius_)) + * sin_alpha_minus_varphi.array().pow(2).sum(); + d = (mess_ * wheel_radius_ / 4.0) + * (double_k1_torque_base_plus_wheel_velocities.array() * cos_alpha_minus_gamma.array()) + .sum(); + e = ((moment_of_inertia_ * wheel_radius_) / (4.0 * vehicle_radius_)) + * angular_acceleration_direction + * (double_k1_torque_base_plus_wheel_velocities.array() * sin_alpha_minus_varphi.array()) + .sum(); + f = (wheel_torque_base.array() + * (k1_ * wheel_torque_base.array() + wheel_velocities.array())) + .sum(); + + return formula; + } + + Eigen::Vector4d calculate_steering_control_torques( + const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, + const Eigen::Vector3d& chassis_acceleration) { + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + const auto& [ax, ay, az] = chassis_acceleration; + + Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() + + chassis_status_expected.wheel_velocity_y.array().square(); + + Eigen::Vector4d steering_control_velocities = + vx * ay - vy * ax - vz * (vx * vx + vy * vy) + + vehicle_radius_ * (az * vx - vz * (ax + vz * vy)) * cos_varphi_.array() + + vehicle_radius_ * (az * vy - vz * (ay - vz * vx)) * sin_varphi_.array(); + Eigen::Vector4d steering_control_angles; + + for (int i = 0; i < steering_control_velocities.size(); ++i) { + if (dot_r_squared[i] > 1e-2) { + steering_control_velocities[i] /= dot_r_squared[i]; + steering_control_angles[i] = std::atan2( + chassis_status_expected.wheel_velocity_y[i], + chassis_status_expected.wheel_velocity_x[i]); + } else { + auto x = ax - vehicle_radius_ * (az * sin_varphi_[i] + 0 * cos_varphi_[i]); + auto y = ay + vehicle_radius_ * (az * cos_varphi_[i] - 0 * sin_varphi_[i]); + if (x * x + y * y > 1e-6) { + steering_control_velocities[i] = 0.0; + steering_control_angles[i] = std::atan2(y, x); + } else { + steering_control_velocities[i] = nan_; + steering_control_angles[i] = nan_; + } + } + } + + Eigen::Vector4d steering_torques = steering_velocity_pid_.update( + steering_control_velocities + + steering_angle_pid_.update( + (steering_control_angles - steering_status.angle).unaryExpr([](double diff) { + diff = std::fmod(diff, std::numbers::pi); + if (diff < -std::numbers::pi / 2) { + diff += std::numbers::pi; + } else if (diff > std::numbers::pi / 2) { + diff -= std::numbers::pi; + } + return diff; + })) + - steering_status.velocity); + return steering_torques.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Eigen::Vector4d calculate_wheel_control_torques( + const SteeringStatus& steering_status, const Eigen::Vector3d& chassis_acceleration, + const Eigen::Vector4d& wheel_pid_torques) { + + const auto& [ax, ay, az] = chassis_acceleration; + Eigen::Vector4d wheel_torques = + wheel_radius_ + * (ax * mess_ * steering_status.cos_angle.array() + + ay * mess_ * steering_status.sin_angle.array() + + az * moment_of_inertia_ + * (cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array()) + / vehicle_radius_) + / 4.0; + + wheel_torques += wheel_pid_torques; + + return wheel_torques; + } + + double calculate_actual_power_estimate( + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector4d& wheel_torques) const { + return no_load_power_ + k2_ * wheel_velocities.array().pow(2).sum() + + (wheel_torques.array() * (k1_ * wheel_torques.array() + wheel_velocities.array())) + .sum(); + } + + void update_control_torques( + const Eigen::Vector4d& steering_torques, const Eigen::Vector4d& wheel_torques) { + *left_front_steering_control_torque_ = steering_torques[0]; + *left_back_steering_control_torque_ = steering_torques[1]; + *right_back_steering_control_torque_ = steering_torques[2]; + *right_front_steering_control_torque_ = steering_torques[3]; + + *left_front_wheel_control_torque_ = wheel_torques[0]; + *left_back_wheel_control_torque_ = wheel_torques[1]; + *right_back_wheel_control_torque_ = wheel_torques[2]; + *right_front_wheel_control_torque_ = wheel_torques[3]; + } + + void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { + auto acceleration_odom = base_link_to_odom_vector(chassis_acceleration); + chassis_velocity_expected_ += dt_ * acceleration_odom; + } + + Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.81; + + const double mess_; + const double moment_of_inertia_; + const double vehicle_radius_; + const double wheel_radius_; + const double friction_coefficient_; + + const double k1_, k2_, no_load_power_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_steering_velocity_; + InputInterface left_back_steering_velocity_; + InputInterface right_back_steering_velocity_; + InputInterface right_front_steering_velocity_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + + OutputInterface left_front_steering_control_torque_; + OutputInterface left_back_steering_control_torque_; + OutputInterface right_back_steering_control_torque_; + OutputInterface right_front_steering_control_torque_; + + OutputInterface left_front_wheel_control_torque_; + OutputInterface left_back_wheel_control_torque_; + OutputInterface right_back_wheel_control_torque_; + OutputInterface right_front_wheel_control_torque_; + OutputInterface steering_wheel_actual_power_estimate_; + + QcpSolver qcp_solver_; + filter::LowPassFilter<3> control_acceleration_filter_; + + double chassis_yaw_angle_imu_ = 0.0; + Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; + pid::PidCalculator chassis_angular_velocity_pid_; + + const Eigen::Vector4d cos_varphi_, sin_varphi_; + + pid::MatrixPidCalculator<4> steering_velocity_pid_, steering_angle_pid_, wheel_velocity_pid_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroSteeringWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index a6e181da..fc194e4a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -131,8 +131,10 @@ class OmniWheelController const auto& [x, y, z] = chassis_velocity; constexpr double a_plus_b = chassis_radius_x_ + chassis_radius_y_; Eigen::Vector4d wheel_control_velocity = { - -x + y + a_plus_b * z, -x - y + a_plus_b * z, // - +x - y + a_plus_b * z, +x + y + a_plus_b * z, // + -x + y + a_plus_b * z, + -x - y + a_plus_b * z, // + +x - y + a_plus_b * z, + +x + y + a_plus_b * z, // }; wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp index aeba601a..6c708586 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp @@ -61,9 +61,9 @@ class QcpSolver { const std::vector& half_planes, const QuadraticConstraint& quadratic_constraint) { std::vector polygon = { - {boundary_constraint.x_max, boundary_constraint.y_max}, - { 0.0, boundary_constraint.y_max}, - { 0.0, -boundary_constraint.y_max}, + {boundary_constraint.x_max, boundary_constraint.y_max}, + {0.0, boundary_constraint.y_max}, + {0.0, -boundary_constraint.y_max}, {boundary_constraint.x_max, -boundary_constraint.y_max}, }; @@ -129,10 +129,7 @@ class QcpSolver { [&line](const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) -> Eigen::Vector2d { Eigen::Vector3d line2 = Eigen::Vector3d(p1.x(), p1.y(), 1).cross(Eigen::Vector3d(p2.x(), p2.y(), 1)); - Eigen::Matrix2d matrix{ - { line.x(), line.y()}, - {line2.x(), line2.y()} - }; + Eigen::Matrix2d matrix{{line.x(), line.y()}, {line2.x(), line2.y()}}; return matrix.inverse() * Eigen::Vector2d(-line.z(), -line2.z()); }); } diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp new file mode 100644 index 00000000..8450b386 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class SteeringWheelStatus + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit SteeringWheelStatus() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) { + register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); + register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); + register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); + register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); + + register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); + + register_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0); + } + + void update() override { + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + }; + + SteeringStatus calculate_steering_status() const { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() const { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + const double vehicle_radius_; + const double wheel_radius_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + OutputInterface chassis_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 739b5859..0d49184e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,9 +1,13 @@ +#include +#include #include #include +#include #include #include +#include #include "controller/pid/pid_calculator.hpp" @@ -39,14 +43,17 @@ class DualYawController register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0); + register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_); + register_output( + "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); + register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -61,6 +68,31 @@ class DualYawController } void update() override { + + const auto mode = *gimbal_mode_; + if (mode == rmcs_msgs::GimbalMode::ENCODER) { + const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + if (entering_encoder) { + if (std::isfinite(*top_yaw_angle_)) { + top_yaw_encoder_locked_ = true; + } else { + top_yaw_encoder_locked_ = false; + } + } + *bottom_yaw_control_angle_shift_ = *control_angle_shift_; + if (top_yaw_encoder_locked_) + *top_yaw_control_angle_ = top_yaw_encoder_angle_; + else + *top_yaw_control_angle_ = nan_; + } else { + *top_yaw_control_angle_ = nan_; + *bottom_yaw_control_angle_shift_ = nan_; + top_yaw_encoder_angle_ = *top_yaw_angle_; + top_yaw_encoder_locked_ = false; + } + + last_gimbal_mode_ = mode; + if (std::isnan(*control_angle_error_)) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_torque_ = nan_; @@ -72,23 +104,30 @@ class DualYawController bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) - bottom_yaw_velocity_imu()); } - - if (std::isnan(*control_angle_shift_)) { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - } else { - *top_yaw_control_angle_ = 0.0; - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; - } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + double wrap_angle(double angle) { + while (angle > 0) + angle += 2 * std::numbers::pi; + while (angle >= 2 * std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + double bottom_yaw_control_error() { - double err = *top_yaw_angle_ + *control_angle_error_; - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; + if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) + return nan_; + + // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. + constexpr double alignment = 2 * std::numbers::pi; + double err = + std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); + if (err < 0) + err += alignment; + err -= std::numbers::pi; return err; } @@ -99,6 +138,7 @@ class DualYawController InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; @@ -110,6 +150,13 @@ class DualYawController OutputInterface top_yaw_control_angle_; OutputInterface bottom_yaw_control_angle_shift_; + OutputInterface bottom_yaw_control_angle_; + OutputInterface top_yaw_control_angle_shift_; + + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + bool top_yaw_encoder_locked_ = false; + double top_yaw_encoder_angle_ = nan_; + class DualYawStatus : public rmcs_executor::Component { public: explicit DualYawStatus() { diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..939f17f6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -1,5 +1,7 @@ +#include #include +#include #include #include #include @@ -23,12 +25,10 @@ class HeroGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , upper_limit_(get_parameter("upper_limit").as_double()) + , lower_limit_(get_parameter("lower_limit").as_double()) + , imu_gimbal_solver(*this, upper_limit_, lower_limit_) + , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -38,6 +38,8 @@ class HeroGimbalController register_input("/remote/keyboard", keyboard_); register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -59,14 +61,17 @@ class HeroGimbalController break; } - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) + if (!last_keyboard_.e && keyboard_->e) { + if (gimbal_mode_keyboard_ == GimbalMode::IMU) { + encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else + } else { gimbal_mode_keyboard_ = GimbalMode::IMU; + } } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + + *gimbal_mode_ = gimbal_mode_keyboard_; + //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = update_imu_control(); @@ -76,6 +81,7 @@ class HeroGimbalController encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; + } else { imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; @@ -121,24 +127,26 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) - return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); + if (!encoder_gimbal_solver.enabled()) { + return encoder_gimbal_solver.update( + PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); + } - constexpr double joystick_sensitivity = 0.006 * 0.1; constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; + constexpr double joystick_sensitivity = 0.006 * 0.05; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); + + mouse_pitch_sensitivity * mouse_velocity_->x(); return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); @@ -147,6 +155,9 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kEInitPitch = -0.476972; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.541591; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; @@ -157,10 +168,13 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface auto_aim_control_direction_; + InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; + const double upper_limit_, lower_limit_; TwoAxisGimbalSolver imu_gimbal_solver; PreciseTwoAxisGimbalSolver encoder_gimbal_solver; @@ -173,4 +187,4 @@ class HeroGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp index 8eb7e647..d4d98715 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp @@ -27,7 +27,8 @@ class PlayerViewer register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_pitch_raw_angle_); register_input("/gimbal/player_viewer/angle", gimbal_player_viewer_angle_); register_output( @@ -46,20 +47,23 @@ class PlayerViewer using namespace rmcs_msgs; const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.e && keyboard.e) - viewer_reset_ = true; if (!last_keyboard_.q && keyboard.q) { - scope_active_ = !scope_active_; - *is_scope_active_ = scope_active_; + scope_active_ = !scope_active_; + *is_scope_active_ = scope_active_; scope_viewer_reset_ = scope_active_; } + if (!last_keyboard_.e && keyboard.e) { + viewer_init_angle_ = keyboard.ctrl ? kCtrlInitViewerAngle : kEInitViewerAngle; + viewer_reset_ = true; + scope_viewer_reset_ = false; + } update_viewer_control(); }; @@ -72,17 +76,17 @@ class PlayerViewer *scope_control_torque_ = nan_; *viewer_control_angle_error_ = nan_; - *viewer_control_angle_ = nan_; + *viewer_control_angle_ = nan_; - *is_scope_active_ = false; - scope_active_ = false; + *is_scope_active_ = false; + scope_active_ = false; scope_viewer_reset_ = false; - viewer_reset_ = true; + viewer_reset_ = true; } void update_viewer_control() { constexpr double scope_offset_angle = 0.31; - *scope_offset_angle_ = scope_offset_angle; + *scope_offset_angle_ = scope_offset_angle; auto unit_sensitivity = [&](double sensitivity) { return (*is_scope_active_) ? sensitivity : 1.0; @@ -92,12 +96,12 @@ class PlayerViewer *viewer_delta_angle_by_mouse_wheel_ = 0.5 * *mouse_wheel_ * unit_sensitivity(0.09); if (viewer_reset_) { - *viewer_control_angle_ = upper_limit_; - viewer_reset_ = false; + *viewer_control_angle_ = viewer_init_angle_; + viewer_reset_ = false; } else { if (scope_viewer_reset_) { *viewer_control_angle_ = *scope_offset_angle_; - scope_viewer_reset_ = false; + scope_viewer_reset_ = false; } else { *viewer_control_angle_ += *viewer_delta_angle_by_mouse_wheel_; } @@ -115,19 +119,24 @@ class PlayerViewer *viewer_control_angle_ - norm_angle(*gimbal_player_viewer_angle_); if (scope_active_) { - *scope_control_torque_ = 0.2; + *scope_control_torque_ = 0.13; } else { - *scope_control_torque_ = -0.2; + *scope_control_torque_ = -0.13; } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double pi_ = std::numbers::pi; + static constexpr double pi_ = std::numbers::pi; + + // The steering-hero viewer angle limit range is [0.68, 1.17]. + static constexpr double kEInitViewerAngle = 0.38905; // Move here when E is pressed. + static constexpr double kCtrlInitViewerAngle = 0.38905; // Move here when Ctrl is pressed. bool scope_viewer_reset_{false}; const double upper_limit_, lower_limit_; + double viewer_init_angle_ = kEInitViewerAngle; InputInterface switch_right_; InputInterface switch_left_; @@ -135,6 +144,7 @@ class PlayerViewer InputInterface mouse_wheel_; InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; InputInterface gimbal_player_viewer_angle_; OutputInterface scope_control_torque_; @@ -155,4 +165,4 @@ class PlayerViewer #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp index 9a5c2948..daa13997 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp @@ -25,8 +25,48 @@ class PreciseTwoAxisGimbalSolver { : upper_limit_(upper_limit) , lower_limit_(lower_limit) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/tf", tf_); } + struct SetControlDirection : Operation { + friend class PreciseTwoAxisGimbalSolver; + + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + double update(PreciseTwoAxisGimbalSolver& super) const { + if (!super.tf_.ready()) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + + auto dir_yaw_link = fast_tf::cast(target_, *super.tf_); + Eigen::Vector3d dir = *dir_yaw_link; + + const double norm = dir.norm(); + if (norm < kEps) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + dir /= norm; + + const double xy_norm = std::hypot(dir.x(), dir.y()); + + // In YawLink, yaw is the increment relative to the current total yaw. + const double yaw_shift = std::atan2(dir.y(), dir.x()); + + // Mechanical pitch convention: level is 0, pitching downward is positive, so negate it. + const double pitch_angle = -std::atan2(dir.z(), std::max(xy_norm, kEps)); + + super.set_pitch_angle(pitch_angle); + return yaw_shift; + } + + static constexpr double kEps = 1e-9; + OdomImu::DirectionVector target_; + }; + struct SetDisabled : Operation { friend class PreciseTwoAxisGimbalSolver; @@ -91,10 +131,11 @@ class PreciseTwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface tf_; const double upper_limit_, lower_limit_; double control_pitch_angle_ = nan_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp new file mode 100644 index 00000000..366f66b6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::controller::pid { + +class FrictionWheelPidRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FrictionWheelPidRecorder() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + const auto wheels = + declare_parameter>("wheels", std::vector{}); + if (wheels.empty()) + throw std::runtime_error("Parameter \"wheels\" must not be empty"); + + min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0); + + const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100); + if (flush_every_n_samples <= 0) + throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive"); + flush_every_n_samples_ = static_cast(flush_every_n_samples); + + const auto output_dir = std::filesystem::path{ + declare_parameter("output_dir", "/tmp/friction_pid_logs")}; + const auto output_name = declare_parameter("output_name", ""); + + wheel_count_ = wheels.size(); + wheel_ids_ = std::make_unique(wheel_count_); + setpoints_ = std::make_unique[]>(wheel_count_); + measurements_ = std::make_unique[]>(wheel_count_); + control_torques_ = std::make_unique[]>(wheel_count_); + run_ids_ = std::make_unique(wheel_count_); + sample_indices_ = std::make_unique(wheel_count_); + previously_enabled_ = std::make_unique(wheel_count_); + + for (size_t i = 0; i < wheel_count_; ++i) { + wheel_ids_[i] = wheel_id_from_topic(wheels[i]); + register_input(wheels[i] + "/control_velocity", setpoints_[i]); + register_input(wheels[i] + "/velocity", measurements_[i]); + register_input(wheels[i] + "/control_torque", control_torques_[i]); + + run_ids_[i] = 0; + sample_indices_[i] = 0; + previously_enabled_[i] = false; + } + + std::filesystem::create_directories(output_dir); + log_file_path_ = output_dir / resolve_output_name(output_name); + log_stream_.open(log_file_path_); + if (!log_stream_.is_open()) + throw std::runtime_error( + "Failed to open recorder output file: " + log_file_path_.string()); + + log_stream_.setf(std::ios::fixed); + log_stream_.precision(9); + log_stream_ + << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity," + "control_torque,enabled\n"; + + RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str()); + } + + ~FrictionWheelPidRecorder() override { + if (log_stream_.is_open()) { + log_stream_.flush(); + log_stream_.close(); + } + } + + void update() override { + const auto timestamp_us = now_timestamp_us(); + + for (size_t i = 0; i < wheel_count_; ++i) { + const bool enabled = should_record(i); + if (enabled && !previously_enabled_[i]) { + ++run_ids_[i]; + sample_indices_[i] = 0; + } + + if (enabled) { + log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ',' + << sample_indices_[i]++ << ',' << *setpoints_[i] << ',' + << *measurements_[i] << ',' << *control_torques_[i] << ",1\n"; + if (++unflushed_samples_ >= flush_every_n_samples_) { + log_stream_.flush(); + unflushed_samples_ = 0; + } + } + + previously_enabled_[i] = enabled; + } + } + +private: + static std::string wheel_id_from_topic(std::string_view topic) { + auto pos = topic.find_last_of('/'); + if (pos == std::string_view::npos || pos + 1 >= topic.size()) + return std::string(topic); + return std::string(topic.substr(pos + 1)); + } + + static uint64_t now_timestamp_us() { + const auto now = std::chrono::steady_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); + } + + static std::string resolve_output_name(const std::string& configured_name) { + if (!configured_name.empty()) + return configured_name; + + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const auto ms = std::chrono::duration_cast(now).count(); + return "friction_wheel_pid_" + std::to_string(ms) + ".csv"; + } + + bool should_record(size_t index) const { + if (!setpoints_[index].ready() || !measurements_[index].ready() + || !control_torques_[index].ready()) + return false; + + const auto setpoint = *setpoints_[index]; + const auto measurement = *measurements_[index]; + const auto control_torque = *control_torques_[index]; + + return std::isfinite(setpoint) && std::isfinite(measurement) + && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_; + } + + size_t wheel_count_ = 0; + double min_setpoint_abs_ = 10.0; + size_t flush_every_n_samples_ = 100; + size_t unflushed_samples_ = 0; + + std::filesystem::path log_file_path_; + std::ofstream log_stream_; + + std::unique_ptr wheel_ids_; + std::unique_ptr[]> setpoints_; + std::unique_ptr[]> measurements_; + std::unique_ptr[]> control_torques_; + std::unique_ptr run_ids_; + std::unique_ptr sample_indices_; + std::unique_ptr previously_enabled_; +}; + +} // namespace rmcs_core::controller::pid + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp index f17c35a1..68814799 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp @@ -12,7 +12,7 @@ namespace rmcs_core::controller::pid { template class MatrixPidCalculator { using Vector = Eigen::Vector; - using Gain = std::conditional, double>::type; + using Gain = std::conditional, double>::type; public: MatrixPidCalculator(Gain kp, Gain ki, Gain kd) @@ -35,7 +35,7 @@ class MatrixPidCalculator { Vector update(Vector err) { Vector control = kp * err + ki * err_integral_; - err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); + err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); control += exclude_nan(kd * (err - last_err_)); last_err_ = err; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index bf3076e3..d8bd95cb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp @@ -31,14 +31,15 @@ class BulletFeederController42mm register_input( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_limited_by_heat_); + register_input("/gimbal/bullet_fired", bullet_fired_); - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; - bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; bullet_feeder_velocity_pid_.integral_max = 60.0; bullet_feeder_velocity_pid_.integral_min = 0.0; - bullet_feeder_angle_pid_.kp = 50.0; + bullet_feeder_angle_pid_.kp = 60.0; bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; @@ -51,9 +52,9 @@ class BulletFeederController42mm void update() override { const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -61,7 +62,6 @@ class BulletFeederController42mm reset_all_controls(); return; } - overdrive_mode_ = keyboard.f; if (keyboard.ctrl && !last_keyboard_.r && keyboard.r) low_latency_mode_ = !low_latency_mode_; @@ -84,8 +84,8 @@ class BulletFeederController42mm } else { if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) { bullet_feeder_control_angle_ = *bullet_feeder_angle_; - shoot_stage_ = ShootStage::PRELOADED; - bullet_fed_count_ = static_cast( + shoot_stage_ = ShootStage::PRELOADED; + bullet_fed_count_ = static_cast( (*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1) / bullet_feeder_angle_per_bullet_); } @@ -100,12 +100,10 @@ class BulletFeederController42mm } } - double err_abs = std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f %.2f", bullet_feeder_control_angle_, - // *bullet_feeder_angle_, err_abs); + double angle_error_abs = + std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); if (shoot_stage_ == ShootStage::PRELOADING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloaded(); } if (shoot_stage_ == ShootStage::PRELOADED) { @@ -113,46 +111,42 @@ class BulletFeederController42mm set_compressing(); } if (shoot_stage_ == ShootStage::COMPRESSING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_compressed(); } if (shoot_stage_ == ShootStage::SHOOTING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloading(); } } - double velocity_err = bullet_feeder_angle_pid_.update( - bullet_feeder_control_angle_ - *bullet_feeder_angle_) - - *bullet_feeder_velocity_; - // bullet_feeder_velocity_pid_.integral_max = std::clamp(1000.0 * velocity_err, - // 0.0, 60.0); - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f", velocity_err, *bullet_feeder_control_torque_); + double velocity_error = bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_error); update_jam_detection(); } last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_mouse_ = mouse; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; } private: void reset_all_controls() { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_mouse_ = rmcs_msgs::Mouse::zero(); - last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); overdrive_mode_ = low_latency_mode_ = false; - shoot_stage_ = ShootStage::PRELOADED; + shoot_stage_ = ShootStage::PRELOADED; bullet_fed_count_ = std::numeric_limits::min(); - bullet_feeder_control_angle_ = nan_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_angle_pid_.output_max = inf_; bullet_feeder_velocity_pid_.reset(); @@ -160,13 +154,13 @@ class BulletFeederController42mm *bullet_feeder_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; + bullet_feeder_cool_down_ = 0; } void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); bullet_fed_count_++; - shoot_stage_ = ShootStage::PRELOADING; + shoot_stage_ = ShootStage::PRELOADING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -179,7 +173,7 @@ class BulletFeederController42mm void set_compressing() { RCLCPP_INFO(get_logger(), "COMPRESSING"); - shoot_stage_ = ShootStage::COMPRESSING; + shoot_stage_ = ShootStage::COMPRESSING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 0.8; @@ -192,14 +186,13 @@ class BulletFeederController42mm void set_shooting() { RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; + shoot_stage_ = ShootStage::SHOOTING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; } void update_jam_detection() { - // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); if (*bullet_feeder_control_torque_ < 300.0) { bullet_feeder_faulty_count_ = 0; return; @@ -215,7 +208,7 @@ class BulletFeederController42mm void enter_jam_protection() { bullet_feeder_control_angle_ = nan_; - bullet_feeder_cool_down_ = 1000; + bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); RCLCPP_INFO(get_logger(), "Jammed!"); @@ -225,9 +218,10 @@ class BulletFeederController42mm static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double bullet_feeder_compressed_zero_point_ = 0.58; - static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; InputInterface friction_ready_; + InputInterface bullet_fired_; InputInterface switch_right_; InputInterface switch_left_; @@ -235,9 +229,9 @@ class BulletFeederController42mm InputInterface keyboard_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); bool overdrive_mode_ = false, low_latency_mode_ = false; @@ -247,8 +241,8 @@ class BulletFeederController42mm InputInterface control_bullet_allowance_limited_by_heat_; enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; - int bullet_fed_count_ = std::numeric_limits::min(); + ShootStage shoot_stage_ = ShootStage::PRELOADED; + int bullet_fed_count_ = std::numeric_limits::min(); double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -256,7 +250,7 @@ class BulletFeederController42mm OutputInterface bullet_feeder_control_torque_; int bullet_feeder_faulty_count_ = 0; - int bullet_feeder_cool_down_ = 0; + int bullet_feeder_cool_down_ = 0; OutputInterface shoot_mode_; }; @@ -266,4 +260,4 @@ class BulletFeederController42mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp new file mode 100644 index 00000000..9f4e62f0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -0,0 +1,263 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroFrictionWheelController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroFrictionWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + + auto friction_wheels = get_parameter("friction_wheels").as_string_array(); + auto friction_profile_0 = get_parameter("friction_velocities_profile_0").as_double_array(); + auto friction_profile_1 = get_parameter("friction_velocities_profile_1").as_double_array(); + if (friction_wheels.size() != friction_profile_0.size() + || friction_wheels.size() != friction_profile_1.size()) { + throw std::runtime_error( + "'friction_wheels' and both friction velocity profiles must have the same length!"); + } else if (friction_wheels.size() == 0) + throw std::runtime_error( + "Empty array error: 'friction_wheels' and 'friction_velocities' cannot be empty!"); + + friction_profile_0_.assign(friction_profile_0.begin(), friction_profile_0.end()); + friction_profile_1_.assign(friction_profile_1.begin(), friction_profile_1.end()); + friction_count_ = friction_wheels.size(); + friction_velocities_ = std::make_unique[]>(friction_count_); + friction_control_velocities_ = std::make_unique[]>(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); + } + + friction_soft_start_stop_step_ = + (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); + + register_output("/gimbal/friction_ready", friction_ready_, false); + register_output("/gimbal/friction_jammed", friction_jammed_, false); + register_output("/gimbal/bullet_fired", bullet_fired_, false); + register_output("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + if (!last_keyboard_.f && keyboard.f) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + const bool profile_switch_now = keyboard.ctrl && keyboard.f; + const bool profile_switch_last = last_keyboard_.ctrl && last_keyboard_.f; + + if (!profile_switch_last && profile_switch_now) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + + *friction_profile_1_active_ = active_profile_ == 1; + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + if (switch_right != Switch::DOWN) { + if ((!last_keyboard_.v && keyboard.v) + || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { + friction_enabled_ = !friction_enabled_; + } + + update_friction_velocities(); + update_friction_status(); + if (*friction_jammed_) + RCLCPP_INFO(logger_, "Friction Jammed!"); + if (*bullet_fired_) + RCLCPP_INFO(logger_, "Bullet Fired!"); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + if (!friction_enabled_) { + reset_all_controls(); + } + } + +private: + void reset_all_controls() { + friction_enabled_ = false; + + last_primary_friction_velocity_ = nan_; + primary_friction_velocity_decrease_integral_ = 0; + + friction_soft_start_stop_percentage_ = nan_; + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = nan_; + + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + } + + void update_friction_velocities() { + if (std::isnan(friction_soft_start_stop_percentage_)) { + friction_soft_start_stop_percentage_ = 0.0; + for (size_t i = 0; i < friction_count_; i++) + friction_soft_start_stop_percentage_ += + *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ /= static_cast(friction_count_); + } + friction_soft_start_stop_percentage_ += + friction_enabled_ ? friction_soft_start_stop_step_ : -friction_soft_start_stop_step_; + friction_soft_start_stop_percentage_ = + std::clamp(friction_soft_start_stop_percentage_, 0.0, 1.0); + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = + friction_soft_start_stop_percentage_ * target_friction_velocity(i); + } + + void update_friction_status() { + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + + if (!friction_enabled_) + return; + if (friction_soft_start_stop_percentage_ < 1.0) + return; + + if (detect_friction_faulty()) { + if (friction_faulty_count_ == 200) { + friction_enabled_ = false; + *friction_jammed_ = true; + } else { + friction_faulty_count_++; + *friction_ready_ = true; + } + return; + } + + *friction_ready_ = true; + *bullet_fired_ = detect_bullet_fire(); + } + + bool detect_friction_faulty() { + for (size_t i = 0; i < friction_count_; i++) { + if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) + return true; + } + return false; + } + + rmcs_utility::FpsCounter fps_counter_; + + bool detect_bullet_fire() { + bool fired = false; + + // TODO(steering-hero): This intentionally keeps the legacy merge behavior by monitoring + // friction_velocities_[2]. Historically, hero config ordering mapped [2,3] to the first + // stage used for fire detection. Replace this hard-coded index with an explicit first- + // stage mapping once the wheel ordering semantics are unified. + if (!std::isnan(last_primary_friction_velocity_)) { + double differential = *friction_velocities_[2] - last_primary_friction_velocity_; + if (differential < 0.1) + primary_friction_velocity_decrease_integral_ += differential; + else { + if (primary_friction_velocity_decrease_integral_ < -14.0 + && last_primary_friction_velocity_ < target_friction_velocity(2) - 25.0) + fired = true; + + primary_friction_velocity_decrease_integral_ = 0; + } + } + last_primary_friction_velocity_ = *friction_velocities_[2]; + + return fired; + } + + double target_friction_velocity(size_t i) const { + return active_profile_ == 0 ? friction_profile_0_[i] : friction_profile_1_[i]; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + rclcpp::Logger logger_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + size_t friction_count_; + + std::vector friction_profile_0_; + std::vector friction_profile_1_; + size_t active_profile_ = 0; + + std::unique_ptr[]> friction_velocities_; + + bool friction_enabled_ = false; + + double friction_soft_start_stop_step_; + double friction_soft_start_stop_percentage_ = nan_; + std::unique_ptr[]> friction_control_velocities_; + + OutputInterface friction_ready_; + + int friction_faulty_count_ = 0; + OutputInterface friction_jammed_; + + double last_primary_friction_velocity_ = nan_; + double primary_friction_velocity_decrease_integral_ = 0; + OutputInterface bullet_fired_; + OutputInterface friction_profile_1_active_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroFrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp new file mode 100644 index 00000000..dfd2df62 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp @@ -0,0 +1,63 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroHeatController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroHeatController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , heat_per_shot(get_parameter("heat_per_shot").as_int()) + , reserved_heat(get_parameter("reserved_heat").as_int()) { + + register_input("/referee/shooter/cooling", shooter_cooling_); + register_input("/referee/shooter/heat_limit", shooter_heat_limit_); + + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_output( + "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0); + register_output("/shoot/heat", shooting_heat_, 0.0); + } + + void update() override { + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + + if (*bullet_fired_) + shooter_heat_ += heat_per_shot; + + *control_bullet_allowance_ = std::max( + 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + *shooting_heat_ = static_cast(shooter_heat_); + } + +private: + InputInterface shooter_cooling_; + InputInterface shooter_heat_limit_; + + InputInterface bullet_fired_; + + const int64_t heat_per_shot; + const int64_t reserved_heat; + + int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; + + OutputInterface control_bullet_allowance_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroHeatController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp new file mode 100644 index 00000000..0b0c6375 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -0,0 +1,407 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include + +namespace rmcs_core::controller::shooting { + +/** + * @class PutterController + * @brief Putter mechanism controller + * + * Firing mechanism notes: + * Since the photoelectric sensor is placed at the chamber opening, testing showed that + * the double-middle action first triggers the putter to retract, and then stall detection + * confirms that the reset is complete. + * By default, a small holding force is applied so the putter does not slide down. The entire + * process uses angle-loop advancement, and grayscale detection determines whether to feed. + * Bullet firing is detected from two sources: the friction wheels and the putter stroke. + * The full scheme completed stress testing before the summer break. + */ +class PutterController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PutterController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + auto set_pid_parameter = [this](pid::PidCalculator& pid, const std::string& name) { + pid.kp = get_parameter(name + "_kp").as_double(); + pid.ki = get_parameter(name + "_ki").as_double(); + pid.kd = get_parameter(name + "_kd").as_double(); + get_parameter(name + "_integral_min", pid.integral_min); + get_parameter(name + "_integral_max", pid.integral_max); + get_parameter(name + "_output_min", pid.output_min); + get_parameter(name + "_output_max", pid.output_max); + }; + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_input("/gimbal/friction_ready", friction_ready_); + + register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_); + register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); + + register_input( + "/gimbal/control_bullet_allowance/limited_by_heat", + control_bullet_allowance_limited_by_heat_); + + register_input("/gimbal/photoelectric_sensor", photoelectric_sensor_status_); + register_input("/gimbal/grayscale_sensor", grayscale_sensor_status_); + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_input("/gimbal/putter/angle", putter_angle_); + register_input("/gimbal/putter/velocity", putter_velocity_); + + set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); + set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); + + putter_velocity_pid_.kp = 0.004; + putter_velocity_pid_.ki = 0.0001; + putter_velocity_pid_.kd = 0.001; + putter_velocity_pid_.integral_max = 0.03; + putter_velocity_pid_.integral_min = 0.; + + putter_return_angle_pid.kp = 0.0001; + // putter_return_angle_pid.ki = 0.000001; + putter_return_angle_pid.kd = 0.; + + register_output( + "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_); + register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_); + + register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); + + // auto_aim + register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + + register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + register_output("/gimbal/shooter/condiction", shoot_condiction_); + } + + ~PutterController() {} + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; + + using namespace rmcs_msgs; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + // Normal control flow after the putter has been initialized. + if (putter_is_initialized_) { + // Handling during bullet-feeder jam-protection cooldown. + if (bullet_feeder_reverse_end_ > 0) { + bullet_feeder_reverse_end_--; + + // Early cooldown stage: reverse the feeder to clear the jam. + if (bullet_feeder_reverse_end_ > 500) + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); + else { + // Late cooldown stage: stop control. + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = 0.0; + } + + } else { + // Normal operating mode: only fire when the friction wheels are ready. + if (*friction_ready_) { + // Detect fire triggers. + if (switch_right != Switch::DOWN) { + + const auto now = std::chrono::steady_clock::now(); + const bool left_click_edge = (!last_mouse_.left && mouse.left); + if (left_click_edge) { + if (now - last_click_time_ < std::chrono::milliseconds(500)) { + click_count_++; + } else { + click_count_ = 1; + } + last_click_time_ = now; + } + + const bool manual_trigger = + (!last_mouse_.left && mouse.left && !mouse.right) + || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE + && switch_left == rmcs_msgs::Switch::DOWN); + + const bool auto_fire_now = + (switch_right == Switch::UP || (mouse.right && mouse.left)) + && (*fire_control_); + + const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); + + const bool auto_trigger = + auto_fire_now + && (now - last_fire_time_ > std::chrono::milliseconds(1000)); + + if (manual_trigger || auto_trigger || auto_trigger_emergence) { + if (*control_bullet_allowance_limited_by_heat_ > 0 + && (shoot_stage_ == ShootStage::PRELOADED || first_shot_)) { + set_shooting(); + last_fire_time_ = now; + first_shot_ = false; + } + } + if (auto_trigger_emergence) { + click_count_ = 0; + } + } + + if (shoot_stage_ == ShootStage::PRELOADING) { + + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + low_latency_velocity_ - *bullet_feeder_velocity_); // Velocity loop. + + update_locked_detection(); + + // This includes the photoelectric-sensor logic: if triggered, switch to + // preloaded; otherwise reverse briefly and continue rotating. + } + + if (shoot_stage_ == ShootStage::SHOOTING) { + // Firing state: detect whether the bullet has been fired. + if (*bullet_fired_ + || *putter_angle_ - putter_start_point_ >= putter_stroke_) { + shot_fired_ = true; + } + + update_putter_jam_detection(); + + if (shot_fired_) { + // Bullet fired: return the putter. + const auto angle_err = putter_start_point_ - *putter_angle_; + if (angle_err > -0.1) { + *putter_control_torque_ = 0.; + set_preloading(); + shot_fired_ = false; + } else { + *putter_control_torque_ = + putter_return_velocity_pid_.update(-80. - *putter_velocity_); + putter_timeout_detection(); + } + } else { + // Bullet not fired yet: continue advancing. + *putter_control_torque_ = + putter_return_velocity_pid_.update(60. - *putter_velocity_); + } + } + } else { + // Friction wheels not ready: stop the bullet feeder. + *bullet_feeder_control_torque_ = 0.; + } + + // Non-firing state: apply a small holding force to the putter. + if (shoot_stage_ != ShootStage::SHOOTING) + *putter_control_torque_ = -0.02; + } + } else { + // Putter not initialized: perform the reset procedure. + *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); + update_putter_jam_detection(); + } + + // Save the current state for the next comparison. + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; + } + +private: + void reset_all_controls() { + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = nan_; + + shoot_stage_ = ShootStage::PRELOADED; + + putter_is_initialized_ = false; + putter_start_point_ = nan_; + putter_return_velocity_pid_.reset(); + putter_velocity_pid_.reset(); + putter_return_angle_pid.reset(); + *putter_control_torque_ = nan_; + + bullet_feeder_faulty_count_ = 0; + + *shoot_delay_ms_ = nan_; + } + + void set_preloading() { shoot_stage_ = ShootStage::PRELOADING; } + + void set_preloaded() { shoot_stage_ = ShootStage::PRELOADED; } + + void set_shooting() { shoot_stage_ = ShootStage::SHOOTING; } + + void update_locked_detection() { + // If feeder speed is near zero and the photoelectric sensor is triggered, + // treat it as locked and start reversing. + + if (*bullet_feeder_velocity_ < 0.5 && *bullet_feeder_control_torque_ > 0.1) { + locked_detect_count_++; + } else { + locked_detect_count_ = 0; + } + + if (locked_detect_count_ > 300) { + if (*photoelectric_sensor_status_) { + set_preloaded(); + } + // If the photoelectric sensor was not triggered, treat it as a simple jam, + // reverse briefly, then continue until stall. + locked_detect_count_ = 0; + enter_jam_protection(); + } + } + + void update_putter_jam_detection() { + if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) + || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) + || std::isnan(*putter_control_torque_)) { + putter_faulty_count_ = 0; + return; + } + + // Accumulate a fault count when the torque is abnormal. + if (putter_faulty_count_ < 500) + ++putter_faulty_count_; + else { + putter_faulty_count_ = 0; + if (shoot_stage_ != ShootStage::SHOOTING) { + // Stall detected outside the firing state: the putter is in position, + // so mark it initialized. + putter_is_initialized_ = true; + putter_start_point_ = *putter_angle_; + } else { + // Stall detected during firing: treat the bullet as fired. + shot_fired_ = true; + } + } + } + + void putter_timeout_detection() { + // If the putter stays in the firing state too long without extending, + // treat it as finished and move to the next state. + if (shoot_stage_ == ShootStage::SHOOTING) { + if (shot_fired_) { + if (putter_timeout_count_ < 1600) + ++putter_timeout_count_; + else { + putter_timeout_count_ = 0; + set_preloading(); + shot_fired_ = false; + } + } + } + } + + void enter_jam_protection() { + // Set the target angle to 60 degrees behind the current angle. + locked_detect_count_ = 0; + bullet_feeder_faulty_count_ = 0; + bullet_feeder_reverse_end_ = 800; + bullet_feeder_velocity_pid_.reset(); + } + + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. + static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. + + static constexpr double putter_stroke_ = 11.5; ///< Putter stroke length. + + static constexpr double max_bullet_feeder_control_torque_ = 0.1; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double low_latency_velocity_ = 5.0; + + InputInterface photoelectric_sensor_status_; + InputInterface grayscale_sensor_status_; + InputInterface bullet_fired_; + bool shot_fired_{false}; + bool first_shot_{true}; + + InputInterface friction_ready_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_velocity_; + + InputInterface control_bullet_allowance_limited_by_heat_; + + bool putter_is_initialized_ = false; + int putter_faulty_count_ = 0; + int putter_timeout_count_ = 0; + double putter_start_point_ = nan_; + pid::PidCalculator putter_return_velocity_pid_; + InputInterface putter_velocity_; + + pid::PidCalculator putter_velocity_pid_; + + enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; + ShootStage shoot_stage_ = ShootStage::PRELOADING; + + pid::PidCalculator bullet_feeder_velocity_pid_; + + OutputInterface bullet_feeder_control_torque_; + + InputInterface putter_angle_; + pid::PidCalculator putter_return_angle_pid; + OutputInterface putter_control_torque_; + + int bullet_feeder_faulty_count_ = 0; + + OutputInterface shoot_delay_ms_; + + InputInterface fire_control_; + std::chrono::steady_clock::time_point last_fire_time_{}; + std::chrono::steady_clock::time_point last_click_time_{}; + int click_count_ = 0; + + int locked_detect_count_ = 0; + int bullet_feeder_reverse_end_ = 0; + + InputInterface bullet_feeder_torque; + InputInterface putter_torque; + + OutputInterface shoot_mode_; + OutputInterface shoot_condiction_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 5500b15c..6cefe275 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -1,7 +1,10 @@ +#include +#include #include #include #include #include +#include namespace rmcs_core::controller::shooting { @@ -18,40 +21,26 @@ class ShootingRecorder log_mode_ = static_cast(get_parameter("log_mode").as_int()); + aim_velocity = get_parameter("aim_velocity").as_double(); + register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); - - if (friction_wheel_count_ == 2) { - const auto topic = std::array{ - "/gimbal/left_friction/control_velocity", - "/gimbal/right_friction/control_velocity", - }; - for (int i = 0; i < 2; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } else if (friction_wheel_count_ == 4) { - const auto topic = std::array{ - "/gimbal/first_left_friction/control_velocity", - "/gimbal/first_right_friction/control_velocity", - "/gimbal/second_left_friction/control_velocity", - "/gimbal/second_right_friction/control_velocity", - }; - for (int i = 0; i < 4; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); + + std::ofstream out_file("shoot_recorder"); + RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str()); } ~ShootingRecorder() { log_stream_.close(); } void update() override { - + // Evaluate friction-wheel quality. switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action @@ -64,38 +53,52 @@ class ShootingRecorder return; break; } + v = *shoot_timestamp_; + + static constexpr size_t max_velocities_size = 1000; + + velocities.push_back(*initial_speed_); + if (velocities.size() > max_velocities_size) { + velocities.erase(velocities.begin()); + } - auto log_text = std::string{}; + analysis3(); + + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); - if (friction_wheel_count_ == 2) { - log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); - } else if (friction_wheel_count_ == 4) { + if (friction_wheel_count_ == 4) { log_text = fmt::format( - "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], - *fractional_temperature_); + "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, + (int)velocities.size(), // + velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, + velocity_min); } log_stream_ << log_text << std::endl; - RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + RCLCPP_INFO( + get_logger(), "%s", + log_text.c_str()); // Record each shot's initial speed and statistics. + + log_velocity = fmt::format("{:.3f}", *initial_speed_); + std::ofstream out_file("shoot_recorder", std::ios::app); + if (out_file.is_open()) { + out_file << log_velocity << std::endl; + out_file.close(); + } last_shoot_timestamp_ = *shoot_timestamp_; } private: /// @brief Component interface + std::array, 2> friction_velocities_; + InputInterface initial_speed_; InputInterface shoot_timestamp_; - InputInterface fractional_temperature_; - std::size_t friction_wheel_count_ = 2; - std::array, 4> friction_wheels_velocity_; + std::array, 2> friction_wheels_velocity_; /// @brief For log enum class LogMode { TRIGGER = 1, TIMING = 2 }; @@ -103,29 +106,153 @@ class ShootingRecorder double last_shoot_timestamp_ = 0; std::ofstream log_stream_; + std::string log_velocity; std::size_t log_count_ = 0; + std::vector velocities; + + double velocity_; + + double excellence_rate_; + double pass_rate_; + + double range_; + double range2_; + + double velocity_min; + double velocity_max; + + double v; + double aim_velocity; + private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); return result; } + void analysis1() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + + range_ = velocities.back() - velocities.front(); + + if (velocities.size() >= 3) { + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } else { + range2_ = range_; + } + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis2() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + + sort(velocities.begin(), velocities.end()); + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int n_adjust = std::max(1, int((int)velocities.size() * 0.05)); + + for (int i = 0; i < n_adjust; i++) { + sum -= velocities[i]; + sum -= velocities[velocities.size() - 1 - i]; + } + + velocity_ = sum / double(velocities.size() - 2 * n_adjust); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis3() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + int excellence_count = 0; + int pass_count = 0; + + for (const auto& v : velocities) { + if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { + excellence_count += 1; + } + if (v >= aim_velocity - 0.1 && v <= aim_velocity + 0.1) { + pass_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } + + static double GetTime() { + using namespace std::chrono; + static auto start = high_resolution_clock::now(); + auto now = high_resolution_clock::now(); + duration elapsed = now - start; + return elapsed.count(); + } }; } // namespace rmcs_core::controller::shooting #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp index fcb9a32a..0f6d9dd2 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp @@ -11,7 +11,9 @@ namespace rmcs_core::filter { class AlphaBetaAngleFilter { public: AlphaBetaAngleFilter(double dt, double alpha, double beta) - : dt_(dt), alpha_(alpha), beta_(beta) {} + : dt_(dt) + , alpha_(alpha) + , beta_(beta) {} void reset() { initialized_ = false; @@ -51,9 +53,7 @@ class AlphaBetaAngleFilter { bool initialized() const { return initialized_; } private: - static double wrap_angle_(double x) { - return std::remainder(x, 2.0 * std::numbers::pi); - } + static double wrap_angle_(double x) { return std::remainder(x, 2.0 * std::numbers::pi); } double dt_; double alpha_; diff --git a/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp new file mode 100644 index 00000000..e7e98cf0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp @@ -0,0 +1,428 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::filter { + +class ImuEkf { +public: + using Vec3 = Eigen::Vector3d; + using Vec4 = Eigen::Vector4d; + using Mat3 = Eigen::Matrix3d; + using Mat4 = Eigen::Matrix4d; + using Mat43 = Eigen::Matrix; + using Mat34 = Eigen::Matrix; + + struct Config { + Mat3 process_noise = 1.22e-3 * Mat3::Identity(); + Mat3 measurement_noise = 50.0 * Mat3::Identity(); + Mat3 gate_noise = 0.02 * Mat3::Identity(); + + Mat4 initial_covariance = 0.15 * Mat4::Identity(); + }; + + class AccelCorrection { + public: + [[nodiscard]] double chi_square() const noexcept { return chi_square_; } + + private: + friend class ImuEkf; + + AccelCorrection( + const std::uint64_t revision, Vec3 innovation, Mat34 jacobian_h_x, + Eigen::LDLT innovation_covariance_ldlt, const double chi_square) + : revision_(revision) + , innovation_(std::move(innovation)) + , jacobian_h_x_(std::move(jacobian_h_x)) + , innovation_covariance_ldlt_(std::move(innovation_covariance_ldlt)) + , chi_square_(chi_square) {} + + std::uint64_t revision_ = 0; + Vec3 innovation_ = Vec3::Zero(); + Mat34 jacobian_h_x_ = Mat34::Zero(); + Eigen::LDLT innovation_covariance_ldlt_; + double chi_square_ = std::numeric_limits::quiet_NaN(); + }; + + ImuEkf() + : ImuEkf(Config{}) {} + + explicit ImuEkf(Config config) + : config_(std::move(config)) { + reset(); + } + + [[nodiscard]] const Config& config() const noexcept { return config_; } + + void set_config(const Config& config) { + config_ = config; + reset(); + } + + void reset() { + state_ = identity_quaternion(); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + } + + [[nodiscard]] bool reset_from_accel(const Vec3& accel_g, const double initial_yaw = 0.0) { + if (!accel_g.allFinite() || !std::isfinite(initial_yaw)) { + return false; + } + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) { + return false; + } + + const double gx = (*normalized_accel)(0); + const double gy = (*normalized_accel)(1); + const double gz = std::clamp((*normalized_accel)(2), -1.0, 1.0); + + double q0_val, q1_val, q2_val, q3_val; + if (gz >= 0.0) { + const double s = std::sqrt(0.5 * (1.0 + gz)); + const double inv = 0.5 / s; + q0_val = s; + q1_val = gy * inv; + q2_val = -gx * inv; + q3_val = 0.0; + } else { + const double s = std::sqrt(0.5 * (1.0 - gz)); + const double inv = 0.5 / s; + q0_val = gy * inv; + q1_val = s; + q2_val = 0.0; + q3_val = gx * inv; + } + + const double cy = std::cos(initial_yaw * 0.5); + const double sy = std::sin(initial_yaw * 0.5); + + state_(0) = cy * q0_val - sy * q3_val; + state_(1) = cy * q1_val - sy * q2_val; + state_(2) = cy * q2_val + sy * q1_val; + state_(3) = cy * q3_val + sy * q0_val; + + state_ = normalize_quaternion(state_); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + return true; + } + + void inflate_attitude_uncertainty_to_initial() { + covariance_ = covariance_with_psd_floor( + covariance_, projected_covariance(state_, config_.initial_covariance)); + ++revision_; + } + + [[nodiscard]] bool predict(const Vec3& gyro_rad_per_sec, const double dt_seconds) { + if (!gyro_rad_per_sec.allFinite()) { + return false; + } + + if (!std::isfinite(dt_seconds) || dt_seconds < 0.0) { + return false; + } + + const PropagationResult propagation = + propagate_quaternion(state_, gyro_rad_per_sec, dt_seconds); + if (!propagation.quaternion.allFinite() || !propagation.orthogonalization.allFinite()) { + return false; + } + const Vec4 prior_state = propagation.quaternion; + const Mat4& orthogonalization = propagation.orthogonalization; + + const Mat4 jacobian_f_x = state_jacobian(gyro_rad_per_sec, dt_seconds, orthogonalization); + const Mat43 jacobian_f_w = process_noise_jacobian(state_, orthogonalization); + Mat4 prior_cov = symmetrized_mat4( + jacobian_f_x * covariance_ * jacobian_f_x.transpose() + + dt_seconds * jacobian_f_w * config_.process_noise * jacobian_f_w.transpose()); + if (!is_positive_semidefinite(prior_cov)) { + return false; + } + + state_ = prior_state; + covariance_ = prior_cov; + ++revision_; + return true; + } + + [[nodiscard]] std::optional prepare_correction(const Vec3& accel_g) const { + if (!accel_g.allFinite()) + return std::nullopt; + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) + return std::nullopt; + + const Vec3 predicted_accel = measurement_model(state_); + const Vec3 gate_innovation = accel_g - predicted_accel; + const Vec3 update_innovation = *normalized_accel - predicted_accel; + const Mat34 jacobian_h_x = measurement_jacobian(state_); + const Mat3 projected_covariance = jacobian_h_x * covariance_ * jacobian_h_x.transpose(); + const Mat3 gate_covariance = symmetrized_mat3(projected_covariance + config_.gate_noise); + + if (!is_positive_definite(gate_covariance)) { + return std::nullopt; + } + + const auto gate_ldt = gate_covariance.ldlt(); + if (gate_ldt.info() != Eigen::Success || !gate_ldt.isPositive()) { + return std::nullopt; + } + const Vec3 innovation_weighted = gate_ldt.solve(gate_innovation); + if (!innovation_weighted.allFinite()) { + return std::nullopt; + } + const double chi_square = gate_innovation.dot(innovation_weighted); + if (!std::isfinite(chi_square)) { + return std::nullopt; + } + + const Mat3 update_covariance = + symmetrized_mat3(projected_covariance + config_.measurement_noise); + if (!is_positive_definite(update_covariance)) { + return std::nullopt; + } + + const auto update_ldt = update_covariance.ldlt(); + if (update_ldt.info() != Eigen::Success || !update_ldt.isPositive()) { + return std::nullopt; + } + + return AccelCorrection(revision_, update_innovation, jacobian_h_x, update_ldt, chi_square); + } + + bool correct(const AccelCorrection& update) { + if (update.revision_ != revision_) { + return false; + } + const Vec4 state_prior = state_; + const Mat34 k_transpose = + update.innovation_covariance_ldlt_.solve(update.jacobian_h_x_ * covariance_); + if (!k_transpose.allFinite()) { + return false; + } + const Eigen::Matrix kalman_gain = k_transpose.transpose(); + Mat4 state_update_mask = Mat4::Identity(); + state_update_mask(3, 3) = 0.0; + const Eigen::Matrix effective_gain = state_update_mask * kalman_gain; + + const Vec4 quaternion_raw = state_prior + effective_gain * update.innovation_; + if (!quaternion_raw.allFinite() || quaternion_raw.squaredNorm() <= kEpsilon) { + return false; + } + + const Mat4 matrix_tmp = Mat4::Identity() - effective_gain * update.jacobian_h_x_; + Mat4 corrected_covariance = + matrix_tmp * covariance_ * matrix_tmp.transpose() + + effective_gain * config_.measurement_noise * effective_gain.transpose(); + + const Mat4 j_norm = orthogonalization_jacobian(quaternion_raw); + corrected_covariance = symmetrized_mat4(j_norm * corrected_covariance * j_norm.transpose()); + + if (!is_positive_semidefinite(corrected_covariance)) { + return false; + } + state_ = normalize_quaternion(quaternion_raw); + covariance_ = corrected_covariance; + ++revision_; + return true; + } + + bool correct(const Vec3& accel_mps2) { + const std::optional update = prepare_correction(accel_mps2); + return update.has_value() && correct(*update); + } + + [[nodiscard]] Eigen::Quaterniond quaternion() const noexcept { + return {state_[0], state_[1], state_[2], state_[3]}; + } + +private: + struct PropagationResult { + Vec4 quaternion = identity_quaternion(); + Mat4 orthogonalization = Mat4::Identity(); + }; + + [[nodiscard]] static Vec4 identity_quaternion() { + Vec4 q; + q << 1.0, 0.0, 0.0, 0.0; + return q; + } + + [[nodiscard]] static std::optional normalize_vector(const Vec3& vector) { + const double squared_norm = vector.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return std::nullopt; + } + + return vector / std::sqrt(squared_norm); + } + + [[nodiscard]] static Vec4 normalize_quaternion(const Vec4& quaternion) { + const double squared_norm = quaternion.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return identity_quaternion(); + } + + return quaternion / std::sqrt(squared_norm); + } + + [[nodiscard]] static Mat3 symmetrized_mat3(const Mat3& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static Mat4 symmetrized_mat4(const Mat4& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static bool is_positive_definite(const Mat3& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat3(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() > tolerance; + } + + [[nodiscard]] static bool is_positive_semidefinite(const Mat4& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() >= -tolerance; + } + + [[nodiscard]] static Mat4 positive_semidefinite_part(const Mat4& matrix) { + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return Mat4::Zero(); + } + + return symmetrized_mat4( + solver.eigenvectors() * solver.eigenvalues().cwiseMax(0.0).asDiagonal() + * solver.eigenvectors().transpose()); + } + + [[nodiscard]] static Mat4 + covariance_with_psd_floor(const Mat4& covariance, const Mat4& covariance_floor) { + Mat4 floor = symmetrized_mat4(covariance_floor); + Mat4 result = floor + positive_semidefinite_part(symmetrized_mat4(covariance) - floor); + result = symmetrized_mat4(result); + if (is_positive_semidefinite(result)) { + return result; + } + + return floor; + } + + [[nodiscard]] static Mat4 + projected_covariance(const Vec4& quaternion, const Mat4& raw_covariance) { + const Mat4 j_norm = orthogonalization_jacobian(quaternion); + Mat4 covariance = symmetrized_mat4(j_norm * raw_covariance * j_norm.transpose()); + if (is_positive_semidefinite(covariance)) { + return covariance; + } + + return symmetrized_mat4(j_norm * Mat4::Identity() * j_norm.transpose()); + } + + [[nodiscard]] static Mat4 omega_matrix(const Vec3& gyro_rad_per_sec) { + const double gx = gyro_rad_per_sec(0); + const double gy = gyro_rad_per_sec(1); + const double gz = gyro_rad_per_sec(2); + Mat4 matrix; + // clang-format off + matrix << 0, -gx, -gy, -gz, + gx, 0, gz, -gy, + gy, -gz, 0, gx, + gz, gy, -gx, 0; + // clang-format on + return matrix; + } + + [[nodiscard]] static Mat4 orthogonalization_jacobian(const Vec4& quaternion) { + const double norm = quaternion.norm(); + const double inv_norm = (std::isfinite(norm) && norm > kEpsilon) ? 1.0 / norm : 1.0; + return inv_norm + * (Mat4::Identity() - inv_norm * inv_norm * quaternion * quaternion.transpose()); + } + + [[nodiscard]] static PropagationResult + propagate_quaternion(const Vec4& state, const Vec3& gyro_rad_per_sec, const double dt) { + const Mat4 omega = omega_matrix(gyro_rad_per_sec); + const Vec4 quaternion_tmp = state + 0.5 * dt * omega * state; + + PropagationResult result; + result.orthogonalization = orthogonalization_jacobian(quaternion_tmp); + result.quaternion = normalize_quaternion(quaternion_tmp); + return result; + } + + [[nodiscard]] static Mat4 state_jacobian( + const Vec3& gyro_rad_per_sec, const double dt, const Mat4& orthogonalization) { + return orthogonalization * (Mat4::Identity() + 0.5 * dt * omega_matrix(gyro_rad_per_sec)); + } + + [[nodiscard]] static Mat43 + process_noise_jacobian(const Vec4& state, const Mat4& orthogonalization) { + Mat43 matrix_q; + // clang-format off + matrix_q << -state(1), -state(2), -state(3), + state(0), -state(3), state(2), + state(3), state(0), -state(1), + -state(2), state(1), state(0); + // clang-format on + return orthogonalization * (0.5 * matrix_q); + } + + [[nodiscard]] static Vec3 measurement_model(const Vec4& state) { + Vec3 result; + result(0) = 2.0 * (state(1) * state(3) - state(0) * state(2)); + result(1) = 2.0 * (state(2) * state(3) + state(0) * state(1)); + result(2) = + state(0) * state(0) - state(1) * state(1) - state(2) * state(2) + state(3) * state(3); + return result; + } + + [[nodiscard]] static Mat34 measurement_jacobian(const Vec4& state) { + Mat34 result; + // clang-format off + result << -2.0 * state(2), 2.0 * state(3), -2.0 * state(0), 2.0 * state(1), + 2.0 * state(1), 2.0 * state(0), 2.0 * state(3), 2.0 * state(2), + 2.0 * state(0), -2.0 * state(1), -2.0 * state(2), 2.0 * state(3); + // clang-format on + return result; + } + + static constexpr double kEpsilon = 1e-12; + + Config config_; + Vec4 state_ = identity_quaternion(); + Mat4 covariance_ = Mat4::Identity(); + std::uint64_t revision_ = 0; +}; + +} // namespace rmcs_core::filter diff --git a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp index 126eb4f4..2d579f0d 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp @@ -55,7 +55,7 @@ requires(variable_number > 0) class LowPassFilter { void set_cutoff(double cutoff_freq, double sampling_freq) { double dt = 1.0 / sampling_freq; double rc = 1.0 / (2 * std::numbers::pi * cutoff_freq); - alpha_ = dt / (dt + rc); + alpha_ = dt / (dt + rc); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp index 613c6f61..23422445 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp @@ -45,7 +45,7 @@ class Benewake { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - *distance_ = package.calculate_distance(); + *distance_ = package.calculate_distance(); signal_strength_ = package.calculate_signal_strength(); } 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 96d84a2d..31a44cbe 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp @@ -150,12 +150,8 @@ class Bmi088 { 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} - }; + 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; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp new file mode 100644 index 00000000..83c1b2e3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "filter/imu_ekf.hpp" + +namespace rmcs_core::hardware::device { + +class Bmi088Ekf { +public: + struct Config { + Eigen::Matrix3d body_to_sensor = Eigen::Matrix3d::Identity(); + }; + + using TimePoint = rmcs_msgs::BoardClock::time_point; + using Snapshot = rmcs_msgs::ImuSnapshot; + + explicit Bmi088Ekf(Config config) + : config_(std::move(config)) {} + + Bmi088Ekf() + : Bmi088Ekf(Config{}) {} + + void push_accelerometer_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d accel_g = + config_.body_to_sensor.transpose() * convert_accelerometer(x, y, z); + + if (!initialized_) { + if (ekf_.reset_from_accel(accel_g)) { + ekf_state_time_ = sample_time; + latest_snapshot_ = { + ekf_.quaternion(), + Eigen::Vector3d::Zero(), + ekf_state_time_, + }; + const auto guard = std::scoped_lock{mutex_}; + initialized_ = true; + } + return; + } + + if (sample_time < ekf_state_time_) + return; + + if (pending_accel_sample_ && sample_time < pending_accel_sample_->sample_time) + return; + + pending_accel_sample_ = {accel_g, sample_time}; + } + + std::optional try_update_with_gyroscope_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d gyro_rad_per_sec = + config_.body_to_sensor.transpose() * convert_gyroscope(x, y, z); + + if (!initialized_) + return std::nullopt; + + if (sample_time < ekf_state_time_) + return std::nullopt; + + if (is_gyro_saturated(gyro_rad_per_sec)) + ekf_.inflate_attitude_uncertainty_to_initial(); + + while (pending_accel_sample_) { + const auto& accel_sample_time = pending_accel_sample_->sample_time; + if (accel_sample_time < ekf_state_time_ || accel_sample_time > sample_time) + break; + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{accel_sample_time - ekf_state_time_}.count())) + break; + ekf_state_time_ = accel_sample_time; + + const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); + if (!correction || correction->chi_square() >= 3.0) + break; + if (!ekf_.correct(*correction)) + break; + + pending_accel_sample_ = std::nullopt; + break; + } + + // Guard against stale IMU frames that librmcs may deliver right after reconnect before the + // device-side buffer is drained. Integrating a frame with a large timestamp jump can inject + // a huge bogus gyro delta, so drop it instead of advancing the filter. + // TODO: Remove this once librmcs guarantees buffered historical IMU frames are flushed on + // connection. + if (std::chrono::duration{sample_time - ekf_state_time_}.count() > 1 / 1000.0) { + ekf_state_time_ = sample_time; + return std::nullopt; + } + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{sample_time - ekf_state_time_}.count())) + return std::nullopt; + ekf_state_time_ = sample_time; + + auto snapshot = Snapshot{ + ekf_.quaternion(), + gyro_rad_per_sec, + ekf_state_time_, + }; + { + const auto guard = std::scoped_lock{mutex_}; + latest_snapshot_ = snapshot; + } + return snapshot; + } + + [[nodiscard]] bool initialized() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + return initialized_; + } + + [[nodiscard]] std::optional snapshot() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + if (!initialized_) + return std::nullopt; + return latest_snapshot_; + } + +private: + [[nodiscard]] static Eigen::Vector3d + convert_accelerometer(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 6.0; + } + + [[nodiscard]] static Eigen::Vector3d + convert_gyroscope(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 2000.0 / 180.0 * std::numbers::pi; + } + + [[nodiscard]] static bool is_gyro_saturated(const Eigen::Vector3d& gyro_rad_per_sec) noexcept { + return gyro_rad_per_sec.cwiseAbs().maxCoeff() >= 0.98 * 2000.0 / 180.0 * std::numbers::pi; + } + + const Config config_; + + mutable std::mutex mutex_; + bool initialized_ = false; + + filter::ImuEkf ekf_; + TimePoint ekf_state_time_; + + struct AccelSample { + Eigen::Vector3d accel_g; + TimePoint sample_time; + }; + std::optional pending_accel_sample_; + + Snapshot latest_snapshot_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp new file mode 100644 index 00000000..1706ea2a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +#include + +namespace rmcs_core::hardware::device { + +class BoardClockLifter { +public: + using time_point = rmcs_msgs::BoardClock::time_point; + + time_point advance_timebase(std::uint32_t raw_timestamp_quarter_us) { + if (!has_latest_timebase_) { + has_latest_timebase_ = true; + last_timebase_raw_ = raw_timestamp_quarter_us; + latest_timebase_timestamp_ = raw_timestamp_quarter_us; + } + + latest_timebase_timestamp_ += + static_cast(raw_timestamp_quarter_us - last_timebase_raw_); + last_timebase_raw_ = raw_timestamp_quarter_us; + + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto timebase() const -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto lift_timestamp(std::uint32_t timestamp_quarter_us) const + -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + + const auto latest_timestamp_low32 = static_cast(latest_timebase_timestamp_); + const auto signed_offset = + static_cast(timestamp_quarter_us - latest_timestamp_low32); + const auto lifted_timestamp = + latest_timebase_timestamp_ + static_cast(signed_offset); + return time_point{rmcs_msgs::BoardClock::duration{lifted_timestamp}}; + } + +private: + bool has_latest_timebase_ = false; + std::uint32_t last_timebase_raw_ = 0; + std::int64_t latest_timebase_timestamp_ = 0; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp index df527150..e5b8d7b3 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp @@ -34,9 +34,9 @@ class Gy614 { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - emissivity_ = package.calculate_emissivity(); - *target_temperature_ = package.calculate_target_temperature(); - environment_temperature_ = package.calculate_environment_temperature(); + emissivity_ = package.calculate_emissivity(); + *target_temperature_ = package.calculate_target_temperature(); + environment_temperature_ = package.calculate_environment_temperature(); body_temperature_from_forhead_ = package.calculate_body_temperature_from_forhead(); } @@ -74,7 +74,7 @@ class Gy614 { double emissivity_ = 0; Component::OutputInterface target_temperature_; - double environment_temperature_ = 0; + double environment_temperature_ = 0; double body_temperature_from_forhead_ = 0; }; 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 7ab8a38f..1bc0ef8a 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 @@ -43,6 +43,7 @@ class LkMotor { rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) { status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/raw_angle", raw_angle_output_, 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); @@ -75,7 +76,7 @@ class LkMotor { switch (config.motor_type) { case Type::kMG5010Ei10: raw_angle_modulus_ = 1 << 16; - torque_constant = 0.90909; + torque_constant = 0.1; reduction_ratio = 10.0; // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. @@ -93,7 +94,7 @@ class LkMotor { break; case Type::kMG6012Ei8: raw_angle_modulus_ = 1 << 16; - torque_constant = 1.09; + torque_constant = 1.09 / 8.0; reduction_ratio = 8.0; max_torque_ = 16.0; break; @@ -193,6 +194,7 @@ class LkMotor { torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); *angle_output_ = angle(); + *raw_angle_output_ = last_raw_angle(); *velocity_output_ = velocity(); *torque_output_ = torque(); *temperature_output_ = temperature(); @@ -521,6 +523,7 @@ class LkMotor { double temperature_; rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface raw_angle_output_; rmcs_executor::Component::OutputInterface velocity_output_; rmcs_executor::Component::OutputInterface torque_output_; rmcs_executor::Component::OutputInterface temperature_output_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 3c01ac5a..69ddef42 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -2,12 +2,12 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -16,18 +16,18 @@ #include #include #include +#include #include #include #include -#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.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/remote_control.hpp" #include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -40,7 +40,7 @@ class OmniInfantry : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::RmcsBoardLite(get_parameter("board_serial").as_string(), {true}) + , librmcs::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -55,8 +55,7 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , remote_control_(*this, dr16_, vt13_) - , bmi088_(1000, 0.2, 0.0) { + , dr16_{*this} { for (auto& motor : chassis_wheel_motors_) motor.configure( @@ -87,19 +86,20 @@ class OmniInfantry register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); 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); - }); + start_transmit().gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx, + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -145,8 +145,6 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); - vt13_.update_status(); - remote_control_.update(); supercap_.update_status(); } @@ -221,14 +219,15 @@ class OmniInfantry } void update_imu() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + const auto snapshot = bmi088_.snapshot(); + if (!snapshot) + return; + tf_->set_transform( - gimbal_imu_pose.conjugate()); + snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); } void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { @@ -280,8 +279,19 @@ class OmniInfantry } } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio != librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); } void uart1_receive_callback(const librmcs::data::UartDataView& data) override { @@ -296,11 +306,21 @@ class OmniInfantry } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } private: @@ -331,12 +351,13 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; - device::Vt13 vt13_; - device::RemoteControl remote_control_; - device::Bmi088 bmi088_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; OutputInterface tf_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp new file mode 100644 index 00000000..40150e60 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -0,0 +1,865 @@ +#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/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + +class SteeringHeroLittle + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SteeringHeroLittle() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component( + get_component_name() + "_command", *this)) { + + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + + ~SteeringHeroLittle() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + + tf_->set_state( + bottom_board_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New player viewer offset: %ld", + top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + } + + class SteeringHeroLittleCommand : public rmcs_executor::Component { + public: + explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) + : hero_(hero) {} + + void update() override { hero_.command_update(); } + + SteeringHeroLittle& hero_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , tf_(steering_hero.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") + , gimbal_friction_wheels_( + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") + , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") + , gimbal_player_viewer_motor_( + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .enable_multi_turn_angle() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. + + return std::make_tuple(-y, x, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + tf_->set_state( + gimbal_pitch_motor_.angle()); + + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + gimbal_player_viewer_motor_.update_status(); + tf_->set_state( + gimbal_player_viewer_motor_.angle()); + + gimbal_scope_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + } + + void command_update() { + auto builder = start_transmit(); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x143, + .can_data = + gimbal_player_viewer_motor_ + .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + private: + void can0_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; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + 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; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_motor_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x143) { + gimbal_player_viewer_motor_.store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 2) { + photoelectric_sensor_status_atomic.store(data.high); + } else if (gpio.channel_index == 3) { + grayscale_sensor_status_atomic.store(!data.high); + } + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[4]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit BottomBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , imu_(1000, 0.2, 0.0) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + // + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + steering_hero.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + steering_hero.register_output( + "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); + steering_hero.register_output( + "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + + gimbal_bottom_yaw_motor_.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void can0_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; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + + device::Bmi088 imu_; + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::LkMotor gimbal_bottom_yaw_motor_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + + OutputInterface referee_serial_; + OutputInterface powermeter_control_enabled_; + OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 0f41b813..0bac8649 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -1,12 +1,16 @@ #include #include #include +#include #include #include #include #include +#include +#include #include +#include #include #include "referee/app/ui/shape/shape.hpp" @@ -21,20 +25,54 @@ class Hero , public rclcpp::Node { public: Hero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , status_ring_(26.5, 26.5, 600, 40) - , rangefinder_() - , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) - , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + // , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , chassis_left_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , chassis_right_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , yaw_angle_number_(Shape::Color::YELLOW, 20, 5, x_center + 270, y_center + 95, 0.0, false) + , pitch_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 35, 0.0, false) + , bottom_yaw_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) + // , bullet_allowance_label_( + // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) + // , bullet_allowance_number_( + // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) + , bullet_allowance_number_( + Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) + , friction_profile_number_( + Shape::Color::GREEN, friction_profile_number_font_size, 5, 0, 0, 12, false) + , friction_profile_indicator_{Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, true)} + , center_green_line_( + Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, + y_center - green_line_offset_y, x_center + green_line_half_length, + y_center - green_line_offset_y, true) + , tracking_pink_line_( + Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, + y_center + pink_line_offset_y, x_center + pink_line_half_length, + y_center + pink_line_offset_y, false) { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); + register_input("/gimbal/mode", gimbal_mode_); + register_input("/remote/keyboard", keyboard_); register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/control_angle", chassis_control_angle_); + register_input("/chassis/climber/left_front_motor/velocity", left_track_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_track_velocity_); + register_input("/chassis/supercap/voltage", supercap_voltage_); // register_input("/chassis/supercap/control_enable", supercap_control_enabled_); @@ -42,56 +80,192 @@ class Hero register_input("/chassis/power", chassis_power_); register_input("/chassis/control_power_limit", chassis_control_power_limit_); register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); - - register_input("/referee/shooter/42mm_bullet_allowance", robot_bullet_allowance_); + register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); + register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_player_viewer_raw_angle_); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); + // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + + register_input("/gimbal/shooter/condiction", shoot_condiction_); register_input("/gimbal/shooter/mode", shoot_mode_); - register_input("/gimbal/scope/active", is_scope_active_); + + // register_input("/gimbal/scope/active", is_scope_active_); register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); + + // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); + // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); - update_sniper_ui(); - - if (*is_scope_active_) { - set_normal_ui_visible(false); - rangefinder_.set_visible(true); - } else { - set_normal_ui_visible(true); - rangefinder_.set_visible(false); - } + // update_bullet_allowance(); + // update_sniper_ui(); + // update_state_word(); + + // if (*is_scope_active_) { + // set_normal_ui_visible(false); + // rangefinder_.set_visible(true); + // } else { + set_normal_ui_visible(true); + // rangefinder_.set_visible(false); + // } } private: + static uint16_t count_digits(int32_t value) { + uint16_t digits = value <= 0 ? 1 : 0; + while (value != 0) { + value /= 10; + ++digits; + } + return digits; + } void set_normal_ui_visible(bool value) { status_ring_.set_visible(value); - chassis_direction_indicator_.set_visible(value); - chassis_control_direction_indicator_.set_visible(value); + // chassis_direction_indicator_.set_visible(value); + chassis_left_wheel_indicator_.set_visible(value); + chassis_right_wheel_indicator_.set_visible(value); + // chassis_control_direction_indicator_.set_visible(value); + yaw_angle_number_.set_visible(value); + pitch_angle_number_.set_visible(value); + bottom_yaw_angle_number_.set_visible(value); + // bullet_allowance_label_.set_visible(value); + bullet_allowance_number_.set_visible(value); + friction_profile_number_.set_visible(value); + center_green_line_.set_visible(value); + tracking_pink_line_.set_visible(value); + + const bool show_friction_profile_box = + value && friction_profile_1_active_.ready() && *friction_profile_1_active_; + for (auto& line : friction_profile_indicator_) + line.set_visible(show_friction_profile_box); + if (!value) + chassis_control_direction_indicator_.set_visible(false); } void update_normal_ui() { update_chassis_direction_indicator(); - - status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + yaw_angle_number_.set_value(static_cast(*gimbal_player_viewer_raw_angle_)); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + update_pitch_raw_angle_color(); + bottom_yaw_angle_number_.set_value(static_cast(*bottom_yaw_raw_angle_)); + update_bottom_yaw_tracking_lines(); + const int32_t bullet_allowance = + static_cast(std::max(0, *robot_bullet_allowance_)); + bullet_allowance_number_.set_value(bullet_allowance); + + const uint16_t yaw_right = + yaw_raw_angle_x + + count_digits(static_cast(*gimbal_player_viewer_raw_angle_)) + * raw_angle_font_size; + const uint16_t pitch_right = + pitch_raw_angle_x + + count_digits(static_cast(*gimbal_pitch_raw_angle_)) * raw_angle_font_size; + const uint16_t box_left = std::max(yaw_right, pitch_right) + friction_profile_box_gap; + const uint16_t box_right = box_left + friction_profile_box_visual_width; + + const uint16_t box_top = yaw_raw_angle_y + raw_angle_font_size; + const uint16_t box_bottom = pitch_raw_angle_y; + + const bool friction_profile_1_active = + friction_profile_1_active_.ready() && *friction_profile_1_active_; + const uint16_t box_center_x = (box_left + box_right) / 2; + const uint16_t profile_number_y = box_top + friction_profile_number_gap; + + for (auto& line : friction_profile_indicator_) + line.set_color(Shape::Color::GREEN); + + friction_profile_number_.set_value(friction_profile_1_active ? 16 : 12); + friction_profile_number_.set_color( + friction_profile_1_active ? Shape::Color::PINK : Shape::Color::GREEN); + friction_profile_number_.set_font_size(friction_profile_number_font_size); + friction_profile_number_.set_xy(box_left, profile_number_y); + friction_profile_number_.set_center_x(box_center_x); + + friction_profile_indicator_[0].set_x(box_left); + friction_profile_indicator_[0].set_y(box_top); + friction_profile_indicator_[0].set_x2(box_right); + friction_profile_indicator_[0].set_y2(box_top); + + friction_profile_indicator_[1].set_x(box_right); + friction_profile_indicator_[1].set_y(box_top); + friction_profile_indicator_[1].set_x2(box_right); + friction_profile_indicator_[1].set_y2(box_bottom); + + friction_profile_indicator_[2].set_x(box_left); + friction_profile_indicator_[2].set_y(box_bottom); + friction_profile_indicator_[2].set_x2(box_right); + friction_profile_indicator_[2].set_y2(box_bottom); + + friction_profile_indicator_[3].set_x(box_left); + friction_profile_indicator_[3].set_y(box_top); + friction_profile_indicator_[3].set_x2(box_left); + friction_profile_indicator_[3].set_y2(box_bottom); status_ring_.update_friction_wheel_speed( std::min(*left_friction_velocity_, *right_friction_velocity_), *left_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); - update_static_status_ring(); + // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; + // const double target_confidence_value = + // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; + + // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); + // update_static_status_ring(); + last_keyboard_ = *keyboard_; + } + + void update_pitch_raw_angle_color() { + if (keyboard_.ready()) { + if (!last_keyboard_.e && keyboard_->e) { + last_e_triggered_with_ctrl_ = keyboard_->ctrl; + } + last_keyboard_ = *keyboard_; + } + + if (!gimbal_mode_.ready()) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + return; + } + + const bool is_encoder = *gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = + last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER && is_encoder; + const bool leaving_encoder = + last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER && !is_encoder; + + if (entering_encoder) { + pitch_encoder_by_ctrl_e_ = last_e_triggered_with_ctrl_; + } + + if (leaving_encoder) { + pitch_encoder_by_ctrl_e_ = false; + } + + if (!is_encoder) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + } else if (pitch_encoder_by_ctrl_e_) { + pitch_angle_number_.set_color(Shape::Color::PINK); + } else { + pitch_angle_number_.set_color(Shape::Color::GREEN); + } + + last_gimbal_mode_ = *gimbal_mode_; } void update_sniper_ui() { @@ -99,7 +273,7 @@ class Hero ? *gimbal_pitch_angle_ - 2 * std::numbers::pi : *gimbal_pitch_angle_; - rangefinder_.update_pitch_angle(-display_angle); + rangefinder_.update_pitch_angle(static_cast(*gimbal_pitch_raw_angle_)); double raw_height = -display_angle / 0.7 * static_cast(height_max); raw_height = std::clamp(raw_height, 0.0, static_cast(height_max)); @@ -107,6 +281,35 @@ class Hero lift_height = std::clamp(lift_height, height_min, height_max); rangefinder_.update_vertical_rangefinder(lift_height); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + } + + void update_bottom_yaw_tracking_lines() { + const bool ctrl_e_pressed = keyboard_->ctrl && keyboard_->e; + if (ctrl_e_pressed) { + bottom_yaw_tracking_enabled_ = !bottom_yaw_tracking_enabled_; + bottom_yaw_anchor_angle_rad_ = *bottom_yaw_angle_; + } + + const double delta_bottom_yaw_rad = *bottom_yaw_angle_ - bottom_yaw_anchor_angle_rad_; + + const int min_center_x = static_cast(pink_line_half_length); + const int max_center_x = + static_cast(screen_width) - static_cast(pink_line_half_length); + + const int pink_center_x = std::clamp( + static_cast(std::lround( + static_cast(x_center) + + delta_bottom_yaw_rad * pink_line_pixels_per_radian)), + min_center_x, max_center_x); + + tracking_pink_line_.set_x( + static_cast(pink_center_x - static_cast(pink_line_half_length))); + tracking_pink_line_.set_y(y_center + pink_line_offset_y); + tracking_pink_line_.set_x2( + static_cast(pink_center_x + static_cast(pink_line_half_length))); + tracking_pink_line_.set_y2(y_center + pink_line_offset_y); + tracking_pink_line_.set_visible(true); } void update_time_reminder() { @@ -121,17 +324,78 @@ class Hero status_ring_.update_static_parts({auto_aim_enable, precise_enable}); } + // void update_bullet_allowance() { + + // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); + // char* allow = text.data(); + // auto color = Shape::Color::YELLOW; + + // bullet_allowance_number_.set_value(allow); + // bullet_allowance_number_.set_font_size(14); + // bullet_allowance_number_.set_color(color); + // bullet_allowance_number_.set_visible(true); + // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); + // } + + void update_state_word() { + + const char* text = "OK"; + auto color = Shape::Color::GREEN; + + if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { + text = " WAITING "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { + text = " SHOOT "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { + text = " FIRED "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { + text = " JAM "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { + text = "PRELOADING"; + } + + state_word_.set_value(text); + state_word_.set_font_size(30); + state_word_.set_color(color); + state_word_.set_visible(true); + state_word_.set_xy(x_center - 800, y_center + 200); + } + void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; auto to_referee_angle = [](double angle) { - return static_cast( - std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + // return static_cast( + // std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + int degrees = static_cast( + std::lround((2.0 * std::numbers::pi - angle) / std::numbers::pi * 180.0)); + degrees %= 360; + if (degrees < 0) + degrees += 360; + return static_cast(degrees); }; - chassis_direction_indicator_.set_color( - chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN - : Shape::Color::PINK); - chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + // chassis_direction_indicator_.set_color( + // chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + // : Shape::Color::PINK); + // chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + const bool left_track_active = + std::abs(*left_track_velocity_) > track_velocity_active_threshold; + const bool right_track_active = + std::abs(*right_track_velocity_) > track_velocity_active_threshold; + + chassis_left_wheel_indicator_.set_color( + left_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + chassis_right_wheel_indicator_.set_color( + right_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + + const double wheel_offset = wheel_indicator_offset_deg * std::numbers::pi / 180.0; + const double left_wheel_angle = *chassis_angle_ + wheel_offset; + const double right_wheel_angle = *chassis_angle_ - wheel_offset; + + chassis_left_wheel_indicator_.set_angle( + to_referee_angle(left_wheel_angle), wheel_indicator_half_angle); + chassis_right_wheel_indicator_.set_angle( + to_referee_angle(right_wheel_angle), wheel_indicator_half_angle); bool chassis_control_direction_indicator_visible = false; if (!std::isnan(*chassis_control_angle_)) { @@ -158,10 +422,48 @@ class Hero static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr uint16_t raw_angle_font_size = 20; + + static constexpr uint16_t yaw_raw_angle_x = x_center + 270; + static constexpr uint16_t yaw_raw_angle_y = y_center + 65; + + static constexpr uint16_t pitch_raw_angle_x = x_center + 270; + static constexpr uint16_t pitch_raw_angle_y = y_center - 65; + + static constexpr uint16_t friction_profile_box_gap = 18; + static constexpr uint16_t friction_profile_box_visual_width = 105; + static constexpr uint16_t friction_profile_box_line_width = 6; + static constexpr uint16_t friction_profile_number_font_size = 24; + static constexpr uint16_t friction_profile_number_gap = 12; + static constexpr uint16_t height_min = 0, height_max = 500; + static constexpr uint16_t wheel_indicator_radius = 110; + static constexpr uint16_t wheel_indicator_width = 12; + static constexpr uint16_t wheel_indicator_half_angle = 10; + static constexpr double wheel_indicator_offset_deg = 28.0; + static constexpr double track_velocity_active_threshold = 1.0; + static constexpr uint16_t green_line_half_length = 8; + static constexpr uint16_t green_line_width = 40; + static constexpr uint16_t green_line_offset_y = 0; + + static constexpr uint16_t pink_line_half_length = 40; + static constexpr uint16_t pink_line_width = 8; + static constexpr uint16_t pink_line_offset_y = 0; + + static constexpr double pink_line_pixels_per_radian = 400.0; + + InputInterface gimbal_mode_; + InputInterface keyboard_; + + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + + bool last_e_triggered_with_ctrl_ = false; + bool pitch_encoder_by_ctrl_e_ = false; InputInterface chassis_mode_; InputInterface chassis_angle_, chassis_control_angle_; + InputInterface left_track_velocity_, right_track_velocity_; InputInterface supercap_voltage_; InputInterface supercap_control_enabled_; @@ -171,33 +473,60 @@ class Hero InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface robot_bullet_allowance_; + InputInterface robot_bullet_allowance_; InputInterface left_friction_control_velocity_; InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; + InputInterface friction_profile_1_active_; InputInterface mouse_; InputInterface game_stage_; + InputInterface gimbal_yaw_angle_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_player_viewer_angle_; - InputInterface laser_distance_; + InputInterface gimbal_player_viewer_raw_angle_; + InputInterface gimbal_pitch_raw_angle_; + InputInterface bottom_yaw_raw_angle_; + InputInterface bottom_yaw_angle_; + // InputInterface laser_distance_; InputInterface shoot_mode_; - InputInterface is_scope_active_; + InputInterface shoot_condiction_; + // InputInterface is_scope_active_; StatusRing status_ring_; Rangefinder rangefinder_; - Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc chassis_left_wheel_indicator_; + Arc chassis_right_wheel_indicator_; + Arc chassis_control_direction_indicator_; + + Float yaw_angle_number_; + Float pitch_angle_number_; + Float bottom_yaw_angle_number_; + Text state_word_; Integer time_reminder_; + + // Text bullet_allowance_label_; + Integer bullet_allowance_number_; + Integer friction_profile_number_; + Line friction_profile_indicator_[4]; + Line center_green_line_; + Line tracking_pink_line_; + + bool bottom_yaw_tracking_enabled_ = false; + double bottom_yaw_anchor_angle_rad_ = 0.0; + + // InputInterface auto_aim_fire_control_; + // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index a1268855..a28dc60e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -20,7 +20,9 @@ class Infantry , public rclcpp::Node { public: Infantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_(Shape::Color::WHITE, x_center - 12, y_center - 37) , status_ring_(26.5, 26.5, 600, 300) , horizontal_center_guidelines_( diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp index b387b972..e73e297d 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp @@ -48,7 +48,7 @@ class CfsScheduler { private: bool operator<(const Entity& obj) const { return vruntime_ < obj.vruntime_; } uint64_t vruntime_ : 48 = 65536; - uint16_t priority_ = 0; + uint16_t priority_ = 0; }; // class T : public Entity {}; @@ -58,10 +58,10 @@ class CfsScheduler { UpdateIterator() : current_(run_queue_.first()) , ignored_(nullptr) {} - UpdateIterator(const UpdateIterator&) = delete; + UpdateIterator(const UpdateIterator&) = delete; UpdateIterator& operator=(const UpdateIterator&) = delete; - UpdateIterator(UpdateIterator&&) = default; - UpdateIterator& operator=(UpdateIterator&&) = default; + UpdateIterator(UpdateIterator&&) = default; + UpdateIterator& operator=(UpdateIterator&&) = default; T* get() const { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) @@ -74,12 +74,12 @@ class CfsScheduler { auto update() { min_vruntime_ = current_->vruntime_; - int shift = 65536 - current_->priority_; + int shift = 65536 - current_->priority_; current_->vruntime_ += shift; run_queue_.erase(*current_); auto result = get()->update(); - current_ = ignored_ ? ignored_->next() : run_queue_.first(); + current_ = ignored_ ? ignored_->next() : run_queue_.first(); return result; } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp index b95766a9..ff7bed62 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp @@ -181,7 +181,7 @@ class BasicRedBlackTree { */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -209,7 +209,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->right; + tmp = node->right; } /* @@ -234,7 +234,7 @@ class BasicRedBlackTree { /* Case 1 - color flips */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -250,7 +250,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->left; + tmp = node->left; } /* Case 3 - left rotate at gparent */ @@ -315,7 +315,7 @@ class BasicRedBlackTree { * - old gets assigned new as a parent and 'color' as a color. */ void rotate_set_parents(Node* old_node, Node* new_node, Color color) { - Node* parent = old_node->parent(); + Node* parent = old_node->parent(); new_node->parent_and_color = old_node->parent_and_color; old_node->set_parent_and_color(new_node, color); change_child(old_node, new_node, parent); @@ -323,7 +323,7 @@ class BasicRedBlackTree { Node* __erase(Node* node) { Node* child = node->right; - Node* tmp = node->left; + Node* tmp = node->left; Node *parent, *rebalance; uintptr_t pc; @@ -335,22 +335,22 @@ class BasicRedBlackTree { * and node must be black due to 4). We adjust colors locally * so as to bypass __rb_erase_color() later on. */ - pc = node->parent_and_color; + pc = node->parent_and_color; parent = ((Node*)(pc & ~3)); change_child(node, child, parent); if (child) { child->parent_and_color = pc; - rebalance = nullptr; + rebalance = nullptr; } else rebalance = ((pc) & 1) ? parent : nullptr; tmp = parent; } else if (!child) { /* Still case 1, but this time the child is node->rb_left */ tmp->parent_and_color = pc = node->parent_and_color; - parent = ((Node*)(pc & ~3)); + parent = ((Node*)(pc & ~3)); change_child(node, tmp, parent); rebalance = nullptr; - tmp = parent; + tmp = parent; } else { Node *successor = child, *child2; @@ -384,9 +384,9 @@ class BasicRedBlackTree { * (c) */ do { - parent = successor; + parent = successor; successor = tmp; - tmp = tmp->left; + tmp = tmp->left; } while (tmp); child2 = successor->right; WRITE_ONCE(parent->left, child2); @@ -398,7 +398,7 @@ class BasicRedBlackTree { WRITE_ONCE(successor->left, tmp); tmp->set_parent(successor); - pc = node->parent_and_color; + pc = node->parent_and_color; tmp = ((Node*)(pc & ~3)); change_child(node, successor, tmp); @@ -409,7 +409,7 @@ class BasicRedBlackTree { rebalance = successor->is_black() ? parent : nullptr; } successor->parent_and_color = pc; - tmp = successor; + tmp = successor; } return rebalance; @@ -468,7 +468,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -508,7 +508,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->right, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* @@ -551,7 +551,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -565,7 +565,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->left, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* Case 4 - right rotate at parent + color flips */ diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp index 7fd0d851..f2305314 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp @@ -13,11 +13,11 @@ class RemoteShape { friend class RemoteShape; friend class RedBlackTree; - Descriptor() = default; - Descriptor(const Descriptor&) = delete; + Descriptor() = default; + Descriptor(const Descriptor&) = delete; Descriptor& operator=(const Descriptor&) = delete; - Descriptor(Descriptor&&) = delete; - Descriptor& operator=(Descriptor&& obj) = delete; + Descriptor(Descriptor&&) = delete; + Descriptor& operator=(Descriptor&& obj) = delete; [[nodiscard]] bool has_id() const { return id_; } [[nodiscard]] bool try_assign_id() @@ -79,9 +79,9 @@ class RemoteShape { private: /* Swap requirement: !this->id_ && victim.id_ */ void swap_id(Descriptor& victim) { - id_ = victim.id_; + id_ = victim.id_; assigned_list_[id_ - 1] = this; - existence_confidence_ = victim.existence_confidence_; + existence_confidence_ = victim.existence_confidence_; victim.revoke_id(); } @@ -94,7 +94,7 @@ class RemoteShape { } void revoke_id() { - id_ = 0; + id_ = 0; existence_confidence_ = 0; static_cast(this)->id_revoked(); @@ -104,7 +104,7 @@ class RemoteShape { return existence_confidence_ < obj.existence_confidence_; } - uint8_t id_ = 0; + uint8_t id_ = 0; uint8_t existence_confidence_ = 0; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index e0ac7365..37be26ae 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -87,18 +87,26 @@ class Shape set_modified(); } + void set_xy(uint16_t x, uint16_t y) { + if (part2_.x == x && part2_.y == y) + return; + part2_.x = x; + part2_.y = y; + set_modified(); + } + bool is_text_shape() const { return is_text_shape_; } enum class Operation : uint8_t { NO_OPERATION = 0, - ADD = 1, - MODIFY = 2, - DELETE = 3, + ADD = 1, + MODIFY = 2, + DELETE = 3, }; Operation predict_update() const { uint8_t predict_existence = existence_confidence(); - uint8_t predict_sync = sync_confidence_; + uint8_t predict_sync = sync_confidence_; if (!has_id() && !predict_try_assign_id(predict_existence)) { return Operation::NO_OPERATION; @@ -120,34 +128,37 @@ class Shape constexpr static inline command::Field no_operation_description() { return command::Field{[](std::byte* buffer) { - auto& description = *new (buffer) DescriptionField{}; + auto& description = *new (buffer) DescriptionField{}; description.part1.operation_type = Operation::NO_OPERATION; return sizeof(DescriptionField); }}; } enum class Color : uint8_t { - SELF = 0, + SELF = 0, YELLOW = 1, - GREEN = 2, + GREEN = 2, ORANGE = 3, PURPLE = 4, - PINK = 5, - CYAN = 6, - BLACK = 7, - WHITE = 8, + PINK = 5, + CYAN = 6, + BLACK = 7, + WHITE = 8, }; protected: + explicit Shape(bool is_text_shape = false) + : is_text_shape_(is_text_shape) {} + enum class ShapeType : uint8_t { - LINE = 0, + LINE = 0, RECTANGLE = 1, - CIRCLE = 2, - ELLIPSE = 3, - ARC = 4, - FLOAT = 5, - INTEGER = 6, - TEXT = 7, + CIRCLE = 2, + ELLIPSE = 3, + ARC = 4, + FLOAT = 5, + INTEGER = 6, + TEXT = 7, }; struct DescriptionField { @@ -173,7 +184,7 @@ class Shape }; void set_modified() { - // Optimization: Assume the modification not exist when invisible. + // Optimization: Assume the modification does not exist when invisible. if (!visible_) return; @@ -187,7 +198,7 @@ class Shape private: void enter_run_queue() { - uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); + uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); uint16_t weighted_priority = (priority_ - 256) << (4 * min_confidence); CfsScheduler::Entity::enter_run_queue(weighted_priority); } @@ -200,7 +211,7 @@ class Shape // Re-enter the update queue to try to get a new id. set_modified(); } else { - // Leave run_queue when shape was hidden. + // Leave run_queue when the shape is hidden. leave_run_queue(); } } @@ -210,10 +221,9 @@ class Shape // Called by CfsScheduler. if (!has_id() && !try_assign_id()) { - // TODO: Print error message. sync_confidence_ = max_update_times; - visible_ = false; - // Do nothing when failed + visible_ = false; + // Do nothing when the update fails. return no_operation_description(); } @@ -224,14 +234,14 @@ class Shape command::Field field; - // Optimization1: Stop adding when shape is invisible. - // Optimization2: Prevent continuous modification. + // Optimization 1: Stop adding when the shape is invisible. + // Optimization 2: Prevent continuous modification. if (visible_ && (existence_confidence() <= sync_confidence_ || (last_time_modified_ && existence_confidence() < max_update_times))) { // Send add packet last_time_modified_ = false; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::ADD); }}; if (increase_existence_confidence() < max_update_times @@ -240,7 +250,7 @@ class Shape } else { // Send modify packet last_time_modified_ = true; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::MODIFY); }}; // No need to compare existence_confidence here. @@ -275,11 +285,11 @@ class Shape auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = Color::WHITE; + description.part1.color = Color::WHITE; description.part2.width = 0; - description.part2.x = 0; - description.part2.y = 0; + description.part2.x = 0; + description.part2.y = 0; description.part3.details_c = 0; description.part3.details_d = 0; @@ -290,7 +300,7 @@ class Shape static constexpr uint8_t max_update_times = 4; - uint8_t priority_ = 15; + uint8_t priority_ = 15; uint8_t sync_confidence_ : 5 = max_update_times; bool is_text_shape_ : 1 = false; bool last_time_modified_ : 1 = false; @@ -305,10 +315,10 @@ class Line : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -342,7 +352,7 @@ class Line : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -370,10 +380,10 @@ class Circle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.rx = rx; - part3_.ry = ry; + part2_.x = x; + part2_.y = y; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -412,7 +422,7 @@ class Circle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ELLIPSE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -438,10 +448,10 @@ class Rectangle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -475,7 +485,7 @@ class Rectangle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::RECTANGLE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -497,18 +507,17 @@ class Arc : public Shape { public: Arc() = default; Arc(Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t angle_start, - uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) - : Arc() { + uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) { angle_start_ = angle_start; - angle_end_ = angle_end; + angle_end_ = angle_end; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; part3_.color = color; - part3_.rx = rx; - part3_.ry = ry; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -577,9 +586,9 @@ class Arc : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ARC; - description.part1.color = part3_.color; - description.part1.details_a = angle_start_; - description.part1.details_b = angle_end_; + description.part1.color = part3_.color; + description.part1.details_a = angle_start_; + description.part1.details_b = angle_end_; description.part2 = part2_; @@ -604,14 +613,13 @@ class Integer : public Shape { Integer() = default; Integer( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, int32_t value, - bool visible = true) - : Integer() { - color_ = color; + bool visible = true) { + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -627,7 +635,7 @@ class Integer : public Shape { } void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value <= 0 ? 1 : 0; for (; value != 0; number_of_digits++) value /= 10; @@ -656,8 +664,8 @@ class Integer : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::INTEGER; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -676,7 +684,7 @@ class Float : public Integer { using Integer::Integer; void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value < 0 ? 1 : 0; int integer_part = value / 1000; @@ -701,8 +709,8 @@ class Float : public Integer { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::FLOAT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -714,17 +722,18 @@ class Float : public Integer { class Text : public Shape { public: - Text() { value_ = nullptr; }; + Text() + : Shape(true) {} Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) : Text() { - color_ = color; + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -760,14 +769,14 @@ class Text : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::TEXT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; constexpr size_t data_part_size = 30; - auto str_length = std::min(strlen(value_), data_part_size); - description.part1.details_b = str_length; + auto str_length = std::min(strlen(value_), data_part_size); + description.part1.details_b = str_length; std::memcpy(buffer + sizeof(DescriptionField), value_, str_length); return sizeof(DescriptionField) + data_part_size; @@ -775,8 +784,8 @@ class Text : public Shape { uint16_t font_size_; Color color_; - const char* value_; + const char* value_ = nullptr; }; } // namespace app::ui -} // namespace rmcs_core::referee \ No newline at end of file +} // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp index 15823c6f..a45a58c4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -10,8 +10,7 @@ class CrossHairCircle { : CrossHairCircle(color, x, y, 12, 2, visible) {} CrossHairCircle( - Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, - bool visible = true) + Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, bool visible = true) : circle_(color, width, x, y, r, r, visible) {} void set_visible(bool value) { circle_.set_visible(value); } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index 72c81891..f8c84704 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -67,15 +67,13 @@ class DeformableChassisLegArcs { static constexpr double default_min_angle_deg_ = 8.0; static constexpr double default_max_angle_deg_ = 58.0; - static constexpr double deg_to_rad_(double degrees) { - return degrees * degrees_to_radians_; - } + static constexpr double deg_to_rad_(double degrees) { return degrees * degrees_to_radians_; } static constexpr std::array leg_base_mid_angles_ = { front_pair_offset_deg_ * degrees_to_radians_, std::numbers::pi_v - rear_pair_offset_deg_ * degrees_to_radians_, std::numbers::pi_v + rear_pair_offset_deg_ * degrees_to_radians_, - -front_pair_offset_deg_ * degrees_to_radians_, + -front_pair_offset_deg_* degrees_to_radians_, }; static constexpr std::array leg_radii_near_ = { @@ -93,9 +91,8 @@ class DeformableChassisLegArcs { }; static uint16_t to_referee_angle_(double angle) { - const auto degrees = - static_cast(std::lround((2.0 * std::numbers::pi_v - angle) - / std::numbers::pi_v * 180.0)); + const auto degrees = static_cast(std::lround( + (2.0 * std::numbers::pi_v - angle) / std::numbers::pi_v * 180.0)); int wrapped = degrees % 360; if (wrapped < 0) wrapped += 360; @@ -122,14 +119,13 @@ class DeformableChassisLegArcs { double leg_angle) const { const double normalized_extension = normalized_leg_extension_(leg_angle); // Min angle looks like a thin leg stretching radially outward from the center ring. - const uint16_t radius = static_cast(std::lround( - far_radius - normalized_extension * (far_radius - near_radius))); + const uint16_t radius = static_cast( + std::lround(far_radius - normalized_extension * (far_radius - near_radius))); const uint16_t half_angle = static_cast(std::lround( prone_leg_half_angle_ - normalized_extension * (prone_leg_half_angle_ - upright_leg_half_angle_))); const uint16_t width = static_cast(std::lround( - prone_leg_width_ - + normalized_extension * (upright_leg_width_ - prone_leg_width_))); + prone_leg_width_ + normalized_extension * (upright_leg_width_ - prone_leg_width_))); leg.set_x(center_x_); leg.set_y(center_y_); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp index f3361b58..efb9f766 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp @@ -151,16 +151,16 @@ class Rangefinder { } private: - constexpr static uint16_t x_center_ = 960; - constexpr static uint16_t y_center_ = 540; + constexpr static uint16_t x_center_ = 960; + constexpr static uint16_t y_center_ = 540; constexpr static uint16_t x_side_center_ = x_center_ - 110; - constexpr static uint16_t horizontal_unit_scale = 15; + constexpr static uint16_t horizontal_unit_scale = 15; constexpr static uint16_t horizontal_scale_accuracy_ = 40; - constexpr static uint16_t vertical_unit_scale_ = 5; + constexpr static uint16_t vertical_unit_scale_ = 5; constexpr static uint16_t vertical_side_unit_scale_ = 15; - constexpr static uint16_t vertical_scale_accuracy_ = 26; + constexpr static uint16_t vertical_scale_accuracy_ = 26; Float pitch_angle_{Shape::Color::BLACK, 17, 2, x_center_ - 100, y_center_ + 170, 0, false}; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 35882689..36c8c8b1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -133,7 +133,7 @@ class StatusRing { void update_static_parts(std::tuple enable) { auto& [auto_aim_enable, precise_enable] = enable; - auto static_enable = auto_aim_enable || precise_enable; + auto static_enable = auto_aim_enable || precise_enable; static auto color{Shape::Color::WHITE}; @@ -331,9 +331,9 @@ class StatusRing { void set_limits( double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { supercap_limit_ = supercap_limit; - battery_limit_ = battery_limit; + battery_limit_ = battery_limit; friction_limit_ = friction_limit; - bullet_limit_ = bullet_limit; + bullet_limit_ = bullet_limit; int scale_angle = 5; for (auto& bullet_scale : bullet_scales_) { @@ -351,13 +351,13 @@ class StatusRing { } double value = 0; - scale_angle = 5; + scale_angle = 5; for (auto& number : bullet_scales_number_) { scale_angle += (visible_angle) / 4; value += static_cast(bullet_limit_) / 4; - const auto r = visible_radius - width_ring + 30; + const auto r = visible_radius - width_ring + 30; const auto angle = static_cast(-scale_angle) * std::numbers::pi / 180; number.set_x(x_center + static_cast(r * std::cos(angle))); @@ -370,11 +370,11 @@ class StatusRing { } } - constexpr static uint16_t x_center = 960; - constexpr static uint16_t y_center = 540; - constexpr static uint16_t width_ring = 15; + constexpr static uint16_t x_center = 960; + constexpr static uint16_t y_center = 540; + constexpr static uint16_t width_ring = 15; constexpr static uint16_t visible_radius = 400; - constexpr static uint16_t visible_angle = 40; + constexpr static uint16_t visible_angle = 40; double supercap_limit_; double battery_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/command.cpp b/rmcs_ws/src/rmcs_core/src/referee/command.cpp index 95326d34..34c55849 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command.cpp @@ -16,7 +16,9 @@ class Command , public rclcpp::Node { public: Command() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , next_sent_(std::chrono::steady_clock::time_point::min()) , interaction_next_sent_(std::chrono::steady_clock::time_point::min()) , map_marker_next_sent_(std::chrono::steady_clock::time_point::min()) diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index dc1e3271..cea7953c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -45,7 +45,7 @@ class Ui resetting_ = 4; } last_game_stage_ = *game_stage_; - last_keyboard_ = *keyboard_; + last_keyboard_ = *keyboard_; if (resetting_) { *ui_field_ = Field{[this](std::byte* buffer) { @@ -67,10 +67,10 @@ class Ui size_t write_resetting_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; - header.command_id = 0x0100; // Clear shapes + auto& header = *new (buffer + written) Header{}; + header.command_id = 0x0100; // Clear shapes auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); @@ -79,7 +79,7 @@ class Ui uint8_t layer; }; auto& command = *new (buffer + written) Command{}; - command.type = 2; // Clear all layers + command.type = 2; // Clear all layers command.layer = 0; written += sizeof(Command); @@ -89,9 +89,9 @@ class Ui size_t write_updating_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; + auto& header = *new (buffer + written) Header{}; auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); diff --git a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp index d8a406e4..4d719add 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee { -constexpr uint8_t sof_value = 0xa5; +constexpr uint8_t sof_value = 0xa5; constexpr size_t frame_data_max_length = 1024; struct __attribute__((packed)) FrameHeader { diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index de3911af..ad321125 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee::status { struct __attribute__((packed)) GameStatus { - uint8_t game_type : 4; + uint8_t game_type : 4; uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t sync_timestamp; diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index bfb10288..bd9ed067 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -1,22 +1,56 @@ #pragma once +#include #include +#include +#include +#include #include #include +#include #include #include #include +#include #include #include #include +#include #include +#include +#include + namespace rmcs_executor { +enum class InterfaceKind { + Normal, + Event, +}; + +inline const char* interface_kind_name(InterfaceKind kind) { + switch (kind) { + case InterfaceKind::Normal: return "Normal"; + case InterfaceKind::Event: return "Event"; + } + + return "Unknown"; +} + +using EventState = std::uint32_t; +constexpr EventState EVENT_DISABLED_BIT = EventState{1} << 31; +constexpr EventState EVENT_ACTIVE_MASK = ~EVENT_DISABLED_BIT; + class Component { public: friend class Executor; + struct OutputInfo { + std::reference_wrapper type; + InterfaceKind kind; + }; + using OutputInfoMap = std::map; + Component(const Component&) = delete; Component& operator=(const Component&) = delete; Component(Component&&) = delete; @@ -24,9 +58,7 @@ class Component { virtual ~Component() = default; - virtual void before_pairing(const std::map& output_map) { - (void)output_map; - } + virtual void before_pairing(const OutputInfoMap& output_map) { (void)output_map; } virtual void before_updating() {} virtual void update() = 0; @@ -79,9 +111,9 @@ class Component { const T& operator*() const { return *data_pointer_; } private: - void** activate() { + void* activate() { activated = true; - return reinterpret_cast(&data_pointer_); + return reinterpret_cast(&data_pointer_); } T* data_pointer_ = nullptr; @@ -90,6 +122,139 @@ class Component { bool delete_data_when_deconstruct = false; }; + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventInputInterface { + public: + friend class Component; + + template + requires std::invocable + explicit EventInputInterface(Callback&& callback) + : callback_(std::forward(callback)) {} + + EventInputInterface(const EventInputInterface&) = delete; + EventInputInterface& operator=(const EventInputInterface&) = delete; + EventInputInterface(EventInputInterface&&) = delete; + EventInputInterface& operator=(EventInputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + [[nodiscard]] bool ready() const { return static_cast(callback_); } + + private: + void* activate() { + if (!ready()) + throw std::runtime_error( + "The event input interface requires a callback before registration"); + + activated = true; + return reinterpret_cast(&callback_); + } + + std::function callback_; + bool activated = false; + }; + + template + requires( + !std::is_reference_v && !std::is_unbounded_array_v + && std::is_nothrow_copy_constructible_v && std::is_nothrow_destructible_v) + class QueuedEventInputInterface final : public EventInputInterface { + public: + template + requires std::invocable + QueuedEventInputInterface(size_t queue_depth, Callback&& callback) + : EventInputInterface([this](const T& event) { enqueue(event); }) + , user_callback_(std::forward(callback)) + , queue_(queue_depth) + , worker_(&QueuedEventInputInterface::worker_main, this) {} + + ~QueuedEventInputInterface() { + stop_requested_.store(true, std::memory_order::relaxed); + notify_event(); + if (worker_.joinable()) + worker_.join(); + } + + QueuedEventInputInterface(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface& operator=(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface(QueuedEventInputInterface&&) = delete; + QueuedEventInputInterface& operator=(QueuedEventInputInterface&&) = delete; + + private: + void enqueue(const T& event) { + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + auto guard = std::scoped_lock{enqueue_mutex_}; + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + if (!queue_.push_back(event)) { + const auto dropped_count = dropped_event_count_++; + if (dropped_count == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface started dropping events because the queue is " + "full"); + } + return; + } + + const auto dropped_count = dropped_event_count_; + dropped_event_count_ = 0; + if (dropped_count != 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface resumed enqueueing after dropping %u events", + dropped_count); + } + + notify_event(); + } + + void notify_event() { + event_count_.fetch_add(1, std::memory_order::release); + event_count_.notify_one(); + } + + void worker_main() { + while (!stop_requested_.load(std::memory_order::relaxed)) { + if (auto* event = queue_.peek_front()) { + try { + user_callback_(std::move(*event)); + } catch (const std::exception& exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by exception: %s", + exception.what()); + return; + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by unknown exception"); + return; + } + + if (!queue_.pop_front([](T&&) noexcept {})) + std::terminate(); + continue; + } + + const auto old = event_count_.load(std::memory_order::relaxed); + if (!queue_.readable() && !stop_requested_.load(std::memory_order::relaxed)) + event_count_.wait(old, std::memory_order::acquire); + } + } + + std::function user_callback_; + rmcs_utility::RingBuffer queue_; + std::atomic stop_requested_{false}; + std::uint32_t dropped_event_count_ = 0; + std::atomic event_count_{0}; + std::mutex enqueue_mutex_; + std::thread worker_; + }; + template requires(!std::is_reference_v && !std::is_unbounded_array_v) class OutputInterface { public: @@ -104,25 +269,111 @@ class Component { ~OutputInterface() { if (active()) - std::destroy_at(std::launder(reinterpret_cast(&data_))); + std::destroy_at(storage_pointer()); }; [[nodiscard]] bool active() const { return activated; } - T* operator->() { return reinterpret_cast(&data_); } - const T* operator->() const { return reinterpret_cast(&data_); } - T& operator*() { return *reinterpret_cast(&data_); } - const T& operator*() const { return *reinterpret_cast(&data_); } + T* operator->() { return storage_pointer(); } + const T* operator->() const { return storage_pointer(); } + T& operator*() { return *storage_pointer(); } + const T& operator*() const { return *storage_pointer(); } private: template void* activate(Args&&... args) { - ::new (&data_) T(std::forward(args)...); + std::construct_at(raw_storage_pointer(), std::forward(args)...); + activated = true; + return data_; + } + + [[nodiscard]] T* raw_storage_pointer() { return reinterpret_cast(data_); } + + [[nodiscard]] T* storage_pointer() { return std::launder(raw_storage_pointer()); } + + [[nodiscard]] const T* storage_pointer() const { + return std::launder(reinterpret_cast(data_)); + } + + alignas(T) std::byte data_[sizeof(T)]; + bool activated = false; + }; + + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventOutputInterface { + public: + friend class Component; + + EventOutputInterface() = default; + + EventOutputInterface(const EventOutputInterface&) = delete; + EventOutputInterface& operator=(const EventOutputInterface&) = delete; + EventOutputInterface(EventOutputInterface&&) = delete; + EventOutputInterface& operator=(EventOutputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + + void emit(const T& event) { + if (!active()) + throw std::runtime_error("The event output interface has not been activated"); + if (!try_enter_emit()) + return; + + struct LeaveEmitGuard { + EventOutputInterface& interface; + + ~LeaveEmitGuard() { interface.leave_emit(); } + } leave_emit_guard{*this}; + + for (const auto* callback : callback_list_) + (*callback)(event); + } + + private: + using EventCallback = std::function; + + void* activate() { activated = true; - return reinterpret_cast(&data_); + return this; } - std::aligned_storage_t data_; + void add_listener(EventCallback* callback) { callback_list_.emplace_back(callback); } + + bool try_enter_emit() { + auto state = state_.load(std::memory_order_relaxed); + while (true) { + if (state & EVENT_DISABLED_BIT) + return false; + if ((state & EVENT_ACTIVE_MASK) == EVENT_ACTIVE_MASK) + throw std::runtime_error("Too many active event emissions"); + + if (state_.compare_exchange_weak( + state, state + 1, std::memory_order_relaxed, std::memory_order_relaxed)) + return true; + } + } + + void leave_emit() { + const auto previous_state = state_.fetch_sub(1, std::memory_order_release); + const auto new_state = previous_state - 1; + if ((new_state & EVENT_DISABLED_BIT) != 0 && (new_state & EVENT_ACTIVE_MASK) == 0) + state_.notify_one(); + } + + void disable() { state_.fetch_or(EVENT_DISABLED_BIT, std::memory_order_relaxed); } + + void enable() { state_.fetch_and(EVENT_ACTIVE_MASK, std::memory_order_relaxed); } + + void wait_idle() { + auto state = state_.load(std::memory_order_acquire); + while ((state & EVENT_ACTIVE_MASK) != 0) { + state_.wait(state, std::memory_order_acquire); + state = state_.load(std::memory_order_acquire); + } + } + + std::vector callback_list_; + std::atomic state_{EVENT_DISABLED_BIT}; bool activated = false; }; @@ -133,7 +384,25 @@ class Component { const std::string& name, InputInterface& interface, bool required = true) { if (interface.active()) throw std::runtime_error("The interface has been activated"); - input_list_.emplace_back(typeid(T), name, required, interface.activate()); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Normal, required, interface.activate(), + &bind_input_interface); + } + + template + void register_input( + const std::string& name, EventInputInterface& interface, bool required = true) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, required, interface.activate(), + &bind_event_input_interface); } template @@ -141,14 +410,31 @@ class Component { void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { if (interface.active()) throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Output); output_list_.emplace_back( - typeid(T), name, interface.activate(std::forward(args)...), this); + typeid(T), name, InterfaceKind::Normal, interface.activate(std::forward(args)...), + nullptr, nullptr, nullptr, this); + } + + template + void register_output(const std::string& name, EventOutputInterface& interface) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Output); + output_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, interface.activate(), + &enable_event_output_interface, &disable_event_output_interface, + &wait_event_output_interface_idle, this); } template requires std::constructible_from std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { - initializing_component_name = name.c_str(); + initializing_component_name = name; auto component = std::make_shared(std::forward(args)...); partner_component_list_.emplace_back(component); @@ -156,26 +442,104 @@ class Component { return component; } - static const char* initializing_component_name; + static std::string initializing_component_name; protected: Component() : component_name_(initializing_component_name) {} private: + enum class RegistrationDirection { + Input, + Output, + }; + + using BindFunction = void (*)(void* input_binding, void* output_binding); + using OutputLifecycleHook = void (*)(void* output_binding); + + static const char* registration_direction_name(RegistrationDirection direction) { + switch (direction) { + case RegistrationDirection::Input: return "input"; + case RegistrationDirection::Output: return "output"; + } + + return "interface"; + } + + void ensure_registration_name_is_available( + const std::string& name, InterfaceKind kind, RegistrationDirection direction) const { + auto check_declarations = [&](const auto& declarations, + RegistrationDirection existing_direction) { + for (const auto& declaration : declarations) { + if (declaration.name != name) + continue; + + if (declaration.kind != kind) { + throw std::runtime_error( + "Component [" + component_name_ + "] cannot register " + + interface_kind_name(kind) + " " + registration_direction_name(direction) + + " \"" + name + "\" because \"" + name + + "\" has already been registered as " + + interface_kind_name(declaration.kind) + " " + + registration_direction_name(existing_direction) + " in this component."); + } + + throw std::runtime_error( + "Component [" + component_name_ + "] registered " + interface_kind_name(kind) + + " " + registration_direction_name(direction) + " \"" + name + + "\" more than once."); + } + }; + + check_declarations(input_list_, RegistrationDirection::Input); + check_declarations(output_list_, RegistrationDirection::Output); + } + + template + static void bind_input_interface(void* input_binding, void* output_binding) { + *reinterpret_cast(input_binding) = reinterpret_cast(output_binding); + } + + template + static void bind_event_input_interface(void* input_binding, void* output_binding) { + auto& callback = *reinterpret_cast*>(input_binding); + reinterpret_cast*>(output_binding)->add_listener(&callback); + } + + template + static void enable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->enable(); + } + + template + static void disable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->disable(); + } + + template + static void wait_event_output_interface_idle(void* output_binding) { + reinterpret_cast*>(output_binding)->wait_idle(); + } + std::string component_name_; struct InputDeclaration { const std::type_info& type; std::string name; + InterfaceKind kind; bool required; - void** pointer_to_data_pointer; + void* binding; + BindFunction bind; }; struct OutputDeclaration { const std::type_info& type; std::string name; - void* data_pointer; + InterfaceKind kind; + void* binding; + OutputLifecycleHook enable; + OutputLifecycleHook disable; + OutputLifecycleHook wait_idle; Component* component; }; diff --git a/rmcs_ws/src/rmcs_executor/package.xml b/rmcs_ws/src/rmcs_executor/package.xml index a287fb49..5ed7bcd6 100644 --- a/rmcs_ws/src/rmcs_executor/package.xml +++ b/rmcs_ws/src/rmcs_executor/package.xml @@ -11,6 +11,7 @@ rclcpp pluginlib + rmcs_utility ament_cmake diff --git a/rmcs_ws/src/rmcs_executor/src/component.cpp b/rmcs_ws/src/rmcs_executor/src/component.cpp index 561ac7a2..181ae975 100644 --- a/rmcs_ws/src/rmcs_executor/src/component.cpp +++ b/rmcs_ws/src/rmcs_executor/src/component.cpp @@ -2,6 +2,6 @@ namespace rmcs_executor { -const char* Component::initializing_component_name; +std::string Component::initializing_component_name; -} // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/src/executor.hpp index a818d50a..eb454fd0 100644 --- a/rmcs_ws/src/rmcs_executor/src/executor.hpp +++ b/rmcs_ws/src/rmcs_executor/src/executor.hpp @@ -2,8 +2,10 @@ #include #include +#include #include #include +#include #include #include @@ -21,18 +23,22 @@ class Executor final : public rclcpp::Node { public: explicit Executor( const std::string& node_name, rclcpp::executors::SingleThreadedExecutor& rcl_executor) - : Node{node_name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} + : Node{ + node_name, + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} , rcl_executor_(rcl_executor) { Component::initializing_component_name = "predefined_msg_provider"; - predefined_msg_provider_ = std::make_shared(); + predefined_msg_provider_ = std::make_shared(); add_component(predefined_msg_provider_); } - ~Executor() { + ~Executor() override { + disable_event_outputs(); + wait_event_outputs_idle(); if (thread_.joinable()) thread_.join(); }; - void add_component(std::shared_ptr component) { + void add_component(const std::shared_ptr& component) { component_list_.emplace_back(component); if (auto node = std::dynamic_pointer_cast(component)) rcl_executor_.add_node(node); @@ -50,35 +56,195 @@ class Executor final : public rclcpp::Node { if (!get_parameter("update_rate", update_rate)) throw std::runtime_error{"Unable to get parameter update_rate"}; predefined_msg_provider_->set_update_rate(update_rate); + enable_event_outputs(); thread_ = std::thread{[update_rate, this]() { const auto period = std::chrono::nanoseconds( static_cast(std::round(1'000'000'000.0 / update_rate))); auto next_iteration_time = std::chrono::steady_clock::now(); - while (rclcpp::ok()) { - predefined_msg_provider_->set_timestamp(next_iteration_time); - next_iteration_time += period; - for (const auto& component : updating_order_) { - component->update(); + try { + while (rclcpp::ok()) { + predefined_msg_provider_->set_timestamp(next_iteration_time); + next_iteration_time += period; + for (const auto& component : updating_order_) { + component->update(); + } + std::this_thread::sleep_until(next_iteration_time); } - std::this_thread::sleep_until(next_iteration_time); + } catch (const std::exception& exception) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by exception: %s", + exception.what()); + rclcpp::shutdown(); + } catch (...) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by unknown exception"); + rclcpp::shutdown(); } }}; } private: + void enable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.enable == nullptr) + continue; + output.enable(output.binding); + } + } + } + + void disable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.disable == nullptr) + continue; + output.disable(output.binding); + } + } + } + + void wait_event_outputs_idle() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.wait_idle == nullptr) + continue; + output.wait_idle(output.binding); + } + } + } + + struct InterfaceKey { + std::string name; + InterfaceKind kind; + + bool operator==(const InterfaceKey& other) const { + return name == other.name && kind == other.kind; + } + }; + + struct InterfaceKeyHash { + std::size_t operator()(const InterfaceKey& key) const { + return std::hash{}(key.name) + ^ (std::hash{}(static_cast(key.kind)) << 1U); + } + }; + + using OutputRecord = Component::OutputDeclaration*; + + struct NameKindRecord { + InterfaceKind kind; + std::string component_name; + const char* direction; + }; + + static std::string describe_output( + const std::string& name, InterfaceKind kind, const std::type_info& type, + const std::string& component_name) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << " output \"" << name << "\" with type \"" << type.name() << "\""; + return stream.str(); + } + + static std::string describe_output(const Component::OutputDeclaration& output) { + return describe_output( + output.name, output.kind, output.type, output.component->get_component_name()); + } + + static std::string describe_declaration( + const std::string& name, InterfaceKind kind, const std::string& component_name, + const char* direction) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << ' ' << direction << " \"" << name << "\""; + return stream.str(); + } + void init() { updating_order_.clear(); - auto output_map = std::unordered_map{}; - auto user_output_map = std::map{}; + auto output_map = std::unordered_map{}; + auto output_by_name = std::unordered_map{}; + auto declaration_kind_map = std::unordered_map{}; + auto user_output_map = Component::OutputInfoMap{}; for (const auto& component : component_list_) { component->dependency_count_ = 0; component->wanted_by_.clear(); for (auto& output : component->output_list_) { - if (!output_map.emplace(output.name, &output).second) - throw std::runtime_error{"Duplicate names of output"}; - user_output_map.emplace(output.name, output.type); + auto declaration_iter = declaration_kind_map.find(output.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != output.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + output.name + + "\": " + + describe_declaration( + output.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + output.name, output.kind, component->get_component_name(), "output") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + output.name, + NameKindRecord{output.kind, component->get_component_name(), "output"}); + + auto existing_output_iter = output_by_name.find(output.name); + if (existing_output_iter != output_by_name.end() + && existing_output_iter->second->kind != output.kind) { + const auto& existing_output = *existing_output_iter->second; + const auto message = std::string{"Conflicting output kinds for name \""} + + output.name + "\": " + describe_output(existing_output) + + "; " + describe_output(output) + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + output_by_name.emplace(output.name, &output); + + const auto [output_iter, inserted] = + output_map.emplace(InterfaceKey{output.name, output.kind}, &output); + if (!inserted) { + const auto& existing_output = *output_iter->second; + const auto message = + std::string{"Duplicate "} + interface_kind_name(output.kind) + + " output name \"" + output.name + + "\": " + describe_output(existing_output) + "; " + describe_output(output) + + ". Only one output may be registered for each (name, kind)."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + user_output_map.emplace( + output.name, Component::OutputInfo{std::cref(output.type), output.kind}); + } + + for (const auto& input : component->input_list_) { + auto declaration_iter = declaration_kind_map.find(input.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != input.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + input.name + "\": " + + describe_declaration( + input.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + input.name, input.kind, component->get_component_name(), "input") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + input.name, + NameKindRecord{input.kind, component->get_component_name(), "input"}); } } @@ -88,36 +254,54 @@ class Executor final : public rclcpp::Node { for (const auto& component : component_list_) { for (const auto& input : component->input_list_) { - auto output_iter = output_map.find(input.name); + auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); if (output_iter == output_map.end()) { + auto output_by_name_iter = output_by_name.find(input.name); + if (output_by_name_iter != output_by_name.end()) { + const auto& available_output = *output_by_name_iter->second; + const auto message = + std::string{"Component ["} + component->get_component_name() + + "] requested " + interface_kind_name(input.kind) + " input \"" + + input.name + "\", but " + describe_output(available_output) + "."; + + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + if (!input.required) continue; - RCLCPP_FATAL( - get_logger(), - "Cannot find the corresponding output of input \"%s\" declared by " - "component [%s]", - input.name.c_str(), component->get_component_name().c_str()); - throw std::runtime_error{"Cannot find the corresponding output"}; + const auto message = std::string{"Cannot find "} + + interface_kind_name(input.kind) + " output \"" + input.name + + "\" required by component [" + + component->get_component_name() + "]."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; } const auto& output = *output_iter->second; if (input.type != output.type) { - RCLCPP_FATAL(get_logger(), "With message \"%s\":", input.name.c_str()); + const auto message = std::string{"Type mismatch for "} + + interface_kind_name(input.kind) + " interface \"" + + input.name + "\": component [" + + output.component->get_component_name() + + "] declared output type \"" + output.type.name() + + "\", but component [" + component->get_component_name() + + "] requested input type \"" + input.type.name() + "\"."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); RCLCPP_FATAL( get_logger(), " Component [%s] declared the output with type \"%s\"", output.component->get_component_name().c_str(), output.type.name()); RCLCPP_FATAL( get_logger(), " Component [%s] requested the input with type \"%s\"", component->get_component_name().c_str(), input.type.name()); - RCLCPP_FATAL(get_logger(), "Type not match."); - throw std::runtime_error{"Type not match"}; + throw std::runtime_error{message}; } if (output.component->wanted_by_.emplace(component.get()).second) component->dependency_count_++; - *input.pointer_to_data_pointer = output.data_pointer; + input.bind(input.binding, output.binding); } } @@ -140,12 +324,17 @@ class Executor final : public rclcpp::Node { RCLCPP_FATAL( get_logger(), "Component [%s]:", component->get_component_name().c_str()); for (const auto& input : component->input_list_) { - const auto& output = output_map[input.name]; + const auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); + if (output_iter == output_map.end()) + continue; + + const auto* output = output_iter->second; if (output->component->dependency_count_ == 0) continue; RCLCPP_FATAL( - get_logger(), " Depends on [%s] because requesting \"%s\"", - output->component->component_name_.c_str(), output->name.c_str()); + get_logger(), " Depends on [%s] because requesting %s interface \"%s\"", + output->component->component_name_.c_str(), interface_kind_name(input.kind), + output->name.c_str()); } } throw std::runtime_error{"Circular dependency found"}; @@ -182,4 +371,4 @@ class Executor final : public rclcpp::Node { size_t dependency_recursive_level_ = 0; }; -}; // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index 99311f24..da4c29fb 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv) { plugin_name = component_name = component_description; } - rmcs_executor::Component::initializing_component_name = component_name.c_str(); + rmcs_executor::Component::initializing_component_name = component_name; auto component = component_loader.createSharedInstance(plugin_name); executor->add_component(component); } @@ -70,4 +70,4 @@ int main(int argc, char** argv) { rcl_executor.spin(); rclcpp::shutdown(); -} \ No newline at end of file +} diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp new file mode 100644 index 00000000..e96fa76c --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct BoardClock { + using rep = std::int64_t; + using period = std::ratio<1, 4'000'000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + + static constexpr bool is_steady = true; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp new file mode 100644 index 00000000..526b79f2 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace rmcs_msgs { + +struct CameraFrameRaw { + static constexpr std::uint32_t kWidth = 1440; + static constexpr std::uint32_t kHeight = 1080; + static constexpr std::size_t kFrameSize = + static_cast(kWidth) * static_cast(kHeight); + + std::array data; + int opencv_cvt_color_code; + + Eigen::Quaterniond imu_snapshot; + + std::chrono::steady_clock::time_point exposure_timestamp; + std::chrono::steady_clock::time_point image_reception_timestamp; + std::chrono::steady_clock::time_point sync_publish_timestamp; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index e534ce0b..92d391b5 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -5,9 +5,9 @@ namespace rmcs_msgs { enum class ChassisMode : uint8_t { - AUTO = 0, - SPIN = 1, - STEP_DOWN = 2, + AUTO = 0, + SPIN = 1, + STEP_DOWN = 2, LAUNCH_RAMP = 3, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp index b0ec852c..4cdc6d5d 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp @@ -12,43 +12,43 @@ class FullRobotId { enum Value : uint16_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, - - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, + + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, - - RED_HERO_CLIENT = 0x0101, - RED_ENGINEER_CLIENT = 0x0102, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, + + RED_HERO_CLIENT = 0x0101, + RED_ENGINEER_CLIENT = 0x0102, RED_INFANTRY_III_CLIENT = 0x0103, - RED_INFANTRY_IV_CLIENT = 0x0104, - RED_INFANTRY_V_CLIENT = 0x0105, - RED_AERIAL_CLIENT = 0x0106, + RED_INFANTRY_IV_CLIENT = 0x0104, + RED_INFANTRY_V_CLIENT = 0x0105, + RED_AERIAL_CLIENT = 0x0106, - BLUE_HERO_CLIENT = 0x0165, - BLUE_ENGINEER_CLIENT = 0x0166, + BLUE_HERO_CLIENT = 0x0165, + BLUE_ENGINEER_CLIENT = 0x0166, BLUE_INFANTRY_III_CLIENT = 0x0167, - BLUE_INFANTRY_IV_CLIENT = 0x0168, - BLUE_INFANTRY_V_CLIENT = 0x0169, - BLUE_AERIAL_CLIENT = 0x016A, + BLUE_INFANTRY_IV_CLIENT = 0x0168, + BLUE_INFANTRY_V_CLIENT = 0x0169, + BLUE_AERIAL_CLIENT = 0x016A, REFEREE_SERVER = 0x8080, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp index 622a171d..d94e04dc 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp @@ -5,13 +5,13 @@ namespace rmcs_msgs { enum class GameStage : uint8_t { - NOT_START = 0, - PREPARATION = 1, + NOT_START = 0, + PREPARATION = 1, REFEREE_CHECK = 2, - COUNTDOWN = 3, - STARTED = 4, - SETTLING = 5, - UNKNOWN = UINT8_MAX, + COUNTDOWN = 3, + STARTED = 4, + SETTLING = 5, + UNKNOWN = UINT8_MAX, }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp new file mode 100644 index 00000000..1c0747b3 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct ImuSnapshot { + Eigen::Quaterniond orientation; + Eigen::Vector3d gyro_body; + rmcs_msgs::BoardClock::time_point timestamp; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 09925e7b..b2ec7c37 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -9,18 +9,21 @@ # endif #endif -#include "chassis_mode.hpp" -#include "full_robot_id.hpp" -#include "game_stage.hpp" -#include "gimbal_mode.hpp" -#include "keyboard.hpp" -#include "mouse.hpp" -#include "robot_color.hpp" -#include "robot_id.hpp" -#include "serial_interface.hpp" -#include "shoot_mode.hpp" -#include "shoot_status.hpp" -#include "switch.hpp" +#include "board_clock.hpp" // IWYU pragma: export +#include "camera_frame_raw.hpp" // IWYU pragma: export +#include "chassis_mode.hpp" // IWYU pragma: export +#include "full_robot_id.hpp" // IWYU pragma: export +#include "game_stage.hpp" // IWYU pragma: export +#include "gimbal_mode.hpp" // IWYU pragma: export +#include "imu_snapshot.hpp" // IWYU pragma: export +#include "keyboard.hpp" // IWYU pragma: export +#include "mouse.hpp" // IWYU pragma: export +#include "robot_color.hpp" // IWYU pragma: export +#include "robot_id.hpp" // IWYU pragma: export +#include "serial_interface.hpp" // IWYU pragma: export +#include "shoot_mode.hpp" // IWYU pragma: export +#include "shoot_status.hpp" // IWYU pragma: export +#include "switch.hpp" // IWYU pragma: export namespace rmcs_msgs { diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp index dc7ee694..c5517f2d 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp @@ -6,8 +6,8 @@ namespace rmcs_msgs { enum class RobotColor : uint8_t { UNKNOWN = 0, - RED = 1, - BLUE = 2, + RED = 1, + BLUE = 2, }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index 622d5fdc..61e063e2 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -6,18 +6,18 @@ namespace rmcs_msgs { enum class ArmorID : uint16_t { - Unknown = 0, - Hero = 1, - Engineer = 2, + Unknown = 0, + Hero = 1, + Engineer = 2, InfantryIII = 3, - InfantryIV = 4, - InfantryV = 5, - Aerial = 6, - Sentry = 7, - Dart = 8, - Radar = 9, - Outpost = 10, - Base = 11, + InfantryIV = 4, + InfantryV = 5, + Aerial = 6, + Sentry = 7, + Dart = 8, + Radar = 9, + Outpost = 10, + Base = 11, }; class RobotId { @@ -25,29 +25,29 @@ class RobotId { enum Value : uint8_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, }; constexpr RobotId() @@ -67,7 +67,9 @@ class RobotId { constexpr bool operator==(const Value value) const { return value_ == value; } constexpr bool operator!=(const Value value) const { return value_ != value; } - constexpr RobotColor color() const { return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } + constexpr RobotColor color() const { + return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; + } constexpr ArmorID id() const { return value_ > 100 ? static_cast(value_ - 100) : static_cast(value_); diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp index a198e5a8..52d736a4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp @@ -7,7 +7,7 @@ namespace rmcs_msgs { struct SerialInterface { std::function read; - std::function write; + std::function write; }; -} \ No newline at end of file +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp new file mode 100644 index 00000000..95dd4090 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ShootCondiction : uint8_t { + FRICTION_WAITING = 0, + PRELOADING = 1, + SHOOT = 2, + JAM = 3, + FIRED = 4, +}; +} diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp new file mode 100644 index 00000000..78ed4062 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_utility { +namespace detail { + +inline auto atomic_futex_address(const std::atomic& atomic) noexcept -> uint32_t* { + return const_cast(reinterpret_cast(&atomic)); +} + +inline bool atomic_futex_wait_until_steady( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, std::memory_order order) noexcept { + + if (atomic.load(order) != old_val) + return true; + + timespec abs_time; + clock_gettime(CLOCK_MONOTONIC, &abs_time); + + const auto remaining = + std::chrono::ceil(deadline - std::chrono::steady_clock::now()); + + if (remaining <= std::chrono::nanoseconds::zero()) + return atomic.load(order) != old_val; + + const auto secs = std::chrono::duration_cast(remaining); + const auto nsecs = std::chrono::duration_cast(remaining - secs); + + abs_time.tv_sec += static_cast(secs.count()); + abs_time.tv_nsec += static_cast(nsecs.count()); + + if (abs_time.tv_nsec >= 1'000'000'000) { + abs_time.tv_sec += 1; + abs_time.tv_nsec -= 1'000'000'000; + } + + auto* const ptr = atomic_futex_address(atomic); + + while (atomic.load(order) == old_val) { + if (syscall( + SYS_futex, ptr, FUTEX_WAIT_BITSET_PRIVATE, old_val, &abs_time, nullptr, + FUTEX_BITSET_MATCH_ANY) + == 0) + continue; + + if (errno == EAGAIN || errno == EINTR) + continue; + + if (errno == ETIMEDOUT) + return atomic.load(order) != old_val; + + return atomic.load(order) != old_val; + } + + return true; +} + +} // namespace detail + +/** + * @brief Waits until a 32-bit atomic value changes or the timeout expires. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param timeout Maximum time to wait. A non-positive timeout performs a non-blocking check. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the timeout + * expires. + */ +inline bool atomic_futex_wait_for( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::duration timeout, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + if (atomic.load(order) != old_val) + return true; + + if (timeout <= std::chrono::steady_clock::duration::zero()) + return false; + + return detail::atomic_futex_wait_until_steady( + atomic, old_val, std::chrono::steady_clock::now() + timeout, order); +} + +/** + * @brief Waits until a 32-bit atomic value changes or the steady-clock deadline is reached. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param deadline Steady-clock deadline after which the wait times out. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the deadline + * is reached. + */ +inline bool atomic_futex_wait_until( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + return detail::atomic_futex_wait_until_steady(atomic, old_val, deadline, order); +} + +/** + * @brief Wakes one thread blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_one(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, 1, nullptr, + nullptr, FUTEX_BITSET_MATCH_ANY); +} + +/** + * @brief Wakes all threads blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_all(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, INT_MAX, + nullptr, nullptr, FUTEX_BITSET_MATCH_ANY); +} + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp index 620c6023..be552b7e 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp @@ -11,7 +11,7 @@ inline auto& get_tail(T& data) { return *reinterpret_cast(reinterpret_cast(&data) + sizeof(T) - sizeof(uint8_t)); } -constexpr uint8_t crc8_init = 0xff; +constexpr uint8_t crc8_init = 0xff; constexpr uint8_t crc8_table[256] = { 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, @@ -31,7 +31,7 @@ constexpr uint8_t crc8_table[256] = { 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, }; -constexpr uint16_t crc16_init = 0xffff; +constexpr uint16_t crc16_init = 0xffff; constexpr uint16_t crc16_table[256] = { 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, @@ -59,7 +59,7 @@ constexpr uint16_t crc16_table[256] = { } // namespace internal inline uint8_t calculate_crc8(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); auto result = internal::crc8_init; while (length--) result = internal::crc8_table[result ^ (*p++)]; @@ -87,7 +87,7 @@ inline void append_crc8(T& package) { } inline uint16_t calculate_crc16(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); uint16_t result = internal::crc16_init; while (length--) { result = (result >> 8) ^ internal::crc16_table[(result ^ (*p++)) & 0x00ff]; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp index 67edfce9..9e473d50 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp @@ -21,7 +21,7 @@ requires(std::is_trivially_copyable_v && std::is_trivially_destructible_v) /// @param data The data to be written to the buffer. void write(const T& data) noexcept { const uint64_t current = current_.load(std::memory_order_relaxed); - const uint64_t next = current + 1; + const uint64_t next = current + 1; std::memcpy(&buffers_[next & 1], &data, sizeof(T)); current_.store(next, std::memory_order_release); } diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp index 2c5ff84f..05af4814 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp @@ -14,11 +14,11 @@ class FpsCounter { bool count() { ++count_; - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); auto elapsed_time = now - start_; if (elapsed_time >= measurement_window_) { start_ = now; - fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); + fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); count_ = 0; return true; } @@ -33,7 +33,7 @@ class FpsCounter { std::chrono::steady_clock::time_point start_; int64_t count_ = 0; - double fps_ = 0; + double fps_ = 0; }; } // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp new file mode 100644 index 00000000..2044f4a1 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp @@ -0,0 +1,196 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_utility { + +template +class MemoryPool { + static_assert(Size > 0, "MemoryPool slot size must be greater than zero"); + static_assert(Align > 0, "MemoryPool slot alignment must be greater than zero"); + static_assert((Align & (Align - 1)) == 0, "MemoryPool slot alignment must be a power of two"); + +public: + /*! + * @brief Construct a fixed-capacity raw storage pool + * @param capacity Number of storage slots to allocate + * @note This container is not thread-safe. + */ + explicit MemoryPool(size_t capacity) + : capacity_(capacity) + , storage_(new Storage[capacity]) + , free_stack_(new size_t[capacity]) + , occupied_(new bool[capacity]{}) { + reset_free_stack(); + } + + MemoryPool(const MemoryPool&) = delete; + MemoryPool& operator=(const MemoryPool&) = delete; + MemoryPool(MemoryPool&&) = delete; + MemoryPool& operator=(MemoryPool&&) = delete; + + /*! + * @brief Destructor + * @note All allocated slots must be returned to the pool before destruction. + * Violating this lifetime contract terminates the program. + */ + ~MemoryPool() noexcept { + if (!empty()) { + std::fprintf( + stderr, + "rmcs_utility::MemoryPool %p destroyed with outstanding slots: size=%zu, " + "capacity=%zu\n", + static_cast(this), size_, capacity_); + std::fflush(stderr); + std::terminate(); + } + } + + /*! + * @brief Payload size of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_size() noexcept { return Size; } + + /*! + * @brief Alignment guarantee of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_align() noexcept { return Align; } + + /*! + * @brief Capacity of the memory pool + * @return Total number of storage slots + */ + [[nodiscard]] size_t max_size() const noexcept { return capacity_; } + + /*! + * @brief Number of currently allocated storage slots + */ + [[nodiscard]] size_t size() const noexcept { return size_; } + + /*! + * @brief Number of free storage slots remaining + */ + [[nodiscard]] size_t available() const noexcept { return capacity_ - size_; } + + /*! + * @brief Check whether the pool contains no allocated slot + */ + [[nodiscard]] bool empty() const noexcept { return size_ == 0; } + + /*! + * @brief Check whether the pool has no free slot + */ + [[nodiscard]] bool full() const noexcept { return size_ == capacity_; } + + /*! + * @brief Allocate one raw storage slot from the pool + * @return Pointer to the slot's raw storage, or nullptr if the pool is full + */ + [[nodiscard]] void* allocate() noexcept { + if (full()) + return nullptr; + + const auto index = free_stack_[top_ - 1]; + occupied_[index] = true; + --top_; + ++size_; + + return static_cast(slot_pointer(index)); + } + + /*! + * @brief Release a raw storage slot back to the pool + * @param pointer Pointer previously returned by this pool + * @return true if the slot was released, false if the pointer was invalid + * @note The pointer must refer to the start address of a live slot. + */ + bool free(void* pointer) noexcept { + const auto index = pointer_to_index(pointer); + + if (index == capacity_ || !occupied_[index]) + return false; + + occupied_[index] = false; + free_stack_[top_] = index; + ++top_; + --size_; + + return true; + } + + /*! + * @brief Check whether a pointer refers to a live slot owned by this pool + */ + [[nodiscard]] bool contains(const void* pointer) const noexcept { + const auto index = pointer_to_index(pointer); + return index != capacity_ && occupied_[index]; + } + + /*! + * @brief Release every live slot and reset the pool + * @return Number of slots that were released + */ + size_t clear() noexcept { + size_t count = 0; + + for (size_t i = 0; i < capacity_; i++) { + if (occupied_[i]) { + occupied_[i] = false; + ++count; + } + } + + size_ = 0; + reset_free_stack(); + + return count; + } + +private: + struct Storage { + alignas(Align) std::byte data[Size]; + }; + + [[nodiscard]] std::byte* slot_pointer(size_t index) noexcept { return storage_[index].data; } + + [[nodiscard]] const std::byte* slot_pointer(size_t index) const noexcept { + return storage_[index].data; + } + + [[nodiscard]] size_t pointer_to_index(const void* pointer) const noexcept { + if (pointer == nullptr) + return capacity_; + + const auto begin = reinterpret_cast(storage_.get()); + const auto end = begin + sizeof(Storage) * capacity_; + const auto target = reinterpret_cast(pointer); + + if (target < begin || target >= end) + return capacity_; + + const auto offset = target - begin; + if (offset % sizeof(Storage) != 0) + return capacity_; + + return static_cast(offset / sizeof(Storage)); + } + + void reset_free_stack() noexcept { + for (size_t i = 0; i < capacity_; i++) + free_stack_[capacity_ - 1 - i] = i; + top_ = capacity_; + } + + size_t capacity_; + std::unique_ptr storage_; + std::unique_ptr free_stack_; + std::unique_ptr occupied_; + + size_t top_{0}; + size_t size_{0}; +}; + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp index 60571f8e..f9678c60 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp @@ -10,8 +10,8 @@ namespace rmcs_utility { enum class ReceiveResult : uint8_t { - SUCCESS = 0, - TIMEOUT = 1, + SUCCESS = 0, + TIMEOUT = 1, HEADER_INVALID = 2, VERIFY_INVALID = 3 }; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp new file mode 100644 index 00000000..c2e65a19 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rmcs_utility/memory_pool.hpp" + +namespace rmcs_utility { + +template +requires std::is_nothrow_destructible_v class PooledSharedFactory { +public: + explicit PooledSharedFactory(size_t capacity) + : pool_(std::make_shared(capacity)) {} + + template + [[nodiscard]] std::shared_ptr make(Args&&... args) { + auto ptr = try_make(std::forward(args)...); + if (!ptr) + throw std::bad_alloc{}; + return ptr; + } + + template + [[nodiscard]] std::shared_ptr try_make(Args&&... args) { + void* storage = nullptr; + { + auto guard = std::scoped_lock{pool_->mutex}; + storage = pool_->allocate(); + } + + if (!storage) + return nullptr; + + try { + return std::shared_ptr( + std::construct_at(static_cast(storage), std::forward(args)...), + [pool = pool_](T* ptr) { + std::destroy_at(ptr); + auto guard = std::scoped_lock{pool->mutex}; + if (!pool->free(ptr)) { + std::fprintf( + stderr, + "rmcs_utility::PooledSharedFactory failed to return slot %p to pool " + "%p\n", + static_cast(ptr), static_cast(pool.get())); + std::fflush(stderr); + std::terminate(); + } + }); + } catch (...) { + auto guard = std::scoped_lock{pool_->mutex}; + pool_->free(storage); + throw; + } + } + + [[nodiscard]] size_t max_size() const noexcept { return pool_->max_size(); } + +private: + class PoolImpl final : public MemoryPool { + public: + using MemoryPool::MemoryPool; + + mutable std::mutex mutex; + }; + + std::shared_ptr pool_; +}; + +} // 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 82a93fba..5f9f065b 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 @@ -2,10 +2,13 @@ #include #include +#include #include +#include #include #include #include +#include #include namespace rmcs_utility { @@ -15,6 +18,179 @@ namespace rmcs_utility { template class RingBuffer { public: + template + class ReadableView; + + template + class BasicIterator { + using Buffer = std::conditional_t; + + public: + using iterator_category = std::random_access_iterator_tag; + using iterator_concept = std::random_access_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using reference = std::conditional_t; + using pointer = std::conditional_t; + + BasicIterator() = default; + BasicIterator(const BasicIterator&) = default; + BasicIterator(BasicIterator&&) = default; + BasicIterator& operator=(const BasicIterator&) = default; + BasicIterator& operator=(BasicIterator&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + BasicIterator(const BasicIterator& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , offset_(other.offset_) {} + + reference operator*() const { return *ptr(); } + pointer operator->() const { return ptr(); } + reference operator[](difference_type n) const { return *(*this + n); } + + BasicIterator& operator++() { + offset_++; + return *this; + } + + BasicIterator operator++(int) { + auto old = *this; + ++*this; + return old; + } + + BasicIterator& operator--() { + offset_--; + return *this; + } + + BasicIterator operator--(int) { + auto old = *this; + --*this; + return old; + } + + BasicIterator& operator+=(difference_type n) { + if (n >= 0) + offset_ += static_cast(n); + else + offset_ -= static_cast(-(n + 1)) + 1; + return *this; + } + + BasicIterator& operator-=(difference_type n) { + if (n >= 0) + offset_ -= static_cast(n); + else + offset_ += static_cast(-(n + 1)) + 1; + return *this; + } + + friend BasicIterator operator+(BasicIterator it, difference_type n) { + it += n; + return it; + } + + friend BasicIterator operator+(difference_type n, BasicIterator it) { + it += n; + return it; + } + + friend BasicIterator operator-(BasicIterator it, difference_type n) { + it -= n; + return it; + } + + friend difference_type operator-(BasicIterator lhs, BasicIterator rhs) { + if (lhs.offset_ >= rhs.offset_) + return static_cast(lhs.offset_ - rhs.offset_); + return -static_cast(rhs.offset_ - lhs.offset_); + } + + friend bool operator==(BasicIterator lhs, BasicIterator rhs) { + return lhs.buffer_ == rhs.buffer_ && lhs.origin_ == rhs.origin_ + && lhs.offset_ == rhs.offset_; + } + + friend std::strong_ordering operator<=>(BasicIterator lhs, BasicIterator rhs) { + return lhs.offset_ <=> rhs.offset_; + } + + private: + friend class RingBuffer; + template + friend class BasicIterator; + template + friend class ReadableView; + + BasicIterator(Buffer* buffer, size_t origin, size_t offset) + : buffer_(buffer) + , origin_(origin) + , offset_(offset) {} + + pointer ptr() const { + return std::launder( + reinterpret_cast( + buffer_->storage_[(origin_ + offset_) & buffer_->mask_].data)); + } + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t offset_ = 0; + }; + + using iterator = BasicIterator; + using const_iterator = BasicIterator; + + template + class ReadableView { + using Buffer = std::conditional_t; + + public: + using iterator = BasicIterator; + using value_type = T; + using difference_type = std::ptrdiff_t; + using size_type = size_t; + using reference = std::conditional_t; + + ReadableView() = default; + ReadableView(const ReadableView&) = default; + ReadableView(ReadableView&&) = default; + ReadableView& operator=(const ReadableView&) = default; + ReadableView& operator=(ReadableView&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + ReadableView(const ReadableView& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , size_(other.size_) {} + + iterator begin() const { return iterator{buffer_, origin_, 0}; } + iterator end() const { return iterator{buffer_, origin_, size_}; } + + [[nodiscard]] bool empty() const { return size_ == 0; } + [[nodiscard]] size_type size() const { return size_; } + + reference front() const { return *begin(); } + reference back() const { return *(end() - 1); } + reference operator[](size_type index) const { return begin()[index]; } + + private: + friend class RingBuffer; + template + friend class ReadableView; + + ReadableView(Buffer* buffer, size_t origin, size_t size) + : buffer_(buffer) + , origin_(origin) + , size_(size) {} + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t size_ = 0; + }; + /*! * @brief Construct an SPSC ring buffer * @param size Minimum capacity requested. Actual capacity is rounded up @@ -75,6 +251,31 @@ class RingBuffer { return max_size() - (in - out); } + /*! + * @brief Snapshot view of elements currently readable by the consumer + * @note Captures [out, in) once. Producer pushes do not extend the returned view. + * Consumer pops invalidate iterators to erased elements. + */ + ReadableView readable_view() noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Const snapshot view of elements currently readable by the consumer + */ + ReadableView readable_view() const noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Explicit const snapshot view for non-const buffers + */ + ReadableView const_readable_view() const noexcept { return readable_view(); } + /*! * @brief Peek the first element (consumer side) * @return Pointer to the first element, or nullptr if empty @@ -277,6 +478,24 @@ class RingBuffer { return pop_front_n(std::forward(callback_functor), 1); } + /*! + * @brief Pop readable elements before a snapshot iterator + * @return Number of elements erased from the front + * @note Consumer-only. `pos` must come from this buffer's current readable range. + */ + size_t pop_front_until(const_iterator pos) { + if (pos.buffer_ != this) + return 0; + + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + const auto count = pos.origin_ + pos.offset_ - out; + if (count > in - out) + return 0; + + return pop_front_n([](const T&) noexcept {}, count); + } + /*! * @brief Clear the buffer by consuming all elements * @return Number of elements that were erased From 5efbff5e269909af28e4993efdebf962fe148931 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 15:58:36 +0800 Subject: [PATCH 73/90] Squashed commit of the following: commit 1e99fd9191b7cc74125234dff105878e6b9aeb70 Author: Zihan Qin Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab65867b13d54858f7d0938b309ad619d3fe02 Author: Zihan Qin Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect commit b5ae6cb717b64b300086ec6cbc434b48561d450c Author: Zihan Qin Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d36a9a4e1de2e54cb452352ef2d16263ff Author: qzhhhi Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files --- .../src/rmcs_bringup/config/mecanum-hero.yaml | 161 ---- .../rmcs_bringup/config/omni-infantry.yaml | 6 +- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 4 +- .../config/steering-hero-little.yaml | 417 +++++++++ .../rmcs_bringup/config/steering-hero.yaml | 399 ++++++++ .../config/steering-infantry.yaml | 80 +- rmcs_ws/src/rmcs_core/plugins.xml | 9 + .../src/broadcaster/tf_broadcaster.cpp | 2 + .../src/broadcaster/value_broadcaster.cpp | 19 +- .../src/rmcs_core/src/controller/adrc/eso.hpp | 4 +- .../rmcs_core/src/controller/adrc/nlesf.hpp | 14 +- .../src/rmcs_core/src/controller/adrc/td.hpp | 14 +- .../chassis/chassis_climber_controller.cpp | 722 +++++++++++++++ .../controller/chassis/chassis_controller.cpp | 24 +- .../chassis/chassis_power_controller.cpp | 18 +- .../deformable_omni_wheel_controller.cpp | 35 +- .../chassis/hero_chassis_controller.cpp | 303 ++++++ .../chassis/hero_chassis_power_controller.cpp | 232 +++++ .../hero_steering_wheel_controller.cpp | 518 +++++++++++ .../chassis/omni_wheel_controller.cpp | 6 +- .../src/controller/chassis/qcp_solver.hpp | 11 +- .../chassis/steering_wheel_status.cpp | 104 +++ .../controller/gimbal/dual_yaw_controller.cpp | 75 +- .../gimbal/hero_gimbal_controller.cpp | 48 +- .../src/controller/gimbal/player_viewer.cpp | 48 +- .../gimbal/precise_two_axis_gimbal_solver.hpp | 43 +- .../pid/friction_wheel_pid_recorder.cpp | 166 ++++ .../controller/pid/matrix_pid_calculator.hpp | 4 +- .../bullet_feeder_controller_42mm.cpp | 88 +- .../hero_friction_wheel_controller.cpp | 263 ++++++ .../shooting/hero_heat_controller.cpp | 63 ++ .../controller/shooting/putter_controller.cpp | 407 ++++++++ .../controller/shooting/shooting_recorder.cpp | 205 ++++- .../src/filter/alpha_beta_angle_filter.hpp | 8 +- rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp | 428 +++++++++ .../rmcs_core/src/filter/low_pass_filter.hpp | 2 +- .../src/hardware/device/benewake.hpp | 2 +- .../rmcs_core/src/hardware/device/bmi088.hpp | 8 +- .../src/hardware/device/bmi088_ekf.hpp | 172 ++++ .../hardware/device/board_clock_lifter.hpp | 53 ++ .../rmcs_core/src/hardware/device/gy614.hpp | 8 +- .../src/hardware/device/lk_motor.hpp | 7 +- .../rmcs_core/src/hardware/omni_infantry.cpp | 87 +- .../src/hardware/steering-hero-little.cpp | 865 ++++++++++++++++++ .../referee/app/ui/deformable_infantry_ui.cpp | 6 +- .../src/rmcs_core/src/referee/app/ui/hero.cpp | 397 +++++++- .../rmcs_core/src/referee/app/ui/infantry.cpp | 4 +- .../referee/app/ui/shape/cfs_scheduler.hpp | 12 +- .../referee/app/ui/shape/red_black_tree.hpp | 36 +- .../src/referee/app/ui/shape/remote_shape.hpp | 16 +- .../src/referee/app/ui/shape/shape.hpp | 165 ++-- .../app/ui/widget/crosshair_circle.hpp | 3 +- .../ui/widget/deformable_chassis_top_view.hpp | 22 +- .../src/referee/app/ui/widget/rangefinder.hpp | 10 +- .../src/referee/app/ui/widget/status_ring.hpp | 18 +- rmcs_ws/src/rmcs_core/src/referee/command.cpp | 4 +- .../src/referee/command/interaction/ui.cpp | 14 +- rmcs_ws/src/rmcs_core/src/referee/frame.hpp | 2 +- .../rmcs_core/src/referee/status/field.hpp | 2 +- .../include/rmcs_executor/component.hpp | 402 +++++++- rmcs_ws/src/rmcs_executor/package.xml | 1 + rmcs_ws/src/rmcs_executor/src/component.cpp | 4 +- rmcs_ws/src/rmcs_executor/src/executor.hpp | 249 ++++- rmcs_ws/src/rmcs_executor/src/main.cpp | 4 +- .../include/rmcs_msgs/board_clock.hpp | 17 + .../include/rmcs_msgs/camera_frame_raw.hpp | 28 + .../include/rmcs_msgs/chassis_mode.hpp | 6 +- .../include/rmcs_msgs/full_robot_id.hpp | 64 +- .../include/rmcs_msgs/game_stage.hpp | 12 +- .../include/rmcs_msgs/imu_snapshot.hpp | 14 + .../rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp | 27 +- .../include/rmcs_msgs/robot_color.hpp | 4 +- .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 66 +- .../include/rmcs_msgs/serial_interface.hpp | 4 +- .../include/rmcs_msgs/shoot_condiction.hpp | 14 + .../include/rmcs_utility/atomic_futex.hpp | 160 ++++ .../include/rmcs_utility/crc/dji_crc.hpp | 8 +- .../include/rmcs_utility/double_buffer.hpp | 2 +- .../include/rmcs_utility/fps_counter.hpp | 6 +- .../include/rmcs_utility/memory_pool.hpp | 196 ++++ .../include/rmcs_utility/package_receive.hpp | 4 +- .../rmcs_utility/pooled_shared_factory.hpp | 76 ++ .../include/rmcs_utility/ring_buffer.hpp | 219 +++++ 83 files changed, 7641 insertions(+), 809 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml deleted file mode 100644 index 41d3572a..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/mecanum-hero.yaml +++ /dev/null @@ -1,161 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::MecanumHero -> hero_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::Hero -> referee_ui_hero - - - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_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::BulletFeederController42mm -> bullet_feeder_controller - - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> second_right_friction_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 - -hero_hardware: - ros__parameters: - 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 - -gimbal_controller: - ros__parameters: - upper_limit: -0.4598 - lower_limit: 0.4362 - -yaw_angle_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_angle_error - feedforward: -/chassis/yaw/velocity_imu - control: /gimbal/yaw/control_velocity - kp: 16.0 - ki: 0.0 - kd: 0.0 - -pitch_angle_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 16.00 - ki: 0.0 - kd: 0.0 - output_max: 10.0 - -friction_wheel_controller: - ros__parameters: - friction_wheels: - - /gimbal/first_left_friction - - /gimbal/first_right_friction - - /gimbal/second_left_friction - - /gimbal/second_right_friction - friction_velocities: - - 450.00 - - 450.00 - - 535.00 - - 535.00 - friction_soft_start_stop_time: 1.0 - -heat_controller: - ros__parameters: - heat_per_shot: 100000 - reserved_heat: 0 - -second_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_left_friction/velocity - setpoint: /gimbal/second_left_friction/control_velocity - control: /gimbal/second_left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -first_left_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_left_friction/velocity - setpoint: /gimbal/first_left_friction/control_velocity - control: /gimbal/first_left_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -first_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/first_right_friction/velocity - setpoint: /gimbal/first_right_friction/control_velocity - control: /gimbal/first_right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - kd: 0.009373434 - -second_right_friction_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/second_right_friction/velocity - setpoint: /gimbal/second_right_friction/control_velocity - control: /gimbal/second_right_friction/control_torque - kp: 0.003436926 - ki: 0.00 - 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.0 - ki: 0.001 - kd: 0.0 - integral_min: -1000.0 - integral_max: 1000.0 - -left_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_front_wheel/velocity - setpoint: /chassis/left_front_wheel/control_velocity - control: /chassis/left_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -left_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/left_back_wheel/velocity - setpoint: /chassis/left_back_wheel/control_velocity - control: /chassis/left_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_back_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_back_wheel/velocity - setpoint: /chassis/right_back_wheel/control_velocity - control: /chassis/right_back_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 - -right_front_wheel_velocity_pid_controller: - ros__parameters: - measurement: /chassis/right_front_wheel/velocity - setpoint: /chassis/right_front_wheel/control_velocity - control: /chassis/right_front_wheel/control_torque_unrestricted - kp: 0.185 - ki: 0.00 - kd: 0.00 diff --git a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml index 6364aec7..304407cf 100644 --- a/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/omni-infantry.yaml @@ -53,9 +53,9 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 40.0 - ki: 0.001 - kd: 0.01 + kp: 4.4 + ki: 0.00011 + kd: 0.0011 pitch_angle_pid_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 99c03710..cec1cd16 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -79,7 +79,7 @@ gimbal_controller: bottom_yaw_angle_kp: 15.0 bottom_yaw_angle_ki: 0.0 bottom_yaw_angle_kd: 0.0 - bottom_yaw_velocity_kp: 22.5 + bottom_yaw_velocity_kp: 2.8125 bottom_yaw_velocity_ki: 0.0 bottom_yaw_velocity_kd: 0.0 @@ -92,7 +92,7 @@ gimbal_controller: k_top_to_bottom: -1.0 - bottom_yaw_viscous_ff_gain: 0.002495 + bottom_yaw_viscous_ff_gain: 0.000311875 # bottom_yaw_coulomb_ff_gain: 0.457343 # bottom_yaw_coulomb_ff_tanh_gain: 100.0 top_yaw_viscous_ff_gain: 0.231 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml new file mode 100644 index 00000000..53970767 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero-little.yaml @@ -0,0 +1,417 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHeroLittle -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + +hero_hardware: + ros__parameters: + board_serial_top_board: "AF-BFF7-0B6F-46A5-2B4B-AA20-89C2-E180-64B9" + serial_bottom_rmcs_board: "AF-60BB-7484-FA24-F3FC-399B-454D-22FA-1B1D" + bottom_yaw_motor_zero_point: 35078 + pitch_motor_zero_point: 57241 + top_yaw_motor_zero_point: 48537 + viewer_motor_zero_point: 31940 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 60480 #39045 + left_front_zero_point: 5799 + right_front_zero_point: 3700 + left_back_zero_point: 7862 + right_back_zero_point: 1608 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/top_yaw/control_angle + - /chassis/climber/left_front_motor/torque + - /chassis/climber/right_front_motor/torque + - /chassis/left_front_steering/torque + - /chassis/left_back_steering/torque + - /chassis/right_front_steering/torque + - /chassis/right_back_steering/torque + # - /shoot/heat12707 + # - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + # - /chassis/control_power_limit + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque + # - /gimbal/bottom_yaw/torque + # - /gimbal/bottom_yaw/angle + # - /gimbal/top_yaw/angle + # - /gimbal/pitch/angle + # - /chassis/power + - /chassis/supercap/voltage + # - /chassis/control_power_limit + # - /referee/chassis/power_limit + # - /referee/chassis/buffer_energy + # - /chassis/climber/front/control_power_limit + # - /chassis/climber/front/power_demand_estimate + # - /chassis/climber/front/actual_power_estimate + # - /chassis/steering_wheel/actual_power_estimate + - /gimbal/bullet_feeder/torque + - /gimbal/bullet_feeder/control_torque + - /gimbal/putter/angle + - /gimbal/putter/velocity + - /gimbal/putter/torque + - /gimbal/putter/control_torque + - /gimbal/bullet_feeder/velocity + - /gimbal/bullet_feeder/angle + + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 50.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 130.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.668 + lower_limit: 0.320 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 14.0 #30.2 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 9.37 #11.0 + top_yaw_velocity_ki: 0.00033 #0.00029 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 10.0 #18.4 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.00 #2.81 + bottom_yaw_velocity_ki: 0.000071 #0.00028 + bottom_yaw_velocity_kd: 0.0 + bottom_yaw_velocity_integral_min: -2500.0 + bottom_yaw_velocity_integral_max: 2500.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 18.6 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 16.05 + ki: 0.00014 + kd: 0.0 + integral_min: -2500.0 + integral_max: 2500.0 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.415996 + lower_limit: -0.066441 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 4.00 + ki: 0.00 + kd: 0.50 + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.0 + bullet_feeder_velocity_ki: 0.1 #1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 6.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.6 + putter_return_velocity_kp: 0.0025 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: true + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/second_left_friction + - /gimbal/second_right_friction + - /gimbal/first_left_friction + - /gimbal/first_right_friction + friction_velocities_profile_0: + - 380.0 + - 380.0 + - 545.0 + - 545.0 + friction_velocities_profile_1: + - 535.0 + - 535.0 + - 595.0 + - 595.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 1: trigger, 2: timing + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00016 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00016 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.006 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.286378 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + wheel_radius: 0.055 + friction_coefficient: 0.6 + 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: 1 + 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. + pitch_error: 0. + 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: 2 + +hero_auto_aim_bridge: + ros__parameters: + config_file: "configs/standard3.yaml" + bullet_speed_fallback: 11.4 + result_timeout: 0.1 # 0.08 + debug: false + + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +pitch_static_torque_test_controller: + ros__parameters: + target: /gimbal/pitch + + interval_angle: 0.05 + wait_time: 1.5 + border_clip: 0.05 + + position_kp: 12.0 + position_ki: 0.0 + position_kd: 0.0 + + velocity_kp: 3.2 + velocity_ki: 0.0005 + velocity_kd: 0.0 + + velocity_integral_min: -2500.0 + velocity_integral_max: 2500.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 15.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml new file mode 100644 index 00000000..a6e01d35 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/steering-hero.yaml @@ -0,0 +1,399 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::SteeringHero -> hero_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Hero -> referee_ui_hero + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::controller::gimbal::HeroGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::gimbal::PlayerViewer -> gimbal_player_viewer_controller + - rmcs_core::controller::pid::ErrorPidController -> viewer_angle_pid_controller + + - rmcs_core::controller::shooting::HeroFrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeroHeatController -> heat_controller + - rmcs_core::controller::shooting::PutterController -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller + # - rmcs_core::controller::shooting::ShootingRecorder -> shooting_recorder + + - rmcs_core::controller::chassis::SteeringWheelStatus -> steering_wheel_status + - rmcs_core::controller::chassis::HeroChassisController -> chassis_controller + - rmcs_core::controller::chassis::HeroChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::HeroSteeringWheelController -> steering_wheel_controller + - rmcs_core::controller::chassis::ChassisClimberController -> climber_controller + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - sp_vision_25::bridge::HeroAutoAimBridge -> hero_auto_aim_bridge + + # - rmcs_core::controller::identification::SweptFrequencyController -> pitch_swept_frequency_controller + # - rmcs_core::controller::identification::StaticTorqueTestController -> pitch_static_torque_test_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> top_yaw_swept_frequency_controller + # - rmcs_core::controller::identification::SweptFrequencyController -> bottom_yaw_swept_frequency_controller + + + + +hero_hardware: + ros__parameters: + board_serial_top_board: "D4-2BCA-2E47-76CD-23BC-0B78-684B" + board_serial_bottom_board_one: "D4-7973-19A9-EA40-4A3E-306F-10F9" + board_serial_bottom_board_two: "D4-3674-7174-8768-879E-E44A-3931" + bottom_yaw_motor_zero_point: 37424 + pitch_motor_zero_point: 14916 + top_yaw_motor_zero_point: 33076 + viewer_motor_zero_point: 3030 + external_imu_port: /dev/ttyUSB0 + bullet_feeder_motor_zero_point: 11865 + left_front_zero_point: 4398 + right_front_zero_point: 1081 + left_back_zero_point: 3794 + right_back_zero_point: 5839 + + + + +value_broadcaster: + ros__parameters: + forward_list: + # - /shoot/heat + - /chassis/power + # - /referee/chassis/power + # - /referee/chassis/power_limit + - /chassis/control_power_limit + - /chassis/climber/front/control_power_limit + - /chassis/climber/front/power_demand_estimate + - /chassis/climber/front/actual_power_estimate + - /chassis/steering_wheel/actual_power_estimate + # - /chassis/supercap/voltage + # - /gimbal/putter/velocity + - /gimbal/first_left_friction/velocity + - /gimbal/first_right_friction/velocity + - /gimbal/second_left_friction/velocity + - /gimbal/second_right_friction/velocity + # - /gimbal/first_left_friction/control_torque + # - /gimbal/first_second_friction/control_torque + # - /gimbal/second_left_friction/control_torque + # - /gimbal/second_right_friction/control_torque + # - /gimbal/bottom_yaw/torque + - /gimbal/bottom_yaw/control_angle_shift + - /gimbal/bottom_yaw/angle + - /gimbal/top_yaw/angle + - /gimbal/pitch/angle + + # - /gimbal/pitch/velocity_imu + # - /gimbal/pitch/control_velocity + # - /gimbal/pitch/control_torque + + - /gimbal/auto_aim/plan_yaw + - /gimbal/auto_aim/plan_pitch + +climber_controller: + ros__parameters: + front_climber_velocity: 20.0 + back_climber_velocity: 30.0 + auto_climb_support_retract_velocity_fast: 60.0 + auto_climb_support_retract_velocity_slow: 20.0 + auto_climb_approach_chassis_velocity: 2.0 + auto_climb_support_deploy_chassis_velocity: 0.3 + auto_climb_support_retract_chassis_velocity: 0.3 + auto_climb_dash_chassis_velocity: 3.0 + first_stair_dash_leveled_pitch_threshold: 0.05 + second_stair_dash_leveled_pitch_threshold: -0.09 + sync_coefficient: 0.2 + first_stair_approach_pitch: 0.517 + second_stair_approach_pitch: 0.365 + front_kp: 1.0 + front_ki: 0.0 + front_kd: 0.5 + front_power_estimate_bias: 0.0 + front_power_estimate_k_tau2: 1.0 + front_power_estimate_k_mech: 1.0 + back_kp: 0.5 + back_ki: 0.0 + back_kd: 0.0 + +chassis_power_controller: + ros__parameters: + front_climber_power_limit_max: 60.0 + drive_power_limit_floor: 50.0 + auto_climb_min_control_power_limit: 150.0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.688 + lower_limit: 0.357 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 5.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 10.0 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + top_yaw_velocity_integral_min: -2500.0 + top_yaw_velocity_integral_max: 2500.0 + bottom_yaw_angle_kp: 13.9 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 2.0 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 0.0 + +# dual_yaw_controller: +# ros__parameters: +# top_yaw_angle_kp: 24.5 +# top_yaw_angle_ki: 0.0 +# top_yaw_angle_kd: 0.0 +# top_yaw_velocity_kp: 77.4 +# top_yaw_velocity_ki: 0.004 +# top_yaw_velocity_kd: 1.0 +# bottom_yaw_angle_kp: 8.6 +# bottom_yaw_angle_ki: 0.0 +# bottom_yaw_angle_kd: 0.0 +# bottom_yaw_velocity_kp: 25.85 +# bottom_yaw_velocity_ki: 0.0 +# bottom_yaw_velocity_kd: 50.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 12.00 #45.00 + ki: 0.00 + kd: 0.0 #1.00 + +gimbal_player_viewer_controller: + ros__parameters: + upper_limit: 0.68 + lower_limit: 1.17 + +viewer_angle_pid_controller: + ros__parameters: + measurement: /gimbal/player_viewer/control_angle_error + control: /gimbal/player_viewer/control_velocity + kp: 17.00 + ki: 0.00 + kd: 2.00 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/first_left_friction + - /gimbal/first_right_friction + - /gimbal/second_left_friction + - /gimbal/second_right_friction + friction_velocities_profile_0: + - 368.00 + - 368.00 + - 532.00 + - 532.00 + friction_velocities_profile_1: + - 525.0 + - 525.0 + - 585.0 + - 585.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 100000 + reserved_heat: 0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 4 + aim_velocity: 11.8 + log_mode: 1 # 1: trigger, 2: timing + +bullet_feeder_controller: + ros__parameters: + bullet_feeder_velocity_kp: 5.5 + bullet_feeder_velocity_ki: 1.1 + bullet_feeder_velocity_kd: 0.0 + bullet_feeder_velocity_integral_min: 0.0 + bullet_feeder_velocity_integral_max: 60.0 + bullet_feeder_angle_kp: 5.0 + bullet_feeder_angle_ki: 0.0 + bullet_feeder_angle_kd: 1.0 + putter_return_velocity_kp: 0.0015 + putter_return_velocity_ki: 0.00005 + putter_return_velocity_kd: 0.0 + putter_return_velocity_integral_min: -0.03 + putter_return_velocity_integral_max: 0.0 + photoelectric_allow: false + +first_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_left_friction/velocity + setpoint: /gimbal/first_left_friction/control_velocity + control: /gimbal/first_left_friction/control_torque + kp: 0.0005 + ki: 0.00 + kd: 0.00004 + +first_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/first_right_friction/velocity + setpoint: /gimbal/first_right_friction/control_velocity + control: /gimbal/first_right_friction/control_torque + kp: 0.0005 + ki: 0.00 + kd: 0.00004 + +second_left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_left_friction/velocity + setpoint: /gimbal/second_left_friction/control_velocity + control: /gimbal/second_left_friction/control_torque + kp: 0.0009 + ki: 0.00 + kd: 0.00008 + +second_right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/second_right_friction/velocity + setpoint: /gimbal/second_right_friction/control_velocity + control: /gimbal/second_right_friction/control_torque + kp: 0.0009 + ki: 0.00 + kd: 0.00008 + +steering_wheel_status: + ros__parameters: + vehicle_radius: 0.318198 + wheel_radius: 0.055 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.318198 + wheel_radius: 0.055 + friction_coefficient: 0.6 + 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: 1 + 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. + pitch_error: 0. + 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: 2 + +# hero_auto_aim_bridge: +# ros__parameters: +# config_file: "configs/standard3.yaml" +# bullet_speed_fallback: 11.7 +# result_timeout: 0.1 # 0.08 +# debug: false + +pitch_swept_frequency_controller: + ros__parameters: + target: /gimbal/pitch + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 10.0 + + pid: true + setpoint: 0.0 + position_kp: 20.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 1.65 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +top_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/top_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 10.0 + duration: 60.0 + amplitude: 20.0 + + pid: true + setpoint: 0.0 + position_kp: 10.0 + position_ki: 0.0 + position_kd: 0.0 + velocity_kp: 3.0 + velocity_ki: 0.0 + velocity_kd: 0.0 + dc_offset: 0.0 + +bottom_yaw_swept_frequency_controller: + ros__parameters: + target: /gimbal/bottom_yaw + + sweep: true + logarithmic: true + start_freq: 0.1 + end_freq: 4.0 + duration: 80.0 + amplitude: 2.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml index d650d044..2e344d14 100644 --- a/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/steering-infantry.yaml @@ -2,18 +2,18 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::SteeringInfantry -> steeringInfantry_hardware + - rmcs_core::hardware::SteeringInfantry -> 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::referee::app::ui::Infantry -> referee_ui_infantry + - rmcs_core::referee::Command -> referee_command - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -26,37 +26,47 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller -steeringInfantry_hardware: +infantry_hardware: ros__parameters: - 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 - left_back_zero_point: 5770 - right_back_zero_point: 2380 - right_front_zero_point: 1705 + board_serial_top_board: "AF-B4E5-CE0E-4342-FF2C-F9E2-DE47-2D85-9B75" + board_serial_bottom_board: "AF-EEF5-24BA-6675-F1B1-C797-50AF-869C-870E" + yaw_motor_zero_point: 20993 + pitch_motor_zero_point: 18874 + left_front_zero_point: 1695 + right_front_zero_point: 5068 + left_back_zero_point: 7870 + right_back_zero_point: 3141 + gimbal_controller: ros__parameters: - upper_limit: -0.39518 - lower_limit: 0.36 + upper_limit: -0.54 + lower_limit: 0.274 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 15.0 + ki: 0.0 + kd: 1.5 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 16.0 + kp: 20.0 ki: 0.0 - kd: 0.0 + kd: 0.9 -pitch_angle_pid_controller: +yaw_velocity_pid_controller: ros__parameters: - measurement: /gimbal/pitch/control_angle_error - control: /gimbal/pitch/control_velocity - kp: 10.00 - ki: 0.0 - kd: 0.0 + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 2.5 + ki: 0.00 + kd: 0.6 friction_wheel_controller: ros__parameters: @@ -64,9 +74,9 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 - friction_soft_start_stop_time: 1.0 + - 590.0 + - 590.0 + friction_soft_start_stop_time: 0.3 heat_controller: ros__parameters: @@ -75,8 +85,8 @@ heat_controller: bullet_feeder_controller: ros__parameters: - bullets_per_feeder_turn: 10.0 - shot_frequency: 24.0 + bullets_per_feeder_turn: 9.0 + shot_frequency: 27.0 safe_shot_frequency: 10.0 eject_frequency: 15.0 eject_time: 0.15 @@ -86,8 +96,8 @@ bullet_feeder_controller: shooting_recorder: ros__parameters: - friction_wheel_count: 4 - log_mode: 2 # 1: trigger, 2: timing + friction_wheel_count: 2 + log_mode: 1 left_friction_velocity_pid_controller: ros__parameters: @@ -112,15 +122,15 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 0.283 + kp: 0.7 ki: 0.0 kd: 0.0 steering_wheel_controller: ros__parameters: - mess: 19.0 - moment_of_inertia: 1.0 - vehicle_radius: 0.24678 + mess: 22.0 + moment_of_inertia: 1.08 + vehicle_radius: 0.28284271247462 wheel_radius: 0.055 friction_coefficient: 0.6 k1: 2.958580e+00 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1438f8a8..8d30c5cf 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,6 +1,7 @@ + @@ -9,9 +10,14 @@ + + + + + @@ -24,9 +30,12 @@ + + + diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp index 40dfe60a..f3d13c70 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp @@ -26,6 +26,8 @@ class TfBroadcaster } ~TfBroadcaster() = default; + void before_updating() override { fast_tf::rcl::broadcast_all(*tf_); } + void update() override { using namespace std::chrono_literals; if (*update_count_ == 0) diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp index c9b9a584..ec612065 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -14,15 +14,16 @@ class ValueBroadcaster : Node{get_component_name()} { declare_parameter>("forward_list", std::vector{}); parameter_subscriber_ = std::make_unique(this); - parameter_callback_ = parameter_subscriber_->add_parameter_callback( + parameter_callback_ = parameter_subscriber_->add_parameter_callback( "forward_list", [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); } - void before_pairing( - const std::map& output_map) override { - for (const auto& [name, type] : output_map) { - if (type == typeid(double)) { + void before_pairing(const OutputInfoMap& output_map) override { + for (const auto& [name, output] : output_map) { + if (output.kind != rmcs_executor::InterfaceKind::Normal) + continue; + if (output.type.get() == typeid(double)) { forward_units_.emplace( name, std::make_unique>(this, name)); @@ -52,7 +53,7 @@ class ValueBroadcaster "unsupported or the output does not exist.", name.c_str()); } else { - auto& unit = iter->second; + auto& unit = iter->second; unit->active = true; } } @@ -70,8 +71,8 @@ class ValueBroadcaster virtual ~BasicForwarderUnit() {} virtual void activate(rclcpp::Node* node) = 0; - virtual void update() = 0; - virtual void deactivate() = 0; + virtual void update() = 0; + virtual void deactivate() = 0; bool active; }; @@ -118,4 +119,4 @@ class ValueBroadcaster #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp index e7d9d6ee..0d79b300 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/eso.hpp @@ -66,9 +66,7 @@ class ESO { } private: - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp index c3bd6725..ad21b4fb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/nlesf.hpp @@ -47,22 +47,22 @@ class NLESF { constexpr double kEps = 1e-9; const double safe_b0 = (std::fabs(b0) < kEps) ? ((b0 >= 0.0) ? kEps : -kEps) : b0; - const double u0 = cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) - + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); + const double u0 = + cfg_.k1 * fal(e1, cfg_.alpha1, cfg_.delta) + cfg_.k2 * fal(e2, cfg_.alpha2, cfg_.delta); const double u = clamp((u0 - z3) / safe_b0, cfg_.u_min, cfg_.u_max); return Output{u0, u}; } private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } void sanitize_config() { constexpr double kEps = 1e-9; diff --git a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp index 6b14ce5f..390d5711 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/adrc/td.hpp @@ -57,14 +57,14 @@ class TD { private: static double sign(double x) { - if (x > 0.0) return 1.0; - if (x < 0.0) return -1.0; + if (x > 0.0) + return 1.0; + if (x < 0.0) + return -1.0; return 0.0; } - static double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(x, hi)); - } + static double clamp(double x, double lo, double hi) { return std::max(lo, std::min(x, hi)); } static double fhan(double x1_minus_v, double x2, double r, double h) { const double d = r * h * h; @@ -72,9 +72,7 @@ class TD { const double y = x1_minus_v + a0; const double a1 = std::sqrt(d * (d + 8.0 * std::fabs(y))); - const double a = (std::fabs(y) > d) - ? (a0 + sign(y) * (a1 - d) * 0.5) - : (a0 + y); + const double a = (std::fabs(y) > d) ? (a0 + sign(y) * (a1 - d) * 0.5) : (a0 + y); if (std::fabs(a) <= d) { return -r * a / d; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp new file mode 100644 index 00000000..cfd1a669 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_climber_controller.cpp @@ -0,0 +1,722 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/switch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +namespace { +double estimate_front_power( + double left_torque, double right_torque, double left_velocity, double right_velocity, + double bias, double k_tau2, double k_mech) { + + if (!std::isfinite(left_torque) || !std::isfinite(right_torque)) + return 0.0; + + return bias + k_tau2 * (std::pow(left_torque, 2) + std::pow(right_torque, 2)) + + k_mech + * (std::abs(left_torque * left_velocity) + std::abs(right_torque * right_velocity)); +} +} // namespace + +enum class AutoClimbState { IDLE, ALIGN, APPROACH, SUPPORT_DEPLOY, DASH, SUPPORT_RETRACT }; + +class ChassisClimberController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ChassisClimberController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) + , front_velocity_pid_calculator_( + get_parameter("front_kp").as_double(), get_parameter("front_ki").as_double(), + get_parameter("front_kd").as_double()) + , back_velocity_pid_calculator_( + get_parameter("back_kp").as_double(), get_parameter("back_ki").as_double(), + get_parameter("back_kd").as_double()) { + + track_velocity_max_ = get_parameter("front_climber_velocity").as_double(); + climber_back_control_velocity_abs_ = get_parameter("back_climber_velocity").as_double(); + auto_climb_support_retract_velocity_fast_abs_ = + get_parameter("auto_climb_support_retract_velocity_fast").as_double(); + auto_climb_support_retract_velocity_slow_abs_ = + get_parameter("auto_climb_support_retract_velocity_slow").as_double(); + auto_climb_approach_chassis_velocity_ = + get_parameter("auto_climb_approach_chassis_velocity").as_double(); + auto_climb_support_deploy_chassis_velocity_ = + get_parameter("auto_climb_support_deploy_chassis_velocity").as_double(); + auto_climb_support_retract_chassis_velocity_ = + get_parameter("auto_climb_support_retract_chassis_velocity").as_double(); + auto_climb_dash_chassis_velocity_ = + get_parameter("auto_climb_dash_chassis_velocity").as_double(); + first_stair_dash_leveled_pitch_threshold_ = + get_parameter("first_stair_dash_leveled_pitch_threshold").as_double(); + second_stair_dash_leveled_pitch_threshold_ = + get_parameter("second_stair_dash_leveled_pitch_threshold").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + first_stair_approach_pitch_ = get_parameter("first_stair_approach_pitch").as_double(); + second_stair_approach_pitch_ = get_parameter("second_stair_approach_pitch").as_double(); + front_power_estimate_bias_ = get_parameter("front_power_estimate_bias").as_double(); + front_power_estimate_k_tau2_ = get_parameter("front_power_estimate_k_tau2").as_double(); + front_power_estimate_k_mech_ = get_parameter("front_power_estimate_k_mech").as_double(); + + register_output( + "/chassis/climber/left_front_motor/requested_control_torque", + climber_front_left_requested_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/requested_control_torque", + climber_front_right_requested_control_torque_, nan_); + register_output( + "/chassis/climber/left_back_motor/control_torque", climber_back_left_control_torque_, + nan_); + register_output( + "/chassis/climber/right_back_motor/control_torque", climber_back_right_control_torque_, + nan_); + register_output("/chassis/climbing_forward_velocity", climbing_forward_velocity_, nan_); + register_output("/chassis/climber/auto_climb_active", auto_climb_active_, false); + register_output( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_output( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, 0.0); + + register_input("/chassis/climber/left_front_motor/velocity", climber_front_left_velocity_); + register_input( + "/chassis/climber/right_front_motor/velocity", climber_front_right_velocity_); + register_input("/chassis/climber/left_back_motor/velocity", climber_back_left_velocity_); + register_input("/chassis/climber/right_back_motor/velocity", climber_back_right_velocity_); + + register_input("/chassis/climber/left_back_motor/torque", climber_back_left_torque_); + register_input("/chassis/climber/right_back_motor/torque", climber_back_right_torque_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); + register_input("/chassis/pitch_imu", chassis_pitch_imu_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + + front_power_limiter_ = create_partner_component( + get_component_name() + "_front_power_limiter", front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void update() override { + using namespace rmcs_msgs; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob_switch = *rotary_knob_switch_; + + bool rotary_knob_to_down = + (last_rotary_knob_switch_ != Switch::DOWN && rotary_knob_switch == Switch::DOWN); + bool rotary_knob_from_down = + (last_rotary_knob_switch_ == Switch::DOWN && rotary_knob_switch != Switch::DOWN); + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + } else { + handle_auto_climb_requests( + (!last_keyboard_.g && keyboard.g) || rotary_knob_to_down, rotary_knob_from_down, + rotary_knob_switch); + + if (auto_climb_state_ != AutoClimbState::IDLE) { + stop_manual_support(); + apply_climb_control(update_auto_climb_control()); + } else { + apply_climb_control(update_manual_support_control(keyboard)); + } + } + + last_keyboard_ = keyboard; + last_rotary_knob_switch_ = rotary_knob_switch; + } + +private: + struct AutoClimbControl { + double front_track_velocity = nan_; + double back_climber_velocity = nan_; + double override_chassis_vx = nan_; + }; + + class ChassisClimberFrontPowerLimiter : public rmcs_executor::Component { + public: + ChassisClimberFrontPowerLimiter(double bias, double k_tau2, double k_mech) + : front_power_estimate_bias_(bias) + , front_power_estimate_k_tau2_(k_tau2) + , front_power_estimate_k_mech_(k_mech) { + register_input( + "/chassis/climber/left_front_motor/requested_control_torque", + left_requested_control_torque_); + register_input( + "/chassis/climber/right_front_motor/requested_control_torque", + right_requested_control_torque_); + register_input("/chassis/climber/left_front_motor/velocity", left_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_velocity_); + register_input("/chassis/climber/left_front_motor/max_torque", left_max_torque_); + register_input("/chassis/climber/right_front_motor/max_torque", right_max_torque_); + register_input("/chassis/climber/front/control_power_limit", control_power_limit_); + + register_output( + "/chassis/climber/left_front_motor/control_torque", left_control_torque_, nan_); + register_output( + "/chassis/climber/right_front_motor/control_torque", right_control_torque_, nan_); + register_output( + "/chassis/climber/front/actual_power_estimate", actual_power_estimate_, 0.0); + } + + void update() override { + const double left_requested = *left_requested_control_torque_; + const double right_requested = *right_requested_control_torque_; + + if (!std::isfinite(left_requested) || !std::isfinite(right_requested)) { + *left_control_torque_ = nan_; + *right_control_torque_ = nan_; + *actual_power_estimate_ = 0.0; + return; + } + + if (*control_power_limit_ <= 0.0) { + *left_control_torque_ = 0.0; + *right_control_torque_ = 0.0; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, + *right_velocity_, front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + return; + } + + const double left_torque = + std::clamp(left_requested, -*left_max_torque_, *left_max_torque_); + const double right_torque = + std::clamp(right_requested, -*right_max_torque_, *right_max_torque_); + + const double estimated_power = estimate_front_power( + left_torque, right_torque, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + + if (estimated_power <= *control_power_limit_ || estimated_power <= 0.0) { + *left_control_torque_ = left_torque; + *right_control_torque_ = right_torque; + *actual_power_estimate_ = estimated_power; + return; + } + + const double scale = std::clamp(*control_power_limit_ / estimated_power, 0.0, 1.0); + *left_control_torque_ = left_torque * scale; + *right_control_torque_ = right_torque * scale; + *actual_power_estimate_ = estimate_front_power( + *left_control_torque_, *right_control_torque_, *left_velocity_, *right_velocity_, + front_power_estimate_bias_, front_power_estimate_k_tau2_, + front_power_estimate_k_mech_); + } + + private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + InputInterface left_requested_control_torque_; + InputInterface right_requested_control_torque_; + InputInterface left_velocity_; + InputInterface right_velocity_; + InputInterface left_max_torque_; + InputInterface right_max_torque_; + InputInterface control_power_limit_; + + OutputInterface left_control_torque_; + OutputInterface right_control_torque_; + OutputInterface actual_power_estimate_; + }; + + void handle_auto_climb_requests( + bool start_requested, bool abort_by_rotary, rmcs_msgs::Switch rotary_knob_switch) { + + if (start_requested) { + if (auto_climb_state_ == AutoClimbState::IDLE) { + start_auto_climb( + rotary_knob_switch == rmcs_msgs::Switch::UP ? "Rotary Knob" : "Keyboard G"); + } else { + abort_auto_climb("toggled again"); + } + } else if (abort_by_rotary && auto_climb_state_ != AutoClimbState::IDLE) { + abort_auto_climb("rotary knob left UP"); + } + } + + void start_auto_climb(const char* source) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = false; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + enter_auto_climb_state(AutoClimbState::ALIGN); + + RCLCPP_INFO(logger_, "Auto climb started by %s. Entering ALIGN.", source); + } + + void abort_auto_climb(const char* reason) { + stop_auto_climb(); + start_back_climber_retract("Auto climb exit"); + RCLCPP_INFO(logger_, "Auto climb aborted (%s).", reason); + } + + AutoClimbControl update_auto_climb_control() { + if (auto_climb_state_ == AutoClimbState::IDLE) + return {}; + + auto_climb_timer_++; + + switch (auto_climb_state_) { + case AutoClimbState::IDLE: return {}; + case AutoClimbState::ALIGN: return update_auto_climb_align(); + case AutoClimbState::APPROACH: return update_auto_climb_approach(); + case AutoClimbState::SUPPORT_DEPLOY: return update_auto_climb_support_deploy(); + case AutoClimbState::DASH: return update_auto_climb_dash(); + case AutoClimbState::SUPPORT_RETRACT: return update_auto_climb_support_retract(); + } + + return {}; + } + + AutoClimbControl update_manual_support_control(const rmcs_msgs::Keyboard& keyboard) { + AutoClimbControl control; + + if (keyboard.b) { + back_climber_zero_velocity_hold_ = false; + control.back_climber_velocity = climber_back_control_velocity_abs_; + return control; + } + + if (last_keyboard_.b) { + start_back_climber_retract("Manual support"); + } + + if (!manual_support_retracting_) { + if (back_climber_zero_velocity_hold_) + control.back_climber_velocity = 0.0; + return control; + } + + if (back_climber_recover_count > 1200) { + control.back_climber_velocity = -auto_climb_support_retract_velocity_slow_abs_; + } else { + control.back_climber_velocity = -auto_climb_support_retract_velocity_fast_abs_; + } + + if (is_back_climber_blocked()) + manual_support_retract_block_count_++; + else + manual_support_retract_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "MANUAL_SUPPORT_RETRACT: blocked_ticks=%d", + manual_support_retract_block_count_); + + if (manual_support_retract_block_count_ >= kManualSupportRetractConfirmTicks) { + stop_manual_support(); + back_climber_zero_velocity_hold_ = true; + control.back_climber_velocity = 0.0; + RCLCPP_INFO(logger_, "Manual support retract completed."); + } + + return control; + } + + AutoClimbControl update_auto_climb_align() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = -5.0, + .override_chassis_vx = 0.0, + }; + + double gimbal_yaw_angle_error = *gimbal_yaw_angle_error_; + if (gimbal_yaw_angle_error < 0) + gimbal_yaw_angle_error += 2 * std::numbers::pi; + + double err = gimbal_yaw_angle_error + *gimbal_yaw_angle_; + while (err >= std::numbers::pi) + err -= 2 * std::numbers::pi; + while (err < -std::numbers::pi) + err += 2 * std::numbers::pi; + + double yaw_velocity = *gimbal_yaw_velocity_imu_; + bool is_aligned = std::abs(err) < kAutoClimbAlignThreshold; + bool is_stable = std::abs(yaw_velocity) < kAutoClimbAlignVelocityThreshold; + + if (is_aligned && is_stable) + auto_climb_align_stable_count_++; + else + auto_climb_align_stable_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "ALIGN: err=%.3f, yaw_velocity=%.3f, stable_ticks=%d", err, + yaw_velocity, auto_climb_align_stable_count_); + + if (auto_climb_align_stable_count_ >= kAutoClimbAlignConfirmTicks) { + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO(logger_, "Chassis aligned. Entering APPROACH."); + } + + return control; + } + + AutoClimbControl update_auto_climb_approach() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = -5.0, + .override_chassis_vx = auto_climb_approach_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double target_pitch = auto_climb_stair_index_ == 0 ? first_stair_approach_pitch_ + : second_stair_approach_pitch_; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "APPROACH (step %d): pitch=%.3f, target>%.3f", + auto_climb_stair_index_ + 1, pitch, target_pitch); + + if (pitch > target_pitch) { + enter_auto_climb_state(AutoClimbState::SUPPORT_DEPLOY); + RCLCPP_INFO( + logger_, "Auto climb entering SUPPORT_DEPLOY (step %d).", + auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_support_deploy() { + AutoClimbControl control{ + .front_track_velocity = 0.0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_support_deploy_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_DEPLOY (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportConfirmTicks) { + enter_auto_climb_state(AutoClimbState::DASH); + RCLCPP_INFO( + logger_, "Auto climb entering DASH (step %d).", auto_climb_stair_index_ + 1); + } + + return control; + } + + AutoClimbControl update_auto_climb_dash() { + AutoClimbControl control{ + .front_track_velocity = 0, + .back_climber_velocity = climber_back_control_velocity_abs_, + .override_chassis_vx = auto_climb_dash_chassis_velocity_, + }; + + double pitch = *chassis_pitch_imu_; + double leveled_pitch_threshold = auto_climb_stair_index_ == 0 + ? first_stair_dash_leveled_pitch_threshold_ + : second_stair_dash_leveled_pitch_threshold_; + bool is_leveled = + pitch < leveled_pitch_threshold && auto_climb_timer_ > kAutoClimbDashMinTicks; + bool timeout = auto_climb_timer_ > kAutoClimbDashTimeoutTicks; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "DASH (step %d): pitch=%.3f, threshold=%.3f, timer=%d", + auto_climb_stair_index_ + 1, pitch, leveled_pitch_threshold, auto_climb_timer_); + + if (is_leveled || timeout) { + enter_auto_climb_state(AutoClimbState::SUPPORT_RETRACT); + + if (timeout) { + RCLCPP_WARN( + logger_, "Auto climb DASH timeout on step %d. Entering SUPPORT_RETRACT.", + auto_climb_stair_index_ + 1); + } else { + RCLCPP_INFO( + logger_, "Auto climb reached platform on step %d.", + auto_climb_stair_index_ + 1); + } + } + + return control; + } + + AutoClimbControl update_auto_climb_support_retract() { + AutoClimbControl control{ + .front_track_velocity = track_velocity_max_, + .back_climber_velocity = back_climber_recover_count <= 1200 + ? -auto_climb_support_retract_velocity_fast_abs_ + : -auto_climb_support_retract_velocity_slow_abs_, + .override_chassis_vx = auto_climb_support_retract_chassis_velocity_, + }; + + if (is_back_climber_blocked()) + auto_climb_support_block_count_++; + else + auto_climb_support_block_count_ = 0; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 500, "SUPPORT_RETRACT (step %d): blocked_ticks=%d", + auto_climb_stair_index_ + 1, auto_climb_support_block_count_); + + if (auto_climb_support_block_count_ >= kAutoClimbSupportRetractConfirmTicks) { + bool has_next_stair = auto_climb_stair_index_ + 1 < kAutoClimbMaxStairs; + + if (has_next_stair) { + auto_climb_stair_index_++; + enter_auto_climb_state(AutoClimbState::APPROACH); + RCLCPP_INFO( + logger_, "Auto climb continuing to step %d.", auto_climb_stair_index_ + 1); + } else { + int finished_steps = auto_climb_stair_index_ + 1; + stop_auto_climb(); + back_climber_zero_velocity_hold_ = true; + control.front_track_velocity = nan_; + control.back_climber_velocity = 0.0; + control.override_chassis_vx = nan_; + RCLCPP_INFO(logger_, "Auto climb completed (finished %d steps).", finished_steps); + } + } + + return control; + } + + void apply_climb_control(const AutoClimbControl& control) { + *climbing_forward_velocity_ = control.override_chassis_vx; + *auto_climb_active_ = auto_climb_state_ != AutoClimbState::IDLE; + if (back_climber_recover_count != 0) { + back_climber_recover_count--; + } + + dual_motor_sync_control( + control.front_track_velocity, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_velocity_pid_calculator_, + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_); + + dual_motor_sync_control( + control.back_climber_velocity, *climber_back_left_velocity_, + *climber_back_right_velocity_, back_velocity_pid_calculator_, + *climber_back_left_control_torque_, *climber_back_right_control_torque_); + + if (back_climber_recover_count > 1200) { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_first_torque_); + } else { + limit_back_climber_retract_torque( + control.back_climber_velocity, *climber_back_left_control_torque_, + *climber_back_right_control_torque_, back_climber_retract_second_torque_); + } + + *front_power_budget_active_ = is_front_power_budget_active(); + *front_power_demand_estimate_ = estimate_front_power( + *climber_front_left_requested_control_torque_, + *climber_front_right_requested_control_torque_, *climber_front_left_velocity_, + *climber_front_right_velocity_, front_power_estimate_bias_, + front_power_estimate_k_tau2_, front_power_estimate_k_mech_); + } + + void reset_all_controls() { + *climber_front_left_requested_control_torque_ = nan_; + *climber_front_right_requested_control_torque_ = nan_; + *climber_back_left_control_torque_ = nan_; + *climber_back_right_control_torque_ = nan_; + *climbing_forward_velocity_ = nan_; + *auto_climb_active_ = false; + *front_power_budget_active_ = false; + *front_power_demand_estimate_ = 0.0; + stop_manual_support(); + stop_auto_climb(); + back_climber_zero_velocity_hold_ = false; + } + + void stop_manual_support() { + back_climber_recover_count = 1500; + manual_support_retracting_ = false; + manual_support_retract_block_count_ = 0; + } + + void start_back_climber_retract(const char* source) { + if (!back_climber_recover_count) { + back_climber_recover_count = 1500; + } + manual_support_retracting_ = true; + manual_support_retract_block_count_ = 0; + back_climber_zero_velocity_hold_ = false; + RCLCPP_INFO(logger_, "%s back climber retract started.", source); + } + + void stop_auto_climb() { + auto_climb_state_ = AutoClimbState::IDLE; + auto_climb_timer_ = 0; + auto_climb_stair_index_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + void enter_auto_climb_state(AutoClimbState state) { + if (state == auto_climb_state_) + return; + auto_climb_state_ = state; + auto_climb_timer_ = 0; + auto_climb_align_stable_count_ = 0; + auto_climb_support_block_count_ = 0; + } + + bool is_back_climber_blocked() const { + return (std::abs(*climber_back_left_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_left_velocity_) < kBackClimberBlockedVelocityThreshold) + || (std::abs(*climber_back_right_torque_) > kBackClimberBlockedTorqueThreshold + && std::abs(*climber_back_right_velocity_) < kBackClimberBlockedVelocityThreshold); + } + + bool is_front_power_budget_active() const { + return auto_climb_state_ == AutoClimbState::APPROACH + || auto_climb_state_ == AutoClimbState::SUPPORT_RETRACT; + } + + void dual_motor_sync_control( + double setpoint, double left_velocity, double right_velocity, + pid::MatrixPidCalculator<2>& pid_calculator, double& left_torque_out, + double& right_torque_out) { + + if (std::isnan(setpoint)) { + left_torque_out = nan_; + right_torque_out = nan_; + return; + } + + Eigen::Vector2d setpoint_error{setpoint - left_velocity, setpoint - right_velocity}; + Eigen::Vector2d relative_velocity{ + left_velocity - right_velocity, right_velocity - left_velocity}; + + Eigen::Vector2d control_error = setpoint_error - sync_coefficient_ * relative_velocity; + auto control_torques = pid_calculator.update(control_error); + + left_torque_out = control_torques[0]; + right_torque_out = control_torques[1]; + } + + void limit_back_climber_retract_torque( + double back_climber_velocity_setpoint, double& left_torque, double& right_torque, + double max_torque) const { + + if (!std::isfinite(back_climber_velocity_setpoint) || back_climber_velocity_setpoint >= 0.0) + return; + if (!(max_torque > 0.0)) + return; + + const double peak = std::max(std::abs(left_torque), std::abs(right_torque)); + if (peak <= max_torque) + return; + + const double scale = max_torque / peak; + left_torque *= scale; + right_torque *= scale; + } + + rclcpp::Logger logger_; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kAutoClimbAlignThreshold = 0.10; + static constexpr double kAutoClimbAlignVelocityThreshold = 0.2; + static constexpr double kBackClimberBlockedTorqueThreshold = 0.1; + static constexpr double kBackClimberBlockedVelocityThreshold = 0.1; + static constexpr int kAutoClimbAlignConfirmTicks = 50; + static constexpr int kAutoClimbSupportConfirmTicks = 50; + static constexpr int kAutoClimbDashMinTicks = 100; + static constexpr int kAutoClimbDashTimeoutTicks = 3000; + static constexpr int kAutoClimbSupportRetractConfirmTicks = 50; + static constexpr int kAutoClimbMaxStairs = 2; + static constexpr int kManualSupportRetractConfirmTicks = 50; + + double sync_coefficient_; + double first_stair_approach_pitch_; + double second_stair_approach_pitch_; + + double track_velocity_max_; + double climber_back_control_velocity_abs_; + double auto_climb_support_retract_velocity_fast_abs_; + double auto_climb_support_retract_velocity_slow_abs_; + double auto_climb_approach_chassis_velocity_; + double auto_climb_support_deploy_chassis_velocity_; + double auto_climb_support_retract_chassis_velocity_; + double auto_climb_dash_chassis_velocity_; + double first_stair_dash_leveled_pitch_threshold_; + double second_stair_dash_leveled_pitch_threshold_; + double front_power_estimate_bias_; + double front_power_estimate_k_tau2_; + double front_power_estimate_k_mech_; + + AutoClimbState auto_climb_state_ = AutoClimbState::IDLE; + int auto_climb_timer_ = 0; + int auto_climb_stair_index_ = 0; + int auto_climb_align_stable_count_ = 0; + int auto_climb_support_block_count_ = 0; + bool manual_support_retracting_ = false; + int manual_support_retract_block_count_ = 0; + bool back_climber_zero_velocity_hold_ = false; + + OutputInterface climber_front_left_requested_control_torque_; + OutputInterface climber_front_right_requested_control_torque_; + OutputInterface climber_back_left_control_torque_; + OutputInterface climber_back_right_control_torque_; + OutputInterface climbing_forward_velocity_; + OutputInterface auto_climb_active_; + OutputInterface front_power_budget_active_; + OutputInterface front_power_demand_estimate_; + + InputInterface climber_front_left_velocity_; + InputInterface climber_front_right_velocity_; + InputInterface climber_back_left_velocity_; + InputInterface climber_back_right_velocity_; + + InputInterface climber_back_left_torque_; + InputInterface climber_back_right_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_switch_; + + InputInterface chassis_pitch_imu_; + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_, gimbal_yaw_velocity_imu_; + + rmcs_msgs::Switch last_rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + pid::MatrixPidCalculator<2> front_velocity_pid_calculator_, back_velocity_pid_calculator_; + + std::shared_ptr front_power_limiter_; + + double back_climber_retract_first_torque_ = 8.0; + double back_climber_retract_second_torque_ = 0.5; + int back_climber_recover_count = 0; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::ChassisClimberController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index da70a96f..5b7d9bca 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -58,8 +58,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +74,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -100,8 +100,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,7 +112,7 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } @@ -133,7 +133,7 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { @@ -171,7 +171,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -202,7 +202,7 @@ class ChassisController // Maximum control velocities static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double angular_velocity_max = 16.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,8 +214,8 @@ class ChassisController InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 1eee280c..91990785 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -52,8 +52,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -75,8 +76,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -92,9 +93,8 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; - *supercap_control_enabled_ = false; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; *chassis_control_power_limit_ = 0.0; } @@ -117,8 +117,8 @@ class ChassisPowerController // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index 85220dd9..cbeea088 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -236,29 +236,18 @@ class DeformableOmniWheelController * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); - const double force_to_torque = - friction_coefficient_ * wheel_radius_; - const double rhs = - force_to_torque * mass_ * g_ / 4.0; - const std::vector - half_planes = { - {lambda_1 - force_to_torque * gamma_1, y_sign, - rhs}, - {-lambda_1 - force_to_torque * gamma_1, - -y_sign, rhs}, - {lambda_2 - force_to_torque * gamma_2, y_sign, - rhs}, - {-lambda_2 - force_to_torque * gamma_2, - -y_sign, rhs}, - {-lambda_1 + force_to_torque * gamma_1, y_sign, - rhs}, - {lambda_1 + force_to_torque * gamma_1, - -y_sign, rhs}, - {-lambda_2 + force_to_torque * gamma_2, y_sign, - rhs}, - {lambda_2 + force_to_torque * gamma_2, - -y_sign, rhs}, - }; + const double force_to_torque = friction_coefficient_ * wheel_radius_; + const double rhs = force_to_torque * mass_ * g_ / 4.0; + const std::vector half_planes = { + {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, + {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, + {lambda_2 - force_to_torque * gamma_2, y_sign, rhs}, + {-lambda_2 - force_to_torque * gamma_2, -y_sign, rhs}, + {-lambda_1 + force_to_torque * gamma_1, y_sign, rhs}, + {lambda_1 + force_to_torque * gamma_1, -y_sign, rhs}, + {-lambda_2 + force_to_torque * gamma_2, y_sign, rhs}, + {lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, + }; result = qcp_solver_.solve( {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp new file mode 100644 index 00000000..dd12e09e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -0,0 +1,303 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(10.0, 0.0, 2.4) { + following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_min = -angular_velocity_max; + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + + register_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + register_output("/chassis/control_velocity", chassis_control_velocity_); + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + chassis_velocity_feedback_ready_ = chassis_velocity_.ready(); + if (!chassis_velocity_feedback_ready_) { + chassis_velocity_.make_and_bind_directly(0.0, 0.0, 0.0); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto mode = *mode_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.c && keyboard.c) { + if (mode == rmcs_msgs::ChassisMode::SPIN) { + mode = rmcs_msgs::ChassisMode::AUTO; + } else { + mode = rmcs_msgs::ChassisMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::BACK)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::FRONT; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::FRONT; + } + } else if (!last_keyboard_.z && keyboard.z) { + if (mode != rmcs_msgs::ChassisMode::STEP_DOWN + || (mode == rmcs_msgs::ChassisMode::STEP_DOWN + && step_down_facing_ == StepDownFacing::FRONT)) { + mode = rmcs_msgs::ChassisMode::STEP_DOWN; + step_down_facing_ = StepDownFacing::BACK; + } else { + mode = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + } + } + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::ChassisMode::AUTO; + step_down_facing_ = StepDownFacing::BACK; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(translational_velocity); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + if (!std::isnan(*climbing_forward_velocity_)) { + return Eigen::Vector2d{*climbing_forward_velocity_, 0.0}; + } + + auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + double update_angular_velocity_control(const Eigen::Vector2d& translational_velocity) { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + if (!std::isnan(*climbing_forward_velocity_)) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } + + switch (*mode_) { + case rmcs_msgs::ChassisMode::AUTO: { + angular_velocity = + update_following_angular_velocity(StepDownFacing::BACK, chassis_control_angle); + + // Keep AUTO rear-following gentle at low translation speed and fully enabled at max. + const double measured_translational_speed = + chassis_velocity_feedback_ready_ ? chassis_velocity_->vector.head<2>().norm() + : translational_velocity.norm(); + angular_velocity *= + std::clamp(measured_translational_speed / translational_velocity_max, 0.0, 0.3); + + } break; + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + } break; + case rmcs_msgs::ChassisMode::STEP_DOWN: { + angular_velocity = + update_following_angular_velocity(step_down_facing_, chassis_control_angle); + } break; + case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> signed + // In launch ramp mode, only one direction can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + } break; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = normalize_positive_angle(*gimbal_yaw_angle_error_); + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = normalize_positive_angle(chassis_control_angle + *gimbal_yaw_angle_); + + return err; + } + +private: + enum class StepDownFacing { FRONT, BACK }; + + double update_following_angular_velocity( + StepDownFacing target_facing, double& chassis_control_angle) { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + if (target_facing == StepDownFacing::BACK) { + chassis_control_angle = + normalize_positive_angle(chassis_control_angle + std::numbers::pi); + err = normalize_positive_angle(err + std::numbers::pi); + } + + err = normalize_signed_angle(err); + return following_velocity_controller_.update(err); + } + + static double normalize_positive_angle(double angle) { + constexpr double full_turn = 2 * std::numbers::pi; + while (angle >= full_turn) + angle -= full_turn; + while (angle < 0.0) + angle += full_turn; + return angle; + } + + static double normalize_signed_angle(double angle) { + angle = normalize_positive_angle(angle); + if (angle > std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + // Maximum control velocities + static constexpr double translational_velocity_max = 10.0; + static constexpr double angular_velocity_max = 16.0; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface chassis_velocity_; + InputInterface climbing_forward_velocity_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + bool spinning_forward_ = true; + bool chassis_velocity_feedback_ready_ = false; + StepDownFacing step_down_facing_ = StepDownFacing::BACK; + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp new file mode 100644 index 00000000..ee59e1ce --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_power_controller.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::controller::chassis { + +using namespace referee::app; + +class HeroChassisPowerController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroChassisPowerController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + get_parameter("front_climber_power_limit_max", front_climber_power_limit_max_); + get_parameter("drive_power_limit_floor", drive_power_limit_floor_); + get_parameter("auto_climb_min_control_power_limit", auto_climb_min_control_power_limit_); + register_input("/chassis/control_mode", mode_); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob", rotary_knob_); + + register_input("/chassis/power", chassis_power_); + register_input("/chassis/supercap/voltage", supercap_voltage_); + register_input("/chassis/supercap/enabled", supercap_enabled_); + + register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); + register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input( + "/chassis/climber/front/power_budget_active", front_power_budget_active_, false); + register_input( + "/chassis/climber/front/power_demand_estimate", front_power_demand_estimate_, false); + register_input("/chassis/climber/auto_climb_active", auto_climb_active_, false); + + register_output("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_, 0.0); + register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); + register_output( + "/chassis/climber/front/control_power_limit", front_climber_control_power_limit_, 0.0); + + register_output( + "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, 12.5); + register_output("/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, 12.0); + register_output("/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, 11.0); + } + + void before_updating() override { + if (!front_power_budget_active_.ready()) + front_power_budget_active_.make_and_bind_directly(false); + if (!front_power_demand_estimate_.ready()) + front_power_demand_estimate_.make_and_bind_directly(0.0); + if (!auto_climb_active_.ready()) + auto_climb_active_.make_and_bind_directly(false); + } + + void update() override { + update_charging_power_limit(); + + update_ui(); + + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_power_control(); + return; + } + + update_virtual_buffer_energy(); + + boost_mode_ = keyboard.shift || rotary_knob < -0.9 || *auto_climb_active_; + update_control_power_limit(); + } + +private: + void update_charging_power_limit() { + // Maximum excess power when buffer energy is sufficient. + constexpr double excess_power_limit = 35; + + // charging_power_limit = + constexpr double buffer_energy_control_line = 120; // = referee + excess + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 + + *supercap_charge_power_limit_ = + *chassis_power_limit_referee_ + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_dead_line) + / (buffer_energy_base_line - buffer_energy_dead_line), + 0.0, 1.0) + + excess_power_limit + * std::clamp( + (*chassis_buffer_energy_referee_ - buffer_energy_base_line) + / (buffer_energy_control_line - buffer_energy_base_line), + 0.0, 1.0); + } + + void reset_power_control() { + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; + *chassis_control_power_limit_ = 0.0; + *front_climber_control_power_limit_ = 0.0; + } + + void update_virtual_buffer_energy() { + constexpr double dt = 1e-3; + virtual_buffer_energy_ += dt * (chassis_power_limit_expected_ - *chassis_power_); + virtual_buffer_energy_ = std::clamp( + virtual_buffer_energy_, 0.0, + std::min(*chassis_buffer_energy_referee_, virtual_buffer_energy_limit_)); + } + + void update_control_power_limit() { + double total_power_limit; + + if (boost_mode_ && *supercap_enabled_) + total_power_limit = *mode_ == rmcs_msgs::ChassisMode::LAUNCH_RAMP + ? inf_ + : *chassis_power_limit_referee_ + 80.0; + else + total_power_limit = *chassis_power_limit_referee_; + chassis_power_limit_expected_ = total_power_limit; + + // chassis_control_power_limit = + constexpr double supercap_voltage_control_line = 12.5; // = supercap + constexpr double supercap_voltage_base_line = 12.0; // = referee + total_power_limit = + *chassis_power_limit_referee_ + + (total_power_limit - *chassis_power_limit_referee_) + * std::clamp( + (*supercap_voltage_ - supercap_voltage_base_line) + / (supercap_voltage_control_line - supercap_voltage_base_line), + 0.0, 1.0); + + // Maximum excess power when virtual buffer energy is full. + constexpr double excess_power_limit = 0; + + total_power_limit += excess_power_limit; + total_power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; + + if (*auto_climb_active_) + total_power_limit = std::max(total_power_limit, auto_climb_min_control_power_limit_); + + const auto [drive_limit, front_limit] = split_control_power_limit(total_power_limit); + *chassis_control_power_limit_ = drive_limit; + *front_climber_control_power_limit_ = front_limit; + } + + std::pair split_control_power_limit(double total_power_limit) const { + if (!*front_power_budget_active_) + return {total_power_limit, 0.0}; + + if (total_power_limit <= drive_power_limit_floor_) + return {total_power_limit, 0.0}; + + const double front_limit = std::min( + {*front_power_demand_estimate_, front_climber_power_limit_max_, + total_power_limit - drive_power_limit_floor_}); + return {total_power_limit - front_limit, front_limit}; + } + + void update_ui() { + chassis_power_ui_.set_value(static_cast(std::round(*chassis_power_))); + chassis_control_power_limit_ui_.set_value( + static_cast(std::round(*chassis_control_power_limit_))); + } + + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + InputInterface mode_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + InputInterface chassis_power_; + static constexpr double virtual_buffer_energy_limit_ = 30.0; + double virtual_buffer_energy_; + + InputInterface supercap_voltage_; + InputInterface supercap_enabled_; + + InputInterface chassis_power_limit_referee_; + InputInterface chassis_buffer_energy_referee_; + InputInterface front_power_budget_active_; + InputInterface front_power_demand_estimate_; + InputInterface auto_climb_active_; + + bool boost_mode_ = false; + OutputInterface supercap_charge_power_limit_; + double chassis_power_limit_expected_; + OutputInterface chassis_control_power_limit_; + OutputInterface front_climber_control_power_limit_; + double front_climber_power_limit_max_; + double drive_power_limit_floor_; + double auto_climb_min_control_power_limit_ = 0.0; + + OutputInterface supercap_voltage_control_line_; + OutputInterface supercap_voltage_base_line_; + OutputInterface supercap_voltage_dead_line_; + + ui::Integer chassis_power_ui_{ui::Shape::Color::WHITE, 15, 2, ui::x_center, 100, 0}; + ui::Integer chassis_control_power_limit_ui_{ + ui::Shape::Color::WHITE, 15, 2, ui::x_center, 150, 0}; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroChassisPowerController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp new file mode 100644 index 00000000..1db280c5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_steering_wheel_controller.cpp @@ -0,0 +1,518 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/qcp_solver.hpp" +#include "controller/pid/matrix_pid_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class HeroSteeringWheelController + : public rmcs_executor::Component + , public rclcpp::Node { + + using Formula = std::tuple; + +public: + explicit HeroSteeringWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , mess_(get_parameter("mess").as_double()) + , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) + , friction_coefficient_(get_parameter("friction_coefficient").as_double()) + , k1_(get_parameter("k1").as_double()) + , k2_(get_parameter("k2").as_double()) + , no_load_power_(get_parameter("no_load_power").as_double()) + , control_acceleration_filter_(5.0, 1000.0) + , chassis_velocity_expected_(Eigen::Vector3d::Zero()) + , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) + , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) + , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2 + , sin_varphi_(0, 1, 0, -1) + , steering_velocity_pid_(0.15, 0.0, 0.0) + , steering_angle_pid_(30.0, 0.0, 0.0) + , wheel_velocity_pid_(0.6, 0.0, 0.0) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + + register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); + register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); + register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); + register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); + + register_input("/chassis/left_front_steering/velocity", left_front_steering_velocity_); + register_input("/chassis/left_back_steering/velocity", left_back_steering_velocity_); + register_input("/chassis/right_back_steering/velocity", right_back_steering_velocity_); + register_input("/chassis/right_front_steering/velocity", right_front_steering_velocity_); + + register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); + + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_power_limit", power_limit_); + + register_output( + "/chassis/left_front_steering/control_torque", left_front_steering_control_torque_); + register_output( + "/chassis/left_back_steering/control_torque", left_back_steering_control_torque_); + register_output( + "/chassis/right_back_steering/control_torque", right_back_steering_control_torque_); + register_output( + "/chassis/right_front_steering/control_torque", right_front_steering_control_torque_); + + register_output( + "/chassis/left_front_wheel/control_torque", left_front_wheel_control_torque_); + register_output("/chassis/left_back_wheel/control_torque", left_back_wheel_control_torque_); + register_output( + "/chassis/right_back_wheel/control_torque", right_back_wheel_control_torque_); + register_output( + "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); + register_output( + "/chassis/steering_wheel/actual_power_estimate", steering_wheel_actual_power_estimate_, + 0.0); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + integral_yaw_angle_imu(); + + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + auto chassis_velocity = calculate_chassis_velocity(steering_status, wheel_velocities); + + auto chassis_status_expected = calculate_chassis_status_expected(chassis_velocity); + auto chassis_control_velocity = calculate_chassis_control_velocity(); + + auto chassis_acceleration = calculate_chassis_control_acceleration( + chassis_status_expected.velocity, chassis_control_velocity); + + double power_limit = + *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); + + auto wheel_pid_torques = + calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); + + auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( + steering_status, wheel_velocities, chassis_acceleration, wheel_pid_torques, + power_limit); + auto filtered_chassis_acceleration = + odom_to_base_link_vector(control_acceleration_filter_.update( + base_link_to_odom_vector(constrained_chassis_acceleration))); + + auto steering_torques = calculate_steering_control_torques( + steering_status, chassis_status_expected, filtered_chassis_acceleration); + auto wheel_torques = calculate_wheel_control_torques( + steering_status, filtered_chassis_acceleration, wheel_pid_torques); + + update_control_torques(steering_torques, wheel_torques); + *steering_wheel_actual_power_estimate_ = + calculate_actual_power_estimate(wheel_velocities, wheel_torques); + update_chassis_velocity_expected(filtered_chassis_acceleration); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + Eigen::Vector4d velocity; + }; + + struct ChassisStatus { + Eigen::Vector3d velocity; + Eigen::Vector4d wheel_velocity_x, wheel_velocity_y; + }; + + void reset_all_controls() { + control_acceleration_filter_.reset(); + + chassis_yaw_angle_imu_ = 0.0; + chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + *left_front_steering_control_torque_ = 0.0; + *left_back_steering_control_torque_ = 0.0; + *right_back_steering_control_torque_ = 0.0; + *right_front_steering_control_torque_ = 0.0; + + *left_front_wheel_control_torque_ = 0.0; + *left_back_wheel_control_torque_ = 0.0; + *right_back_wheel_control_torque_ = 0.0; + *right_front_wheel_control_torque_ = 0.0; + *steering_wheel_actual_power_estimate_ = 0.0; + } + + void integral_yaw_angle_imu() { + chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; + chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); + } + + SteeringStatus calculate_steering_status() { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + steering_status.velocity = { + *left_front_steering_velocity_, // + *left_back_steering_velocity_, // + *right_back_steering_velocity_, // + *right_front_steering_velocity_ // + }; + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + ChassisStatus calculate_chassis_status_expected(const Eigen::Vector3d& chassis_velocity) { + auto calculate_energy = [this](const Eigen::Vector3d& velocity) { + return mess_ * velocity.head<2>().squaredNorm() + + moment_of_inertia_ * velocity.z() * velocity.z(); + }; + auto chassis_energy = calculate_energy(chassis_velocity); + auto chassis_energy_expected = calculate_energy(chassis_velocity_expected_); + if (chassis_energy_expected > chassis_energy) { + double k = std::sqrt(chassis_energy / chassis_energy_expected); + chassis_velocity_expected_ *= k; + } + + ChassisStatus chassis_status_expected; + chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + chassis_status_expected.wheel_velocity_x = vx - vehicle_radius_ * vz * sin_varphi_.array(); + chassis_status_expected.wheel_velocity_y = vy + vehicle_radius_ * vz * cos_varphi_.array(); + + return chassis_status_expected; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; + chassis_control_velocity.head<2>() = + Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); + + return chassis_control_velocity; + } + + Eigen::Vector3d calculate_chassis_control_acceleration( + const Eigen::Vector3d& chassis_velocity_expected, + const Eigen::Vector3d& chassis_control_velocity) { + + Eigen::Vector2d translational_control_velocity = chassis_control_velocity.head<2>(); + Eigen::Vector2d translational_velocity = chassis_velocity_expected.head<2>(); + Eigen::Vector2d translational_control_acceleration = + chassis_translational_velocity_pid_.update( + translational_control_velocity - translational_velocity); + + const double& angular_control_velocity = chassis_control_velocity[2]; + const double& angular_velocity = chassis_velocity_expected[2]; + double angular_control_acceleration = + chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); + + Eigen::Vector3d chassis_control_acceleration; + chassis_control_acceleration << translational_control_acceleration, + angular_control_acceleration; + + if (chassis_control_acceleration.lpNorm<1>() < 1e-1) + chassis_control_acceleration.setZero(); + + return chassis_control_acceleration; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const ChassisStatus& chassis_status_expected) { + Eigen::Vector4d wheel_control_velocity = + chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() + + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); + return wheel_velocity_pid_.update( + wheel_control_velocity / wheel_radius_ - wheel_velocities); + } + + Eigen::Vector3d constrain_chassis_control_acceleration( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_acceleration, const Eigen::Vector4d& wheel_pid_torques, + const double& power_limit) { + + Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); + double translational_acceleration_max = translational_acceleration_direction.norm(); + if (translational_acceleration_max > 0.0) + translational_acceleration_direction /= translational_acceleration_max; + + double angular_acceleration_max = chassis_acceleration.z(); + double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; + angular_acceleration_max *= angular_acceleration_direction; + + const double rhombus_right = friction_coefficient_ * g_; + const double rhombus_top = rhombus_right * mess_ * vehicle_radius_ / moment_of_inertia_; + + auto [a, b, c, d, e, f] = calculate_ellipse_parameters( + steering_status, wheel_velocities, translational_acceleration_direction, + angular_acceleration_direction, wheel_pid_torques); + + Eigen::Vector2d best_point = qcp_solver_.solve( + {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f - power_limit}); + + Eigen::Vector3d best_acceleration; + best_acceleration << best_point.x() * translational_acceleration_direction, + best_point.y() * angular_acceleration_direction; + return best_acceleration; + } + + Eigen::Vector calculate_ellipse_parameters( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector2d& translational_acceleration_direction, + const double& angular_acceleration_direction, const Eigen::Vector4d& wheel_torque_base) { + Eigen::Vector4d cos_alpha_minus_gamma = + steering_status.cos_angle.array() * translational_acceleration_direction.x() + + steering_status.sin_angle.array() * translational_acceleration_direction.y(); + Eigen::Vector4d sin_alpha_minus_varphi = + cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array(); + Eigen::Vector4d double_k1_torque_base_plus_wheel_velocities = + 2 * k1_ * wheel_torque_base.array() + wheel_velocities.array(); + + Eigen::Vector formula; + auto& [a, b, c, d, e, f] = formula; + + a = (k1_ * mess_ * mess_ * wheel_radius_ * wheel_radius_ / 16.0) + * cos_alpha_minus_gamma.array().pow(2).sum(); + b = ((k1_ * mess_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (8.0 * vehicle_radius_)) + * angular_acceleration_direction + * (cos_alpha_minus_gamma.array() * sin_alpha_minus_varphi.array()).sum(); + c = ((k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (16.0 * vehicle_radius_ * vehicle_radius_)) + * sin_alpha_minus_varphi.array().pow(2).sum(); + d = (mess_ * wheel_radius_ / 4.0) + * (double_k1_torque_base_plus_wheel_velocities.array() * cos_alpha_minus_gamma.array()) + .sum(); + e = ((moment_of_inertia_ * wheel_radius_) / (4.0 * vehicle_radius_)) + * angular_acceleration_direction + * (double_k1_torque_base_plus_wheel_velocities.array() * sin_alpha_minus_varphi.array()) + .sum(); + f = (wheel_torque_base.array() + * (k1_ * wheel_torque_base.array() + wheel_velocities.array())) + .sum(); + + return formula; + } + + Eigen::Vector4d calculate_steering_control_torques( + const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, + const Eigen::Vector3d& chassis_acceleration) { + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + const auto& [ax, ay, az] = chassis_acceleration; + + Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() + + chassis_status_expected.wheel_velocity_y.array().square(); + + Eigen::Vector4d steering_control_velocities = + vx * ay - vy * ax - vz * (vx * vx + vy * vy) + + vehicle_radius_ * (az * vx - vz * (ax + vz * vy)) * cos_varphi_.array() + + vehicle_radius_ * (az * vy - vz * (ay - vz * vx)) * sin_varphi_.array(); + Eigen::Vector4d steering_control_angles; + + for (int i = 0; i < steering_control_velocities.size(); ++i) { + if (dot_r_squared[i] > 1e-2) { + steering_control_velocities[i] /= dot_r_squared[i]; + steering_control_angles[i] = std::atan2( + chassis_status_expected.wheel_velocity_y[i], + chassis_status_expected.wheel_velocity_x[i]); + } else { + auto x = ax - vehicle_radius_ * (az * sin_varphi_[i] + 0 * cos_varphi_[i]); + auto y = ay + vehicle_radius_ * (az * cos_varphi_[i] - 0 * sin_varphi_[i]); + if (x * x + y * y > 1e-6) { + steering_control_velocities[i] = 0.0; + steering_control_angles[i] = std::atan2(y, x); + } else { + steering_control_velocities[i] = nan_; + steering_control_angles[i] = nan_; + } + } + } + + Eigen::Vector4d steering_torques = steering_velocity_pid_.update( + steering_control_velocities + + steering_angle_pid_.update( + (steering_control_angles - steering_status.angle).unaryExpr([](double diff) { + diff = std::fmod(diff, std::numbers::pi); + if (diff < -std::numbers::pi / 2) { + diff += std::numbers::pi; + } else if (diff > std::numbers::pi / 2) { + diff -= std::numbers::pi; + } + return diff; + })) + - steering_status.velocity); + return steering_torques.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Eigen::Vector4d calculate_wheel_control_torques( + const SteeringStatus& steering_status, const Eigen::Vector3d& chassis_acceleration, + const Eigen::Vector4d& wheel_pid_torques) { + + const auto& [ax, ay, az] = chassis_acceleration; + Eigen::Vector4d wheel_torques = + wheel_radius_ + * (ax * mess_ * steering_status.cos_angle.array() + + ay * mess_ * steering_status.sin_angle.array() + + az * moment_of_inertia_ + * (cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array()) + / vehicle_radius_) + / 4.0; + + wheel_torques += wheel_pid_torques; + + return wheel_torques; + } + + double calculate_actual_power_estimate( + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector4d& wheel_torques) const { + return no_load_power_ + k2_ * wheel_velocities.array().pow(2).sum() + + (wheel_torques.array() * (k1_ * wheel_torques.array() + wheel_velocities.array())) + .sum(); + } + + void update_control_torques( + const Eigen::Vector4d& steering_torques, const Eigen::Vector4d& wheel_torques) { + *left_front_steering_control_torque_ = steering_torques[0]; + *left_back_steering_control_torque_ = steering_torques[1]; + *right_back_steering_control_torque_ = steering_torques[2]; + *right_front_steering_control_torque_ = steering_torques[3]; + + *left_front_wheel_control_torque_ = wheel_torques[0]; + *left_back_wheel_control_torque_ = wheel_torques[1]; + *right_back_wheel_control_torque_ = wheel_torques[2]; + *right_front_wheel_control_torque_ = wheel_torques[3]; + } + + void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { + auto acceleration_odom = base_link_to_odom_vector(chassis_acceleration); + chassis_velocity_expected_ += dt_ * acceleration_odom; + } + + Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.81; + + const double mess_; + const double moment_of_inertia_; + const double vehicle_radius_; + const double wheel_radius_; + const double friction_coefficient_; + + const double k1_, k2_, no_load_power_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_steering_velocity_; + InputInterface left_back_steering_velocity_; + InputInterface right_back_steering_velocity_; + InputInterface right_front_steering_velocity_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + + OutputInterface left_front_steering_control_torque_; + OutputInterface left_back_steering_control_torque_; + OutputInterface right_back_steering_control_torque_; + OutputInterface right_front_steering_control_torque_; + + OutputInterface left_front_wheel_control_torque_; + OutputInterface left_back_wheel_control_torque_; + OutputInterface right_back_wheel_control_torque_; + OutputInterface right_front_wheel_control_torque_; + OutputInterface steering_wheel_actual_power_estimate_; + + QcpSolver qcp_solver_; + filter::LowPassFilter<3> control_acceleration_filter_; + + double chassis_yaw_angle_imu_ = 0.0; + Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; + pid::PidCalculator chassis_angular_velocity_pid_; + + const Eigen::Vector4d cos_varphi_, sin_varphi_; + + pid::MatrixPidCalculator<4> steering_velocity_pid_, steering_angle_pid_, wheel_velocity_pid_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::HeroSteeringWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index 5512981d..46733c51 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -129,8 +129,10 @@ class OmniWheelController const auto& [x, y, z] = chassis_velocity; constexpr double a_plus_b = chassis_radius_x_ + chassis_radius_y_; Eigen::Vector4d wheel_control_velocity = { - -x + y + a_plus_b * z, -x - y + a_plus_b * z, // - +x - y + a_plus_b * z, +x + y + a_plus_b * z, // + -x + y + a_plus_b * z, + -x - y + a_plus_b * z, // + +x - y + a_plus_b * z, + +x + y + a_plus_b * z, // }; wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp index aeba601a..6c708586 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp @@ -61,9 +61,9 @@ class QcpSolver { const std::vector& half_planes, const QuadraticConstraint& quadratic_constraint) { std::vector polygon = { - {boundary_constraint.x_max, boundary_constraint.y_max}, - { 0.0, boundary_constraint.y_max}, - { 0.0, -boundary_constraint.y_max}, + {boundary_constraint.x_max, boundary_constraint.y_max}, + {0.0, boundary_constraint.y_max}, + {0.0, -boundary_constraint.y_max}, {boundary_constraint.x_max, -boundary_constraint.y_max}, }; @@ -129,10 +129,7 @@ class QcpSolver { [&line](const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) -> Eigen::Vector2d { Eigen::Vector3d line2 = Eigen::Vector3d(p1.x(), p1.y(), 1).cross(Eigen::Vector3d(p2.x(), p2.y(), 1)); - Eigen::Matrix2d matrix{ - { line.x(), line.y()}, - {line2.x(), line2.y()} - }; + Eigen::Matrix2d matrix{{line.x(), line.y()}, {line2.x(), line2.y()}}; return matrix.inverse() * Eigen::Vector2d(-line.z(), -line2.z()); }); } diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp new file mode 100644 index 00000000..8450b386 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_status.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class SteeringWheelStatus + : public rmcs_executor::Component + , public rclcpp::Node { +public: + explicit SteeringWheelStatus() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) { + register_input("/chassis/left_front_steering/angle", left_front_steering_angle_); + register_input("/chassis/left_back_steering/angle", left_back_steering_angle_); + register_input("/chassis/right_back_steering/angle", right_back_steering_angle_); + register_input("/chassis/right_front_steering/angle", right_front_steering_angle_); + + register_input("/chassis/left_front_wheel/velocity", left_front_wheel_velocity_); + register_input("/chassis/left_back_wheel/velocity", left_back_wheel_velocity_); + register_input("/chassis/right_back_wheel/velocity", right_back_wheel_velocity_); + register_input("/chassis/right_front_wheel/velocity", right_front_wheel_velocity_); + + register_output("/chassis/velocity", chassis_velocity_, 0.0, 0.0, 0.0); + } + + void update() override { + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + chassis_velocity_->vector = calculate_chassis_velocity(steering_status, wheel_velocities); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + }; + + SteeringStatus calculate_steering_status() const { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() const { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + const double vehicle_radius_; + const double wheel_radius_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + OutputInterface chassis_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelStatus, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 739b5859..0d49184e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,9 +1,13 @@ +#include +#include #include #include +#include #include #include +#include #include "controller/pid/pid_calculator.hpp" @@ -39,14 +43,17 @@ class DualYawController register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/gimbal/mode", gimbal_mode_); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0); + register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_); + register_output( + "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); + register_output("/gimbal/top_yaw/control_angle_shift", top_yaw_control_angle_shift_, nan_); + register_output("/gimbal/bottom_yaw/control_angle", bottom_yaw_control_angle_, nan_); status_component_ = create_partner_component(get_component_name() + "_status"); @@ -61,6 +68,31 @@ class DualYawController } void update() override { + + const auto mode = *gimbal_mode_; + if (mode == rmcs_msgs::GimbalMode::ENCODER) { + const bool entering_encoder = last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER; + if (entering_encoder) { + if (std::isfinite(*top_yaw_angle_)) { + top_yaw_encoder_locked_ = true; + } else { + top_yaw_encoder_locked_ = false; + } + } + *bottom_yaw_control_angle_shift_ = *control_angle_shift_; + if (top_yaw_encoder_locked_) + *top_yaw_control_angle_ = top_yaw_encoder_angle_; + else + *top_yaw_control_angle_ = nan_; + } else { + *top_yaw_control_angle_ = nan_; + *bottom_yaw_control_angle_shift_ = nan_; + top_yaw_encoder_angle_ = *top_yaw_angle_; + top_yaw_encoder_locked_ = false; + } + + last_gimbal_mode_ = mode; + if (std::isnan(*control_angle_error_)) { *top_yaw_control_torque_ = nan_; *bottom_yaw_control_torque_ = nan_; @@ -72,23 +104,30 @@ class DualYawController bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) - bottom_yaw_velocity_imu()); } - - if (std::isnan(*control_angle_shift_)) { - *top_yaw_control_angle_ = nan_; - *bottom_yaw_control_angle_shift_ = nan_; - } else { - *top_yaw_control_angle_ = 0.0; - *bottom_yaw_control_angle_shift_ = *control_angle_shift_; - } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + double wrap_angle(double angle) { + while (angle > 0) + angle += 2 * std::numbers::pi; + while (angle >= 2 * std::numbers::pi) + angle -= 2 * std::numbers::pi; + return angle; + } + double bottom_yaw_control_error() { - double err = *top_yaw_angle_ + *control_angle_error_; - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; + if (!std::isfinite(*top_yaw_angle_) || !std::isfinite(*control_angle_error_)) + return nan_; + + // Avoid relying on top_yaw_angle in [0, 2pi) and control_angle_error in [-pi, pi]. + constexpr double alignment = 2 * std::numbers::pi; + double err = + std::fmod(*top_yaw_angle_ + *control_angle_error_ + std::numbers::pi, alignment); + if (err < 0) + err += alignment; + err -= std::numbers::pi; return err; } @@ -99,6 +138,7 @@ class DualYawController InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface gimbal_mode_; InputInterface control_angle_error_, control_angle_shift_; pid::PidCalculator top_yaw_angle_pid_, top_yaw_velocity_pid_; @@ -110,6 +150,13 @@ class DualYawController OutputInterface top_yaw_control_angle_; OutputInterface bottom_yaw_control_angle_shift_; + OutputInterface bottom_yaw_control_angle_; + OutputInterface top_yaw_control_angle_shift_; + + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + bool top_yaw_encoder_locked_ = false; + double top_yaw_encoder_angle_ = nan_; + class DualYawStatus : public rmcs_executor::Component { public: explicit DualYawStatus() { diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..939f17f6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -1,5 +1,7 @@ +#include #include +#include #include #include #include @@ -23,12 +25,10 @@ class HeroGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , upper_limit_(get_parameter("upper_limit").as_double()) + , lower_limit_(get_parameter("lower_limit").as_double()) + , imu_gimbal_solver(*this, upper_limit_, lower_limit_) + , encoder_gimbal_solver(*this, upper_limit_, lower_limit_) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/left", switch_left_); @@ -38,6 +38,8 @@ class HeroGimbalController register_input("/remote/keyboard", keyboard_); register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); @@ -59,14 +61,17 @@ class HeroGimbalController break; } - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) + if (!last_keyboard_.e && keyboard_->e) { + if (gimbal_mode_keyboard_ == GimbalMode::IMU) { + encoder_init_pitch_ = keyboard_->ctrl ? kCtrlEInitPitch : kEInitPitch; gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else + } else { gimbal_mode_keyboard_ = GimbalMode::IMU; + } } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; + + *gimbal_mode_ = gimbal_mode_keyboard_; + //*gimbal_mode_ = switch_right == Switch::UP ? GimbalMode::ENCODER : GimbalMode::IMU; if (*gimbal_mode_ == GimbalMode::IMU) { auto angle_error = update_imu_control(); @@ -76,6 +81,7 @@ class HeroGimbalController encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); *yaw_control_angle_shift_ = nan_; *pitch_control_angle_ = nan_; + } else { imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); *yaw_angle_error_ = nan_; @@ -121,24 +127,26 @@ class HeroGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return imu_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) - return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); + if (!encoder_gimbal_solver.enabled()) { + return encoder_gimbal_solver.update( + PreciseTwoAxisGimbalSolver::SetControlPitch{encoder_init_pitch_}); + } - constexpr double joystick_sensitivity = 0.006 * 0.1; constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; + constexpr double joystick_sensitivity = 0.006 * 0.05; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_yaw_sensitivity * mouse_velocity_->y(); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); + + mouse_pitch_sensitivity * mouse_velocity_->x(); return encoder_gimbal_solver.update( PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); @@ -147,6 +155,9 @@ class HeroGimbalController private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double kEInitPitch = -0.476972; // Initial angle for standalone E. + static constexpr double kCtrlEInitPitch = -0.541591; // Initial angle for Ctrl+E. + double encoder_init_pitch_ = kEInitPitch; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; @@ -157,10 +168,13 @@ class HeroGimbalController rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface auto_aim_control_direction_; + InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; OutputInterface gimbal_mode_; + const double upper_limit_, lower_limit_; TwoAxisGimbalSolver imu_gimbal_solver; PreciseTwoAxisGimbalSolver encoder_gimbal_solver; @@ -173,4 +187,4 @@ class HeroGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::HeroGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp index 8eb7e647..d4d98715 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/player_viewer.cpp @@ -27,7 +27,8 @@ class PlayerViewer register_input("/remote/mouse/mouse_wheel", mouse_wheel_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/angle", gimbal_pitch_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_pitch_raw_angle_); register_input("/gimbal/player_viewer/angle", gimbal_player_viewer_angle_); register_output( @@ -46,20 +47,23 @@ class PlayerViewer using namespace rmcs_msgs; const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - if (!last_keyboard_.e && keyboard.e) - viewer_reset_ = true; if (!last_keyboard_.q && keyboard.q) { - scope_active_ = !scope_active_; - *is_scope_active_ = scope_active_; + scope_active_ = !scope_active_; + *is_scope_active_ = scope_active_; scope_viewer_reset_ = scope_active_; } + if (!last_keyboard_.e && keyboard.e) { + viewer_init_angle_ = keyboard.ctrl ? kCtrlInitViewerAngle : kEInitViewerAngle; + viewer_reset_ = true; + scope_viewer_reset_ = false; + } update_viewer_control(); }; @@ -72,17 +76,17 @@ class PlayerViewer *scope_control_torque_ = nan_; *viewer_control_angle_error_ = nan_; - *viewer_control_angle_ = nan_; + *viewer_control_angle_ = nan_; - *is_scope_active_ = false; - scope_active_ = false; + *is_scope_active_ = false; + scope_active_ = false; scope_viewer_reset_ = false; - viewer_reset_ = true; + viewer_reset_ = true; } void update_viewer_control() { constexpr double scope_offset_angle = 0.31; - *scope_offset_angle_ = scope_offset_angle; + *scope_offset_angle_ = scope_offset_angle; auto unit_sensitivity = [&](double sensitivity) { return (*is_scope_active_) ? sensitivity : 1.0; @@ -92,12 +96,12 @@ class PlayerViewer *viewer_delta_angle_by_mouse_wheel_ = 0.5 * *mouse_wheel_ * unit_sensitivity(0.09); if (viewer_reset_) { - *viewer_control_angle_ = upper_limit_; - viewer_reset_ = false; + *viewer_control_angle_ = viewer_init_angle_; + viewer_reset_ = false; } else { if (scope_viewer_reset_) { *viewer_control_angle_ = *scope_offset_angle_; - scope_viewer_reset_ = false; + scope_viewer_reset_ = false; } else { *viewer_control_angle_ += *viewer_delta_angle_by_mouse_wheel_; } @@ -115,19 +119,24 @@ class PlayerViewer *viewer_control_angle_ - norm_angle(*gimbal_player_viewer_angle_); if (scope_active_) { - *scope_control_torque_ = 0.2; + *scope_control_torque_ = 0.13; } else { - *scope_control_torque_ = -0.2; + *scope_control_torque_ = -0.13; } } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - static constexpr double pi_ = std::numbers::pi; + static constexpr double pi_ = std::numbers::pi; + + // The steering-hero viewer angle limit range is [0.68, 1.17]. + static constexpr double kEInitViewerAngle = 0.38905; // Move here when E is pressed. + static constexpr double kCtrlInitViewerAngle = 0.38905; // Move here when Ctrl is pressed. bool scope_viewer_reset_{false}; const double upper_limit_, lower_limit_; + double viewer_init_angle_ = kEInitViewerAngle; InputInterface switch_right_; InputInterface switch_left_; @@ -135,6 +144,7 @@ class PlayerViewer InputInterface mouse_wheel_; InputInterface gimbal_pitch_angle_; + InputInterface gimbal_pitch_raw_angle_; InputInterface gimbal_player_viewer_angle_; OutputInterface scope_control_torque_; @@ -155,4 +165,4 @@ class PlayerViewer #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::PlayerViewer, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp index 9a5c2948..daa13997 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/precise_two_axis_gimbal_solver.hpp @@ -25,8 +25,48 @@ class PreciseTwoAxisGimbalSolver { : upper_limit_(upper_limit) , lower_limit_(lower_limit) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/tf", tf_); } + struct SetControlDirection : Operation { + friend class PreciseTwoAxisGimbalSolver; + + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + double update(PreciseTwoAxisGimbalSolver& super) const { + if (!super.tf_.ready()) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + + auto dir_yaw_link = fast_tf::cast(target_, *super.tf_); + Eigen::Vector3d dir = *dir_yaw_link; + + const double norm = dir.norm(); + if (norm < kEps) { + super.control_pitch_angle_ = PreciseTwoAxisGimbalSolver::nan_; + return PreciseTwoAxisGimbalSolver::nan_; + } + dir /= norm; + + const double xy_norm = std::hypot(dir.x(), dir.y()); + + // In YawLink, yaw is the increment relative to the current total yaw. + const double yaw_shift = std::atan2(dir.y(), dir.x()); + + // Mechanical pitch convention: level is 0, pitching downward is positive, so negate it. + const double pitch_angle = -std::atan2(dir.z(), std::max(xy_norm, kEps)); + + super.set_pitch_angle(pitch_angle); + return yaw_shift; + } + + static constexpr double kEps = 1e-9; + OdomImu::DirectionVector target_; + }; + struct SetDisabled : Operation { friend class PreciseTwoAxisGimbalSolver; @@ -91,10 +131,11 @@ class PreciseTwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface tf_; const double upper_limit_, lower_limit_; double control_pitch_angle_ = nan_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp new file mode 100644 index 00000000..366f66b6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/friction_wheel_pid_recorder.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace rmcs_core::controller::pid { + +class FrictionWheelPidRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FrictionWheelPidRecorder() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + const auto wheels = + declare_parameter>("wheels", std::vector{}); + if (wheels.empty()) + throw std::runtime_error("Parameter \"wheels\" must not be empty"); + + min_setpoint_abs_ = declare_parameter("min_setpoint_abs", 10.0); + + const auto flush_every_n_samples = declare_parameter("flush_every_n_samples", 100); + if (flush_every_n_samples <= 0) + throw std::runtime_error("Parameter \"flush_every_n_samples\" must be positive"); + flush_every_n_samples_ = static_cast(flush_every_n_samples); + + const auto output_dir = std::filesystem::path{ + declare_parameter("output_dir", "/tmp/friction_pid_logs")}; + const auto output_name = declare_parameter("output_name", ""); + + wheel_count_ = wheels.size(); + wheel_ids_ = std::make_unique(wheel_count_); + setpoints_ = std::make_unique[]>(wheel_count_); + measurements_ = std::make_unique[]>(wheel_count_); + control_torques_ = std::make_unique[]>(wheel_count_); + run_ids_ = std::make_unique(wheel_count_); + sample_indices_ = std::make_unique(wheel_count_); + previously_enabled_ = std::make_unique(wheel_count_); + + for (size_t i = 0; i < wheel_count_; ++i) { + wheel_ids_[i] = wheel_id_from_topic(wheels[i]); + register_input(wheels[i] + "/control_velocity", setpoints_[i]); + register_input(wheels[i] + "/velocity", measurements_[i]); + register_input(wheels[i] + "/control_torque", control_torques_[i]); + + run_ids_[i] = 0; + sample_indices_[i] = 0; + previously_enabled_[i] = false; + } + + std::filesystem::create_directories(output_dir); + log_file_path_ = output_dir / resolve_output_name(output_name); + log_stream_.open(log_file_path_); + if (!log_stream_.is_open()) + throw std::runtime_error( + "Failed to open recorder output file: " + log_file_path_.string()); + + log_stream_.setf(std::ios::fixed); + log_stream_.precision(9); + log_stream_ + << "timestamp_us,wheel_id,run_id,sample_idx,setpoint_velocity,measured_velocity," + "control_torque,enabled\n"; + + RCLCPP_INFO(get_logger(), "FrictionWheelPidRecorder writing to %s", log_file_path_.c_str()); + } + + ~FrictionWheelPidRecorder() override { + if (log_stream_.is_open()) { + log_stream_.flush(); + log_stream_.close(); + } + } + + void update() override { + const auto timestamp_us = now_timestamp_us(); + + for (size_t i = 0; i < wheel_count_; ++i) { + const bool enabled = should_record(i); + if (enabled && !previously_enabled_[i]) { + ++run_ids_[i]; + sample_indices_[i] = 0; + } + + if (enabled) { + log_stream_ << timestamp_us << ',' << wheel_ids_[i] << ',' << run_ids_[i] << ',' + << sample_indices_[i]++ << ',' << *setpoints_[i] << ',' + << *measurements_[i] << ',' << *control_torques_[i] << ",1\n"; + if (++unflushed_samples_ >= flush_every_n_samples_) { + log_stream_.flush(); + unflushed_samples_ = 0; + } + } + + previously_enabled_[i] = enabled; + } + } + +private: + static std::string wheel_id_from_topic(std::string_view topic) { + auto pos = topic.find_last_of('/'); + if (pos == std::string_view::npos || pos + 1 >= topic.size()) + return std::string(topic); + return std::string(topic.substr(pos + 1)); + } + + static uint64_t now_timestamp_us() { + const auto now = std::chrono::steady_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); + } + + static std::string resolve_output_name(const std::string& configured_name) { + if (!configured_name.empty()) + return configured_name; + + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const auto ms = std::chrono::duration_cast(now).count(); + return "friction_wheel_pid_" + std::to_string(ms) + ".csv"; + } + + bool should_record(size_t index) const { + if (!setpoints_[index].ready() || !measurements_[index].ready() + || !control_torques_[index].ready()) + return false; + + const auto setpoint = *setpoints_[index]; + const auto measurement = *measurements_[index]; + const auto control_torque = *control_torques_[index]; + + return std::isfinite(setpoint) && std::isfinite(measurement) + && std::isfinite(control_torque) && std::abs(setpoint) >= min_setpoint_abs_; + } + + size_t wheel_count_ = 0; + double min_setpoint_abs_ = 10.0; + size_t flush_every_n_samples_ = 100; + size_t unflushed_samples_ = 0; + + std::filesystem::path log_file_path_; + std::ofstream log_stream_; + + std::unique_ptr wheel_ids_; + std::unique_ptr[]> setpoints_; + std::unique_ptr[]> measurements_; + std::unique_ptr[]> control_torques_; + std::unique_ptr run_ids_; + std::unique_ptr sample_indices_; + std::unique_ptr previously_enabled_; +}; + +} // namespace rmcs_core::controller::pid + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::pid::FrictionWheelPidRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp index f17c35a1..68814799 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp @@ -12,7 +12,7 @@ namespace rmcs_core::controller::pid { template class MatrixPidCalculator { using Vector = Eigen::Vector; - using Gain = std::conditional, double>::type; + using Gain = std::conditional, double>::type; public: MatrixPidCalculator(Gain kp, Gain ki, Gain kd) @@ -35,7 +35,7 @@ class MatrixPidCalculator { Vector update(Vector err) { Vector control = kp * err + ki * err_integral_; - err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); + err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); control += exclude_nan(kd * (err - last_err_)); last_err_ = err; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp index bf3076e3..d8bd95cb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_42mm.cpp @@ -31,14 +31,15 @@ class BulletFeederController42mm register_input( "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_limited_by_heat_); + register_input("/gimbal/bullet_fired", bullet_fired_); - bullet_feeder_velocity_pid_.kp = 50.0; - bullet_feeder_velocity_pid_.ki = 10.0; - bullet_feeder_velocity_pid_.kd = 0.0; + bullet_feeder_velocity_pid_.kp = 50.0; + bullet_feeder_velocity_pid_.ki = 10.0; + bullet_feeder_velocity_pid_.kd = 0.0; bullet_feeder_velocity_pid_.integral_max = 60.0; bullet_feeder_velocity_pid_.integral_min = 0.0; - bullet_feeder_angle_pid_.kp = 50.0; + bullet_feeder_angle_pid_.kp = 60.0; bullet_feeder_angle_pid_.ki = 0.0; bullet_feeder_angle_pid_.kd = 2.0; @@ -51,9 +52,9 @@ class BulletFeederController42mm void update() override { const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto mouse = *mouse_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -61,7 +62,6 @@ class BulletFeederController42mm reset_all_controls(); return; } - overdrive_mode_ = keyboard.f; if (keyboard.ctrl && !last_keyboard_.r && keyboard.r) low_latency_mode_ = !low_latency_mode_; @@ -84,8 +84,8 @@ class BulletFeederController42mm } else { if (!*friction_ready_ || std::isnan(bullet_feeder_control_angle_)) { bullet_feeder_control_angle_ = *bullet_feeder_angle_; - shoot_stage_ = ShootStage::PRELOADED; - bullet_fed_count_ = static_cast( + shoot_stage_ = ShootStage::PRELOADED; + bullet_fed_count_ = static_cast( (*bullet_feeder_angle_ - bullet_feeder_compressed_zero_point_ - 0.1) / bullet_feeder_angle_per_bullet_); } @@ -100,12 +100,10 @@ class BulletFeederController42mm } } - double err_abs = std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f %.2f", bullet_feeder_control_angle_, - // *bullet_feeder_angle_, err_abs); + double angle_error_abs = + std::abs(bullet_feeder_control_angle_ - *bullet_feeder_angle_); if (shoot_stage_ == ShootStage::PRELOADING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloaded(); } if (shoot_stage_ == ShootStage::PRELOADED) { @@ -113,46 +111,42 @@ class BulletFeederController42mm set_compressing(); } if (shoot_stage_ == ShootStage::COMPRESSING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_compressed(); } if (shoot_stage_ == ShootStage::SHOOTING) { - if (err_abs < 0.1) + if (angle_error_abs < 0.1) set_preloading(); } } - double velocity_err = bullet_feeder_angle_pid_.update( - bullet_feeder_control_angle_ - *bullet_feeder_angle_) - - *bullet_feeder_velocity_; - // bullet_feeder_velocity_pid_.integral_max = std::clamp(1000.0 * velocity_err, - // 0.0, 60.0); - *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_err); - // RCLCPP_INFO( - // get_logger(), "%.2f %.2f", velocity_err, *bullet_feeder_control_torque_); + double velocity_error = bullet_feeder_angle_pid_.update( + bullet_feeder_control_angle_ - *bullet_feeder_angle_) + - *bullet_feeder_velocity_; + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update(velocity_error); update_jam_detection(); } last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_mouse_ = mouse; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; } private: void reset_all_controls() { last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_mouse_ = rmcs_msgs::Mouse::zero(); - last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); overdrive_mode_ = low_latency_mode_ = false; - shoot_stage_ = ShootStage::PRELOADED; + shoot_stage_ = ShootStage::PRELOADED; bullet_fed_count_ = std::numeric_limits::min(); - bullet_feeder_control_angle_ = nan_; + bullet_feeder_control_angle_ = nan_; bullet_feeder_angle_pid_.output_max = inf_; bullet_feeder_velocity_pid_.reset(); @@ -160,13 +154,13 @@ class BulletFeederController42mm *bullet_feeder_control_torque_ = nan_; bullet_feeder_faulty_count_ = 0; - bullet_feeder_cool_down_ = 0; + bullet_feeder_cool_down_ = 0; } void set_preloading() { RCLCPP_INFO(get_logger(), "PRELOADING"); bullet_fed_count_++; - shoot_stage_ = ShootStage::PRELOADING; + shoot_stage_ = ShootStage::PRELOADING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 0.5) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; @@ -179,7 +173,7 @@ class BulletFeederController42mm void set_compressing() { RCLCPP_INFO(get_logger(), "COMPRESSING"); - shoot_stage_ = ShootStage::COMPRESSING; + shoot_stage_ = ShootStage::COMPRESSING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 0.8; @@ -192,14 +186,13 @@ class BulletFeederController42mm void set_shooting() { RCLCPP_INFO(get_logger(), "SHOOTING"); - shoot_stage_ = ShootStage::SHOOTING; + shoot_stage_ = ShootStage::SHOOTING; bullet_feeder_control_angle_ = bullet_feeder_compressed_zero_point_ + (bullet_fed_count_ + 1.2) * bullet_feeder_angle_per_bullet_; bullet_feeder_angle_pid_.output_max = 1.0; } void update_jam_detection() { - // RCLCPP_INFO(get_logger(), "%.2f --", *bullet_feeder_control_torque_); if (*bullet_feeder_control_torque_ < 300.0) { bullet_feeder_faulty_count_ = 0; return; @@ -215,7 +208,7 @@ class BulletFeederController42mm void enter_jam_protection() { bullet_feeder_control_angle_ = nan_; - bullet_feeder_cool_down_ = 1000; + bullet_feeder_cool_down_ = 1000; bullet_feeder_angle_pid_.reset(); bullet_feeder_velocity_pid_.reset(); RCLCPP_INFO(get_logger(), "Jammed!"); @@ -225,9 +218,10 @@ class BulletFeederController42mm static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double bullet_feeder_compressed_zero_point_ = 0.58; - static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; InputInterface friction_ready_; + InputInterface bullet_fired_; InputInterface switch_right_; InputInterface switch_left_; @@ -235,9 +229,9 @@ class BulletFeederController42mm InputInterface keyboard_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); bool overdrive_mode_ = false, low_latency_mode_ = false; @@ -247,8 +241,8 @@ class BulletFeederController42mm InputInterface control_bullet_allowance_limited_by_heat_; enum class ShootStage { PRELOADING, PRELOADED, COMPRESSING, COMPRESSED, SHOOTING }; - ShootStage shoot_stage_ = ShootStage::PRELOADED; - int bullet_fed_count_ = std::numeric_limits::min(); + ShootStage shoot_stage_ = ShootStage::PRELOADED; + int bullet_fed_count_ = std::numeric_limits::min(); double bullet_feeder_control_angle_ = nan_; pid::PidCalculator bullet_feeder_velocity_pid_; @@ -256,7 +250,7 @@ class BulletFeederController42mm OutputInterface bullet_feeder_control_torque_; int bullet_feeder_faulty_count_ = 0; - int bullet_feeder_cool_down_ = 0; + int bullet_feeder_cool_down_ = 0; OutputInterface shoot_mode_; }; @@ -266,4 +260,4 @@ class BulletFeederController42mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController42mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp new file mode 100644 index 00000000..9f4e62f0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_friction_wheel_controller.cpp @@ -0,0 +1,263 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroFrictionWheelController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroFrictionWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/keyboard", keyboard_); + + auto friction_wheels = get_parameter("friction_wheels").as_string_array(); + auto friction_profile_0 = get_parameter("friction_velocities_profile_0").as_double_array(); + auto friction_profile_1 = get_parameter("friction_velocities_profile_1").as_double_array(); + if (friction_wheels.size() != friction_profile_0.size() + || friction_wheels.size() != friction_profile_1.size()) { + throw std::runtime_error( + "'friction_wheels' and both friction velocity profiles must have the same length!"); + } else if (friction_wheels.size() == 0) + throw std::runtime_error( + "Empty array error: 'friction_wheels' and 'friction_velocities' cannot be empty!"); + + friction_profile_0_.assign(friction_profile_0.begin(), friction_profile_0.end()); + friction_profile_1_.assign(friction_profile_1.begin(), friction_profile_1.end()); + friction_count_ = friction_wheels.size(); + friction_velocities_ = std::make_unique[]>(friction_count_); + friction_control_velocities_ = std::make_unique[]>(friction_count_); + for (size_t i = 0; i < friction_count_; i++) { + register_input(friction_wheels[i] + "/velocity", friction_velocities_[i]); + register_output( + friction_wheels[i] + "/control_velocity", friction_control_velocities_[i], nan_); + } + + friction_soft_start_stop_step_ = + (1 / 1000.0) / get_parameter("friction_soft_start_stop_time").as_double(); + + register_output("/gimbal/friction_ready", friction_ready_, false); + register_output("/gimbal/friction_jammed", friction_jammed_, false); + register_output("/gimbal/bullet_fired", bullet_fired_, false); + register_output("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + } + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; + if (!last_keyboard_.f && keyboard.f) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + const bool profile_switch_now = keyboard.ctrl && keyboard.f; + const bool profile_switch_last = last_keyboard_.ctrl && last_keyboard_.f; + + if (!profile_switch_last && profile_switch_now) { + active_profile_ ^= 1; + + if (!std::isnan(friction_soft_start_stop_percentage_)) { + double sum = 0.0; + for (size_t i = 0; i < friction_count_; i++) + sum += *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ = + std::clamp(sum / static_cast(friction_count_), 0.0, 1.0); + } + } + + *friction_profile_1_active_ = active_profile_ == 1; + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + if (switch_right != Switch::DOWN) { + if ((!last_keyboard_.v && keyboard.v) + || (last_switch_left_ == Switch::MIDDLE && switch_left == Switch::UP)) { + friction_enabled_ = !friction_enabled_; + } + + update_friction_velocities(); + update_friction_status(); + if (*friction_jammed_) + RCLCPP_INFO(logger_, "Friction Jammed!"); + if (*bullet_fired_) + RCLCPP_INFO(logger_, "Bullet Fired!"); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + if (!friction_enabled_) { + reset_all_controls(); + } + } + +private: + void reset_all_controls() { + friction_enabled_ = false; + + last_primary_friction_velocity_ = nan_; + primary_friction_velocity_decrease_integral_ = 0; + + friction_soft_start_stop_percentage_ = nan_; + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = nan_; + + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + } + + void update_friction_velocities() { + if (std::isnan(friction_soft_start_stop_percentage_)) { + friction_soft_start_stop_percentage_ = 0.0; + for (size_t i = 0; i < friction_count_; i++) + friction_soft_start_stop_percentage_ += + *friction_velocities_[i] / target_friction_velocity(i); + friction_soft_start_stop_percentage_ /= static_cast(friction_count_); + } + friction_soft_start_stop_percentage_ += + friction_enabled_ ? friction_soft_start_stop_step_ : -friction_soft_start_stop_step_; + friction_soft_start_stop_percentage_ = + std::clamp(friction_soft_start_stop_percentage_, 0.0, 1.0); + + for (size_t i = 0; i < friction_count_; i++) + *friction_control_velocities_[i] = + friction_soft_start_stop_percentage_ * target_friction_velocity(i); + } + + void update_friction_status() { + *friction_ready_ = *friction_jammed_ = *bullet_fired_ = false; + + if (!friction_enabled_) + return; + if (friction_soft_start_stop_percentage_ < 1.0) + return; + + if (detect_friction_faulty()) { + if (friction_faulty_count_ == 200) { + friction_enabled_ = false; + *friction_jammed_ = true; + } else { + friction_faulty_count_++; + *friction_ready_ = true; + } + return; + } + + *friction_ready_ = true; + *bullet_fired_ = detect_bullet_fire(); + } + + bool detect_friction_faulty() { + for (size_t i = 0; i < friction_count_; i++) { + if (abs(*friction_velocities_[i]) < abs(*friction_control_velocities_[i] * 0.5)) + return true; + } + return false; + } + + rmcs_utility::FpsCounter fps_counter_; + + bool detect_bullet_fire() { + bool fired = false; + + // TODO(steering-hero): This intentionally keeps the legacy merge behavior by monitoring + // friction_velocities_[2]. Historically, hero config ordering mapped [2,3] to the first + // stage used for fire detection. Replace this hard-coded index with an explicit first- + // stage mapping once the wheel ordering semantics are unified. + if (!std::isnan(last_primary_friction_velocity_)) { + double differential = *friction_velocities_[2] - last_primary_friction_velocity_; + if (differential < 0.1) + primary_friction_velocity_decrease_integral_ += differential; + else { + if (primary_friction_velocity_decrease_integral_ < -14.0 + && last_primary_friction_velocity_ < target_friction_velocity(2) - 25.0) + fired = true; + + primary_friction_velocity_decrease_integral_ = 0; + } + } + last_primary_friction_velocity_ = *friction_velocities_[2]; + + return fired; + } + + double target_friction_velocity(size_t i) const { + return active_profile_ == 0 ? friction_profile_0_[i] : friction_profile_1_[i]; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + rclcpp::Logger logger_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + size_t friction_count_; + + std::vector friction_profile_0_; + std::vector friction_profile_1_; + size_t active_profile_ = 0; + + std::unique_ptr[]> friction_velocities_; + + bool friction_enabled_ = false; + + double friction_soft_start_stop_step_; + double friction_soft_start_stop_percentage_ = nan_; + std::unique_ptr[]> friction_control_velocities_; + + OutputInterface friction_ready_; + + int friction_faulty_count_ = 0; + OutputInterface friction_jammed_; + + double last_primary_friction_velocity_ = nan_; + double primary_friction_velocity_decrease_integral_ = 0; + OutputInterface bullet_fired_; + OutputInterface friction_profile_1_active_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroFrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp new file mode 100644 index 00000000..dfd2df62 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/hero_heat_controller.cpp @@ -0,0 +1,63 @@ +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::shooting { + +class HeroHeatController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HeroHeatController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , heat_per_shot(get_parameter("heat_per_shot").as_int()) + , reserved_heat(get_parameter("reserved_heat").as_int()) { + + register_input("/referee/shooter/cooling", shooter_cooling_); + register_input("/referee/shooter/heat_limit", shooter_heat_limit_); + + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_output( + "/gimbal/control_bullet_allowance/limited_by_heat", control_bullet_allowance_, 0); + register_output("/shoot/heat", shooting_heat_, 0.0); + } + + void update() override { + shooter_heat_ = std::max(0, shooter_heat_ - *shooter_cooling_); + + if (*bullet_fired_) + shooter_heat_ += heat_per_shot; + + *control_bullet_allowance_ = std::max( + 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + *shooting_heat_ = static_cast(shooter_heat_); + } + +private: + InputInterface shooter_cooling_; + InputInterface shooter_heat_limit_; + + InputInterface bullet_fired_; + + const int64_t heat_per_shot; + const int64_t reserved_heat; + + int64_t shooter_heat_ = 0; + OutputInterface shooting_heat_; + + OutputInterface control_bullet_allowance_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::shooting::HeroHeatController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp new file mode 100644 index 00000000..0b0c6375 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/putter_controller.cpp @@ -0,0 +1,407 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" +#include + +namespace rmcs_core::controller::shooting { + +/** + * @class PutterController + * @brief Putter mechanism controller + * + * Firing mechanism notes: + * Since the photoelectric sensor is placed at the chamber opening, testing showed that + * the double-middle action first triggers the putter to retract, and then stall detection + * confirms that the reset is complete. + * By default, a small holding force is applied so the putter does not slide down. The entire + * process uses angle-loop advancement, and grayscale detection determines whether to feed. + * Bullet firing is detected from two sources: the friction wheels and the putter stroke. + * The full scheme completed stress testing before the summer break. + */ +class PutterController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PutterController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + auto set_pid_parameter = [this](pid::PidCalculator& pid, const std::string& name) { + pid.kp = get_parameter(name + "_kp").as_double(); + pid.ki = get_parameter(name + "_ki").as_double(); + pid.kd = get_parameter(name + "_kd").as_double(); + get_parameter(name + "_integral_min", pid.integral_min); + get_parameter(name + "_integral_max", pid.integral_max); + get_parameter(name + "_output_min", pid.output_min); + get_parameter(name + "_output_max", pid.output_max); + }; + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_input("/gimbal/friction_ready", friction_ready_); + + register_input("/gimbal/bullet_feeder/angle", bullet_feeder_angle_); + register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); + + register_input( + "/gimbal/control_bullet_allowance/limited_by_heat", + control_bullet_allowance_limited_by_heat_); + + register_input("/gimbal/photoelectric_sensor", photoelectric_sensor_status_); + register_input("/gimbal/grayscale_sensor", grayscale_sensor_status_); + register_input("/gimbal/bullet_fired", bullet_fired_); + + register_input("/gimbal/putter/angle", putter_angle_); + register_input("/gimbal/putter/velocity", putter_velocity_); + + set_pid_parameter(bullet_feeder_velocity_pid_, "bullet_feeder_velocity"); + set_pid_parameter(putter_return_velocity_pid_, "putter_return_velocity"); + + putter_velocity_pid_.kp = 0.004; + putter_velocity_pid_.ki = 0.0001; + putter_velocity_pid_.kd = 0.001; + putter_velocity_pid_.integral_max = 0.03; + putter_velocity_pid_.integral_min = 0.; + + putter_return_angle_pid.kp = 0.0001; + // putter_return_angle_pid.ki = 0.000001; + putter_return_angle_pid.kd = 0.; + + register_output( + "/gimbal/bullet_feeder/control_torque", bullet_feeder_control_torque_, nan_); + register_output("/gimbal/putter/control_torque", putter_control_torque_, nan_); + + register_output("/gimbal/shoot/delay_ms", shoot_delay_ms_, nan_); + + // auto_aim + register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + + register_output("/gimbal/shooter/mode", shoot_mode_, rmcs_msgs::ShootMode::SINGLE); + register_output("/gimbal/shooter/condiction", shoot_condiction_); + } + + ~PutterController() {} + + void update() override { + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto mouse = *mouse_; + const auto keyboard = *keyboard_; + + using namespace rmcs_msgs; + + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + return; + } + + // Normal control flow after the putter has been initialized. + if (putter_is_initialized_) { + // Handling during bullet-feeder jam-protection cooldown. + if (bullet_feeder_reverse_end_ > 0) { + bullet_feeder_reverse_end_--; + + // Early cooldown stage: reverse the feeder to clear the jam. + if (bullet_feeder_reverse_end_ > 500) + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + -low_latency_velocity_ / 2 - *bullet_feeder_velocity_); + else { + // Late cooldown stage: stop control. + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = 0.0; + } + + } else { + // Normal operating mode: only fire when the friction wheels are ready. + if (*friction_ready_) { + // Detect fire triggers. + if (switch_right != Switch::DOWN) { + + const auto now = std::chrono::steady_clock::now(); + const bool left_click_edge = (!last_mouse_.left && mouse.left); + if (left_click_edge) { + if (now - last_click_time_ < std::chrono::milliseconds(500)) { + click_count_++; + } else { + click_count_ = 1; + } + last_click_time_ = now; + } + + const bool manual_trigger = + (!last_mouse_.left && mouse.left && !mouse.right) + || (last_switch_left_ == rmcs_msgs::Switch::MIDDLE + && switch_left == rmcs_msgs::Switch::DOWN); + + const bool auto_fire_now = + (switch_right == Switch::UP || (mouse.right && mouse.left)) + && (*fire_control_); + + const bool auto_trigger_emergence = mouse.right && (click_count_ >= 2); + + const bool auto_trigger = + auto_fire_now + && (now - last_fire_time_ > std::chrono::milliseconds(1000)); + + if (manual_trigger || auto_trigger || auto_trigger_emergence) { + if (*control_bullet_allowance_limited_by_heat_ > 0 + && (shoot_stage_ == ShootStage::PRELOADED || first_shot_)) { + set_shooting(); + last_fire_time_ = now; + first_shot_ = false; + } + } + if (auto_trigger_emergence) { + click_count_ = 0; + } + } + + if (shoot_stage_ == ShootStage::PRELOADING) { + + *bullet_feeder_control_torque_ = bullet_feeder_velocity_pid_.update( + low_latency_velocity_ - *bullet_feeder_velocity_); // Velocity loop. + + update_locked_detection(); + + // This includes the photoelectric-sensor logic: if triggered, switch to + // preloaded; otherwise reverse briefly and continue rotating. + } + + if (shoot_stage_ == ShootStage::SHOOTING) { + // Firing state: detect whether the bullet has been fired. + if (*bullet_fired_ + || *putter_angle_ - putter_start_point_ >= putter_stroke_) { + shot_fired_ = true; + } + + update_putter_jam_detection(); + + if (shot_fired_) { + // Bullet fired: return the putter. + const auto angle_err = putter_start_point_ - *putter_angle_; + if (angle_err > -0.1) { + *putter_control_torque_ = 0.; + set_preloading(); + shot_fired_ = false; + } else { + *putter_control_torque_ = + putter_return_velocity_pid_.update(-80. - *putter_velocity_); + putter_timeout_detection(); + } + } else { + // Bullet not fired yet: continue advancing. + *putter_control_torque_ = + putter_return_velocity_pid_.update(60. - *putter_velocity_); + } + } + } else { + // Friction wheels not ready: stop the bullet feeder. + *bullet_feeder_control_torque_ = 0.; + } + + // Non-firing state: apply a small holding force to the putter. + if (shoot_stage_ != ShootStage::SHOOTING) + *putter_control_torque_ = -0.02; + } + } else { + // Putter not initialized: perform the reset procedure. + *putter_control_torque_ = putter_return_velocity_pid_.update(-80. - *putter_velocity_); + update_putter_jam_detection(); + } + + // Save the current state for the next comparison. + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_mouse_ = mouse; + last_keyboard_ = keyboard; + } + +private: + void reset_all_controls() { + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_mouse_ = rmcs_msgs::Mouse::zero(); + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + bullet_feeder_velocity_pid_.reset(); + *bullet_feeder_control_torque_ = nan_; + + shoot_stage_ = ShootStage::PRELOADED; + + putter_is_initialized_ = false; + putter_start_point_ = nan_; + putter_return_velocity_pid_.reset(); + putter_velocity_pid_.reset(); + putter_return_angle_pid.reset(); + *putter_control_torque_ = nan_; + + bullet_feeder_faulty_count_ = 0; + + *shoot_delay_ms_ = nan_; + } + + void set_preloading() { shoot_stage_ = ShootStage::PRELOADING; } + + void set_preloaded() { shoot_stage_ = ShootStage::PRELOADED; } + + void set_shooting() { shoot_stage_ = ShootStage::SHOOTING; } + + void update_locked_detection() { + // If feeder speed is near zero and the photoelectric sensor is triggered, + // treat it as locked and start reversing. + + if (*bullet_feeder_velocity_ < 0.5 && *bullet_feeder_control_torque_ > 0.1) { + locked_detect_count_++; + } else { + locked_detect_count_ = 0; + } + + if (locked_detect_count_ > 300) { + if (*photoelectric_sensor_status_) { + set_preloaded(); + } + // If the photoelectric sensor was not triggered, treat it as a simple jam, + // reverse briefly, then continue until stall. + locked_detect_count_ = 0; + enter_jam_protection(); + } + } + + void update_putter_jam_detection() { + if ((*putter_control_torque_ > -0.03 && shoot_stage_ == ShootStage::PRELOADING) + || (*putter_control_torque_ < 0.05 && shoot_stage_ == ShootStage::SHOOTING) + || std::isnan(*putter_control_torque_)) { + putter_faulty_count_ = 0; + return; + } + + // Accumulate a fault count when the torque is abnormal. + if (putter_faulty_count_ < 500) + ++putter_faulty_count_; + else { + putter_faulty_count_ = 0; + if (shoot_stage_ != ShootStage::SHOOTING) { + // Stall detected outside the firing state: the putter is in position, + // so mark it initialized. + putter_is_initialized_ = true; + putter_start_point_ = *putter_angle_; + } else { + // Stall detected during firing: treat the bullet as fired. + shot_fired_ = true; + } + } + } + + void putter_timeout_detection() { + // If the putter stays in the firing state too long without extending, + // treat it as finished and move to the next state. + if (shoot_stage_ == ShootStage::SHOOTING) { + if (shot_fired_) { + if (putter_timeout_count_ < 1600) + ++putter_timeout_count_; + else { + putter_timeout_count_ = 0; + set_preloading(); + shot_fired_ = false; + } + } + } + } + + void enter_jam_protection() { + // Set the target angle to 60 degrees behind the current angle. + locked_detect_count_ = 0; + bullet_feeder_faulty_count_ = 0; + bullet_feeder_reverse_end_ = 800; + bullet_feeder_velocity_pid_.reset(); + } + + static constexpr double nan_ = + std::numeric_limits::quiet_NaN(); ///< Not-a-number constant. + static constexpr double inf_ = std::numeric_limits::infinity(); ///< Infinity constant. + + static constexpr double putter_stroke_ = 11.5; ///< Putter stroke length. + + static constexpr double max_bullet_feeder_control_torque_ = 0.1; + static constexpr double bullet_feeder_angle_per_bullet_ = 2 * std::numbers::pi / 6; + static constexpr double low_latency_velocity_ = 5.0; + + InputInterface photoelectric_sensor_status_; + InputInterface grayscale_sensor_status_; + InputInterface bullet_fired_; + bool shot_fired_{false}; + bool first_shot_{true}; + + InputInterface friction_ready_; + + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_; + InputInterface keyboard_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Mouse last_mouse_ = rmcs_msgs::Mouse::zero(); + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface bullet_feeder_angle_; + InputInterface bullet_feeder_velocity_; + + InputInterface control_bullet_allowance_limited_by_heat_; + + bool putter_is_initialized_ = false; + int putter_faulty_count_ = 0; + int putter_timeout_count_ = 0; + double putter_start_point_ = nan_; + pid::PidCalculator putter_return_velocity_pid_; + InputInterface putter_velocity_; + + pid::PidCalculator putter_velocity_pid_; + + enum class ShootStage { PRELOADING, PRELOADED, SHOOTING }; + ShootStage shoot_stage_ = ShootStage::PRELOADING; + + pid::PidCalculator bullet_feeder_velocity_pid_; + + OutputInterface bullet_feeder_control_torque_; + + InputInterface putter_angle_; + pid::PidCalculator putter_return_angle_pid; + OutputInterface putter_control_torque_; + + int bullet_feeder_faulty_count_ = 0; + + OutputInterface shoot_delay_ms_; + + InputInterface fire_control_; + std::chrono::steady_clock::time_point last_fire_time_{}; + std::chrono::steady_clock::time_point last_click_time_{}; + int click_count_ = 0; + + int locked_detect_count_ = 0; + int bullet_feeder_reverse_end_ = 0; + + InputInterface bullet_feeder_torque; + InputInterface putter_torque; + + OutputInterface shoot_mode_; + OutputInterface shoot_condiction_; +}; + +} // namespace rmcs_core::controller::shooting + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::PutterController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 5500b15c..6cefe275 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -1,7 +1,10 @@ +#include +#include #include #include #include #include +#include namespace rmcs_core::controller::shooting { @@ -18,40 +21,26 @@ class ShootingRecorder log_mode_ = static_cast(get_parameter("log_mode").as_int()); + aim_velocity = get_parameter("aim_velocity").as_double(); + register_input("/referee/shooter/initial_speed", initial_speed_); register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); - register_input("/friction_wheels/temperature", fractional_temperature_); - - if (friction_wheel_count_ == 2) { - const auto topic = std::array{ - "/gimbal/left_friction/control_velocity", - "/gimbal/right_friction/control_velocity", - }; - for (int i = 0; i < 2; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } else if (friction_wheel_count_ == 4) { - const auto topic = std::array{ - "/gimbal/first_left_friction/control_velocity", - "/gimbal/first_right_friction/control_velocity", - "/gimbal/second_left_friction/control_velocity", - "/gimbal/second_right_friction/control_velocity", - }; - for (int i = 0; i < 4; i++) - register_input(topic[i], friction_wheels_velocity_[i]); - } using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); + + std::ofstream out_file("shoot_recorder"); + RCLCPP_INFO(get_logger(), "ShootingRecorder initialized, log file: %s", file.c_str()); } ~ShootingRecorder() { log_stream_.close(); } void update() override { - + // Evaluate friction-wheel quality. switch (log_mode_) { case LogMode::TRIGGER: // It will be triggered by shooting action @@ -64,38 +53,52 @@ class ShootingRecorder return; break; } + v = *shoot_timestamp_; + + static constexpr size_t max_velocities_size = 1000; + + velocities.push_back(*initial_speed_); + if (velocities.size() > max_velocities_size) { + velocities.erase(velocities.begin()); + } - auto log_text = std::string{}; + analysis3(); + + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); - if (friction_wheel_count_ == 2) { - log_text = fmt::format( - "{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *fractional_temperature_); - } else if (friction_wheel_count_ == 4) { + if (friction_wheel_count_ == 4) { log_text = fmt::format( - "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // - *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], - *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], - *fractional_temperature_); + "{},{},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f},{:.3f}", *initial_speed_, + (int)velocities.size(), // + velocity_, excellence_rate_, pass_rate_, range_, range2_, velocity_max, + velocity_min); } log_stream_ << log_text << std::endl; - RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); + RCLCPP_INFO( + get_logger(), "%s", + log_text.c_str()); // Record each shot's initial speed and statistics. + + log_velocity = fmt::format("{:.3f}", *initial_speed_); + std::ofstream out_file("shoot_recorder", std::ios::app); + if (out_file.is_open()) { + out_file << log_velocity << std::endl; + out_file.close(); + } last_shoot_timestamp_ = *shoot_timestamp_; } private: /// @brief Component interface + std::array, 2> friction_velocities_; + InputInterface initial_speed_; InputInterface shoot_timestamp_; - InputInterface fractional_temperature_; - std::size_t friction_wheel_count_ = 2; - std::array, 4> friction_wheels_velocity_; + std::array, 2> friction_wheels_velocity_; /// @brief For log enum class LogMode { TRIGGER = 1, TIMING = 2 }; @@ -103,29 +106,153 @@ class ShootingRecorder double last_shoot_timestamp_ = 0; std::ofstream log_stream_; + std::string log_velocity; std::size_t log_count_ = 0; + std::vector velocities; + + double velocity_; + + double excellence_rate_; + double pass_rate_; + + double range_; + double range2_; + + double velocity_min; + double velocity_max; + + double v; + double aim_velocity; + private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); return result; } + void analysis1() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + + range_ = velocities.back() - velocities.front(); + + if (velocities.size() >= 3) { + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } else { + range2_ = range_; + } + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis2() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + + sort(velocities.begin(), velocities.end()); + + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + int n_adjust = std::max(1, int((int)velocities.size() * 0.05)); + + for (int i = 0; i < n_adjust; i++) { + sum -= velocities[i]; + sum -= velocities[velocities.size() - 1 - i]; + } + + velocity_ = sum / double(velocities.size() - 2 * n_adjust); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + + int excellence_count = 0; + int pass_count = 0; + for (int i = 0; i < int(velocities.size()); i++) { + if (velocities[i] >= velocity_ - 0.1 && velocities[i] <= velocity_ + 0.1) { + pass_count += 1; + } + if (velocities[i] >= velocity_ - 0.05 && velocities[i] <= velocity_ + 0.05) { + excellence_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + } + + void analysis3() { + double sum = 0.0; + for (const auto& v : velocities) { + sum += v; + } + velocity_ = sum / double(velocities.size()); + + int excellence_count = 0; + int pass_count = 0; + + for (const auto& v : velocities) { + if (v >= aim_velocity - 0.05 && v <= aim_velocity + 0.05) { + excellence_count += 1; + } + if (v >= aim_velocity - 0.1 && v <= aim_velocity + 0.1) { + pass_count += 1; + } + } + excellence_rate_ = double(excellence_count) / double(velocities.size()); + pass_rate_ = double(pass_count) / double(velocities.size()); + + sort(velocities.begin(), velocities.end()); + velocity_max = velocities.back(); + velocity_min = velocities.front(); + + range_ = velocities.back() - velocities.front(); + range2_ = velocities[int(velocities.size() - 2)] - velocities[1]; + } + + static double GetTime() { + using namespace std::chrono; + static auto start = high_resolution_clock::now(); + auto now = high_resolution_clock::now(); + duration elapsed = now - start; + return elapsed.count(); + } }; } // namespace rmcs_core::controller::shooting #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp index fcb9a32a..0f6d9dd2 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/alpha_beta_angle_filter.hpp @@ -11,7 +11,9 @@ namespace rmcs_core::filter { class AlphaBetaAngleFilter { public: AlphaBetaAngleFilter(double dt, double alpha, double beta) - : dt_(dt), alpha_(alpha), beta_(beta) {} + : dt_(dt) + , alpha_(alpha) + , beta_(beta) {} void reset() { initialized_ = false; @@ -51,9 +53,7 @@ class AlphaBetaAngleFilter { bool initialized() const { return initialized_; } private: - static double wrap_angle_(double x) { - return std::remainder(x, 2.0 * std::numbers::pi); - } + static double wrap_angle_(double x) { return std::remainder(x, 2.0 * std::numbers::pi); } double dt_; double alpha_; diff --git a/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp new file mode 100644 index 00000000..e7e98cf0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/imu_ekf.hpp @@ -0,0 +1,428 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::filter { + +class ImuEkf { +public: + using Vec3 = Eigen::Vector3d; + using Vec4 = Eigen::Vector4d; + using Mat3 = Eigen::Matrix3d; + using Mat4 = Eigen::Matrix4d; + using Mat43 = Eigen::Matrix; + using Mat34 = Eigen::Matrix; + + struct Config { + Mat3 process_noise = 1.22e-3 * Mat3::Identity(); + Mat3 measurement_noise = 50.0 * Mat3::Identity(); + Mat3 gate_noise = 0.02 * Mat3::Identity(); + + Mat4 initial_covariance = 0.15 * Mat4::Identity(); + }; + + class AccelCorrection { + public: + [[nodiscard]] double chi_square() const noexcept { return chi_square_; } + + private: + friend class ImuEkf; + + AccelCorrection( + const std::uint64_t revision, Vec3 innovation, Mat34 jacobian_h_x, + Eigen::LDLT innovation_covariance_ldlt, const double chi_square) + : revision_(revision) + , innovation_(std::move(innovation)) + , jacobian_h_x_(std::move(jacobian_h_x)) + , innovation_covariance_ldlt_(std::move(innovation_covariance_ldlt)) + , chi_square_(chi_square) {} + + std::uint64_t revision_ = 0; + Vec3 innovation_ = Vec3::Zero(); + Mat34 jacobian_h_x_ = Mat34::Zero(); + Eigen::LDLT innovation_covariance_ldlt_; + double chi_square_ = std::numeric_limits::quiet_NaN(); + }; + + ImuEkf() + : ImuEkf(Config{}) {} + + explicit ImuEkf(Config config) + : config_(std::move(config)) { + reset(); + } + + [[nodiscard]] const Config& config() const noexcept { return config_; } + + void set_config(const Config& config) { + config_ = config; + reset(); + } + + void reset() { + state_ = identity_quaternion(); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + } + + [[nodiscard]] bool reset_from_accel(const Vec3& accel_g, const double initial_yaw = 0.0) { + if (!accel_g.allFinite() || !std::isfinite(initial_yaw)) { + return false; + } + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) { + return false; + } + + const double gx = (*normalized_accel)(0); + const double gy = (*normalized_accel)(1); + const double gz = std::clamp((*normalized_accel)(2), -1.0, 1.0); + + double q0_val, q1_val, q2_val, q3_val; + if (gz >= 0.0) { + const double s = std::sqrt(0.5 * (1.0 + gz)); + const double inv = 0.5 / s; + q0_val = s; + q1_val = gy * inv; + q2_val = -gx * inv; + q3_val = 0.0; + } else { + const double s = std::sqrt(0.5 * (1.0 - gz)); + const double inv = 0.5 / s; + q0_val = gy * inv; + q1_val = s; + q2_val = 0.0; + q3_val = gx * inv; + } + + const double cy = std::cos(initial_yaw * 0.5); + const double sy = std::sin(initial_yaw * 0.5); + + state_(0) = cy * q0_val - sy * q3_val; + state_(1) = cy * q1_val - sy * q2_val; + state_(2) = cy * q2_val + sy * q1_val; + state_(3) = cy * q3_val + sy * q0_val; + + state_ = normalize_quaternion(state_); + covariance_ = projected_covariance(state_, config_.initial_covariance); + ++revision_; + return true; + } + + void inflate_attitude_uncertainty_to_initial() { + covariance_ = covariance_with_psd_floor( + covariance_, projected_covariance(state_, config_.initial_covariance)); + ++revision_; + } + + [[nodiscard]] bool predict(const Vec3& gyro_rad_per_sec, const double dt_seconds) { + if (!gyro_rad_per_sec.allFinite()) { + return false; + } + + if (!std::isfinite(dt_seconds) || dt_seconds < 0.0) { + return false; + } + + const PropagationResult propagation = + propagate_quaternion(state_, gyro_rad_per_sec, dt_seconds); + if (!propagation.quaternion.allFinite() || !propagation.orthogonalization.allFinite()) { + return false; + } + const Vec4 prior_state = propagation.quaternion; + const Mat4& orthogonalization = propagation.orthogonalization; + + const Mat4 jacobian_f_x = state_jacobian(gyro_rad_per_sec, dt_seconds, orthogonalization); + const Mat43 jacobian_f_w = process_noise_jacobian(state_, orthogonalization); + Mat4 prior_cov = symmetrized_mat4( + jacobian_f_x * covariance_ * jacobian_f_x.transpose() + + dt_seconds * jacobian_f_w * config_.process_noise * jacobian_f_w.transpose()); + if (!is_positive_semidefinite(prior_cov)) { + return false; + } + + state_ = prior_state; + covariance_ = prior_cov; + ++revision_; + return true; + } + + [[nodiscard]] std::optional prepare_correction(const Vec3& accel_g) const { + if (!accel_g.allFinite()) + return std::nullopt; + + const std::optional normalized_accel = normalize_vector(accel_g); + if (!normalized_accel.has_value()) + return std::nullopt; + + const Vec3 predicted_accel = measurement_model(state_); + const Vec3 gate_innovation = accel_g - predicted_accel; + const Vec3 update_innovation = *normalized_accel - predicted_accel; + const Mat34 jacobian_h_x = measurement_jacobian(state_); + const Mat3 projected_covariance = jacobian_h_x * covariance_ * jacobian_h_x.transpose(); + const Mat3 gate_covariance = symmetrized_mat3(projected_covariance + config_.gate_noise); + + if (!is_positive_definite(gate_covariance)) { + return std::nullopt; + } + + const auto gate_ldt = gate_covariance.ldlt(); + if (gate_ldt.info() != Eigen::Success || !gate_ldt.isPositive()) { + return std::nullopt; + } + const Vec3 innovation_weighted = gate_ldt.solve(gate_innovation); + if (!innovation_weighted.allFinite()) { + return std::nullopt; + } + const double chi_square = gate_innovation.dot(innovation_weighted); + if (!std::isfinite(chi_square)) { + return std::nullopt; + } + + const Mat3 update_covariance = + symmetrized_mat3(projected_covariance + config_.measurement_noise); + if (!is_positive_definite(update_covariance)) { + return std::nullopt; + } + + const auto update_ldt = update_covariance.ldlt(); + if (update_ldt.info() != Eigen::Success || !update_ldt.isPositive()) { + return std::nullopt; + } + + return AccelCorrection(revision_, update_innovation, jacobian_h_x, update_ldt, chi_square); + } + + bool correct(const AccelCorrection& update) { + if (update.revision_ != revision_) { + return false; + } + const Vec4 state_prior = state_; + const Mat34 k_transpose = + update.innovation_covariance_ldlt_.solve(update.jacobian_h_x_ * covariance_); + if (!k_transpose.allFinite()) { + return false; + } + const Eigen::Matrix kalman_gain = k_transpose.transpose(); + Mat4 state_update_mask = Mat4::Identity(); + state_update_mask(3, 3) = 0.0; + const Eigen::Matrix effective_gain = state_update_mask * kalman_gain; + + const Vec4 quaternion_raw = state_prior + effective_gain * update.innovation_; + if (!quaternion_raw.allFinite() || quaternion_raw.squaredNorm() <= kEpsilon) { + return false; + } + + const Mat4 matrix_tmp = Mat4::Identity() - effective_gain * update.jacobian_h_x_; + Mat4 corrected_covariance = + matrix_tmp * covariance_ * matrix_tmp.transpose() + + effective_gain * config_.measurement_noise * effective_gain.transpose(); + + const Mat4 j_norm = orthogonalization_jacobian(quaternion_raw); + corrected_covariance = symmetrized_mat4(j_norm * corrected_covariance * j_norm.transpose()); + + if (!is_positive_semidefinite(corrected_covariance)) { + return false; + } + state_ = normalize_quaternion(quaternion_raw); + covariance_ = corrected_covariance; + ++revision_; + return true; + } + + bool correct(const Vec3& accel_mps2) { + const std::optional update = prepare_correction(accel_mps2); + return update.has_value() && correct(*update); + } + + [[nodiscard]] Eigen::Quaterniond quaternion() const noexcept { + return {state_[0], state_[1], state_[2], state_[3]}; + } + +private: + struct PropagationResult { + Vec4 quaternion = identity_quaternion(); + Mat4 orthogonalization = Mat4::Identity(); + }; + + [[nodiscard]] static Vec4 identity_quaternion() { + Vec4 q; + q << 1.0, 0.0, 0.0, 0.0; + return q; + } + + [[nodiscard]] static std::optional normalize_vector(const Vec3& vector) { + const double squared_norm = vector.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return std::nullopt; + } + + return vector / std::sqrt(squared_norm); + } + + [[nodiscard]] static Vec4 normalize_quaternion(const Vec4& quaternion) { + const double squared_norm = quaternion.squaredNorm(); + if (!std::isfinite(squared_norm) || squared_norm <= kEpsilon) { + return identity_quaternion(); + } + + return quaternion / std::sqrt(squared_norm); + } + + [[nodiscard]] static Mat3 symmetrized_mat3(const Mat3& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static Mat4 symmetrized_mat4(const Mat4& matrix) { + return 0.5 * (matrix + matrix.transpose()); + } + + [[nodiscard]] static bool is_positive_definite(const Mat3& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat3(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() > tolerance; + } + + [[nodiscard]] static bool is_positive_semidefinite(const Mat4& matrix) { + if (!matrix.allFinite()) { + return false; + } + + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return false; + } + + const double max_abs_eigenvalue = solver.eigenvalues().cwiseAbs().maxCoeff(); + const double tolerance = kEpsilon * std::max(1.0, max_abs_eigenvalue); + return solver.eigenvalues().minCoeff() >= -tolerance; + } + + [[nodiscard]] static Mat4 positive_semidefinite_part(const Mat4& matrix) { + const Eigen::SelfAdjointEigenSolver solver(symmetrized_mat4(matrix)); + if (solver.info() != Eigen::Success || !solver.eigenvalues().allFinite()) { + return Mat4::Zero(); + } + + return symmetrized_mat4( + solver.eigenvectors() * solver.eigenvalues().cwiseMax(0.0).asDiagonal() + * solver.eigenvectors().transpose()); + } + + [[nodiscard]] static Mat4 + covariance_with_psd_floor(const Mat4& covariance, const Mat4& covariance_floor) { + Mat4 floor = symmetrized_mat4(covariance_floor); + Mat4 result = floor + positive_semidefinite_part(symmetrized_mat4(covariance) - floor); + result = symmetrized_mat4(result); + if (is_positive_semidefinite(result)) { + return result; + } + + return floor; + } + + [[nodiscard]] static Mat4 + projected_covariance(const Vec4& quaternion, const Mat4& raw_covariance) { + const Mat4 j_norm = orthogonalization_jacobian(quaternion); + Mat4 covariance = symmetrized_mat4(j_norm * raw_covariance * j_norm.transpose()); + if (is_positive_semidefinite(covariance)) { + return covariance; + } + + return symmetrized_mat4(j_norm * Mat4::Identity() * j_norm.transpose()); + } + + [[nodiscard]] static Mat4 omega_matrix(const Vec3& gyro_rad_per_sec) { + const double gx = gyro_rad_per_sec(0); + const double gy = gyro_rad_per_sec(1); + const double gz = gyro_rad_per_sec(2); + Mat4 matrix; + // clang-format off + matrix << 0, -gx, -gy, -gz, + gx, 0, gz, -gy, + gy, -gz, 0, gx, + gz, gy, -gx, 0; + // clang-format on + return matrix; + } + + [[nodiscard]] static Mat4 orthogonalization_jacobian(const Vec4& quaternion) { + const double norm = quaternion.norm(); + const double inv_norm = (std::isfinite(norm) && norm > kEpsilon) ? 1.0 / norm : 1.0; + return inv_norm + * (Mat4::Identity() - inv_norm * inv_norm * quaternion * quaternion.transpose()); + } + + [[nodiscard]] static PropagationResult + propagate_quaternion(const Vec4& state, const Vec3& gyro_rad_per_sec, const double dt) { + const Mat4 omega = omega_matrix(gyro_rad_per_sec); + const Vec4 quaternion_tmp = state + 0.5 * dt * omega * state; + + PropagationResult result; + result.orthogonalization = orthogonalization_jacobian(quaternion_tmp); + result.quaternion = normalize_quaternion(quaternion_tmp); + return result; + } + + [[nodiscard]] static Mat4 state_jacobian( + const Vec3& gyro_rad_per_sec, const double dt, const Mat4& orthogonalization) { + return orthogonalization * (Mat4::Identity() + 0.5 * dt * omega_matrix(gyro_rad_per_sec)); + } + + [[nodiscard]] static Mat43 + process_noise_jacobian(const Vec4& state, const Mat4& orthogonalization) { + Mat43 matrix_q; + // clang-format off + matrix_q << -state(1), -state(2), -state(3), + state(0), -state(3), state(2), + state(3), state(0), -state(1), + -state(2), state(1), state(0); + // clang-format on + return orthogonalization * (0.5 * matrix_q); + } + + [[nodiscard]] static Vec3 measurement_model(const Vec4& state) { + Vec3 result; + result(0) = 2.0 * (state(1) * state(3) - state(0) * state(2)); + result(1) = 2.0 * (state(2) * state(3) + state(0) * state(1)); + result(2) = + state(0) * state(0) - state(1) * state(1) - state(2) * state(2) + state(3) * state(3); + return result; + } + + [[nodiscard]] static Mat34 measurement_jacobian(const Vec4& state) { + Mat34 result; + // clang-format off + result << -2.0 * state(2), 2.0 * state(3), -2.0 * state(0), 2.0 * state(1), + 2.0 * state(1), 2.0 * state(0), 2.0 * state(3), 2.0 * state(2), + 2.0 * state(0), -2.0 * state(1), -2.0 * state(2), 2.0 * state(3); + // clang-format on + return result; + } + + static constexpr double kEpsilon = 1e-12; + + Config config_; + Vec4 state_ = identity_quaternion(); + Mat4 covariance_ = Mat4::Identity(); + std::uint64_t revision_ = 0; +}; + +} // namespace rmcs_core::filter diff --git a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp index 126eb4f4..2d579f0d 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp @@ -55,7 +55,7 @@ requires(variable_number > 0) class LowPassFilter { void set_cutoff(double cutoff_freq, double sampling_freq) { double dt = 1.0 / sampling_freq; double rc = 1.0 / (2 * std::numbers::pi * cutoff_freq); - alpha_ = dt / (dt + rc); + alpha_ = dt / (dt + rc); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp index 613c6f61..23422445 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp @@ -45,7 +45,7 @@ class Benewake { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - *distance_ = package.calculate_distance(); + *distance_ = package.calculate_distance(); signal_strength_ = package.calculate_signal_strength(); } 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 96d84a2d..31a44cbe 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp @@ -150,12 +150,8 @@ class Bmi088 { 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} - }; + 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; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp new file mode 100644 index 00000000..83c1b2e3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088_ekf.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "filter/imu_ekf.hpp" + +namespace rmcs_core::hardware::device { + +class Bmi088Ekf { +public: + struct Config { + Eigen::Matrix3d body_to_sensor = Eigen::Matrix3d::Identity(); + }; + + using TimePoint = rmcs_msgs::BoardClock::time_point; + using Snapshot = rmcs_msgs::ImuSnapshot; + + explicit Bmi088Ekf(Config config) + : config_(std::move(config)) {} + + Bmi088Ekf() + : Bmi088Ekf(Config{}) {} + + void push_accelerometer_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d accel_g = + config_.body_to_sensor.transpose() * convert_accelerometer(x, y, z); + + if (!initialized_) { + if (ekf_.reset_from_accel(accel_g)) { + ekf_state_time_ = sample_time; + latest_snapshot_ = { + ekf_.quaternion(), + Eigen::Vector3d::Zero(), + ekf_state_time_, + }; + const auto guard = std::scoped_lock{mutex_}; + initialized_ = true; + } + return; + } + + if (sample_time < ekf_state_time_) + return; + + if (pending_accel_sample_ && sample_time < pending_accel_sample_->sample_time) + return; + + pending_accel_sample_ = {accel_g, sample_time}; + } + + std::optional try_update_with_gyroscope_sample( + std::int16_t x, std::int16_t y, std::int16_t z, TimePoint sample_time) { + const Eigen::Vector3d gyro_rad_per_sec = + config_.body_to_sensor.transpose() * convert_gyroscope(x, y, z); + + if (!initialized_) + return std::nullopt; + + if (sample_time < ekf_state_time_) + return std::nullopt; + + if (is_gyro_saturated(gyro_rad_per_sec)) + ekf_.inflate_attitude_uncertainty_to_initial(); + + while (pending_accel_sample_) { + const auto& accel_sample_time = pending_accel_sample_->sample_time; + if (accel_sample_time < ekf_state_time_ || accel_sample_time > sample_time) + break; + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{accel_sample_time - ekf_state_time_}.count())) + break; + ekf_state_time_ = accel_sample_time; + + const auto correction = ekf_.prepare_correction(pending_accel_sample_->accel_g); + if (!correction || correction->chi_square() >= 3.0) + break; + if (!ekf_.correct(*correction)) + break; + + pending_accel_sample_ = std::nullopt; + break; + } + + // Guard against stale IMU frames that librmcs may deliver right after reconnect before the + // device-side buffer is drained. Integrating a frame with a large timestamp jump can inject + // a huge bogus gyro delta, so drop it instead of advancing the filter. + // TODO: Remove this once librmcs guarantees buffered historical IMU frames are flushed on + // connection. + if (std::chrono::duration{sample_time - ekf_state_time_}.count() > 1 / 1000.0) { + ekf_state_time_ = sample_time; + return std::nullopt; + } + + if (!ekf_.predict( + gyro_rad_per_sec, + std::chrono::duration{sample_time - ekf_state_time_}.count())) + return std::nullopt; + ekf_state_time_ = sample_time; + + auto snapshot = Snapshot{ + ekf_.quaternion(), + gyro_rad_per_sec, + ekf_state_time_, + }; + { + const auto guard = std::scoped_lock{mutex_}; + latest_snapshot_ = snapshot; + } + return snapshot; + } + + [[nodiscard]] bool initialized() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + return initialized_; + } + + [[nodiscard]] std::optional snapshot() const noexcept { + const auto guard = std::scoped_lock{mutex_}; + if (!initialized_) + return std::nullopt; + return latest_snapshot_; + } + +private: + [[nodiscard]] static Eigen::Vector3d + convert_accelerometer(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 6.0; + } + + [[nodiscard]] static Eigen::Vector3d + convert_gyroscope(std::int16_t x, std::int16_t y, std::int16_t z) noexcept { + return Eigen::Vector3d{ + static_cast(x), static_cast(y), static_cast(z)} + / 32767.0 * 2000.0 / 180.0 * std::numbers::pi; + } + + [[nodiscard]] static bool is_gyro_saturated(const Eigen::Vector3d& gyro_rad_per_sec) noexcept { + return gyro_rad_per_sec.cwiseAbs().maxCoeff() >= 0.98 * 2000.0 / 180.0 * std::numbers::pi; + } + + const Config config_; + + mutable std::mutex mutex_; + bool initialized_ = false; + + filter::ImuEkf ekf_; + TimePoint ekf_state_time_; + + struct AccelSample { + Eigen::Vector3d accel_g; + TimePoint sample_time; + }; + std::optional pending_accel_sample_; + + Snapshot latest_snapshot_; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp new file mode 100644 index 00000000..1706ea2a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/board_clock_lifter.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +#include + +namespace rmcs_core::hardware::device { + +class BoardClockLifter { +public: + using time_point = rmcs_msgs::BoardClock::time_point; + + time_point advance_timebase(std::uint32_t raw_timestamp_quarter_us) { + if (!has_latest_timebase_) { + has_latest_timebase_ = true; + last_timebase_raw_ = raw_timestamp_quarter_us; + latest_timebase_timestamp_ = raw_timestamp_quarter_us; + } + + latest_timebase_timestamp_ += + static_cast(raw_timestamp_quarter_us - last_timebase_raw_); + last_timebase_raw_ = raw_timestamp_quarter_us; + + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto timebase() const -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + return time_point{rmcs_msgs::BoardClock::duration{latest_timebase_timestamp_}}; + } + + [[nodiscard]] auto lift_timestamp(std::uint32_t timestamp_quarter_us) const + -> std::optional { + if (!has_latest_timebase_) + return std::nullopt; + + const auto latest_timestamp_low32 = static_cast(latest_timebase_timestamp_); + const auto signed_offset = + static_cast(timestamp_quarter_us - latest_timestamp_low32); + const auto lifted_timestamp = + latest_timebase_timestamp_ + static_cast(signed_offset); + return time_point{rmcs_msgs::BoardClock::duration{lifted_timestamp}}; + } + +private: + bool has_latest_timebase_ = false; + std::uint32_t last_timebase_raw_ = 0; + std::int64_t latest_timebase_timestamp_ = 0; +}; + +} // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp index df527150..e5b8d7b3 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp @@ -34,9 +34,9 @@ class Gy614 { void update_status() { const auto package = package_.load(std::memory_order::relaxed); - emissivity_ = package.calculate_emissivity(); - *target_temperature_ = package.calculate_target_temperature(); - environment_temperature_ = package.calculate_environment_temperature(); + emissivity_ = package.calculate_emissivity(); + *target_temperature_ = package.calculate_target_temperature(); + environment_temperature_ = package.calculate_environment_temperature(); body_temperature_from_forhead_ = package.calculate_body_temperature_from_forhead(); } @@ -74,7 +74,7 @@ class Gy614 { double emissivity_ = 0; Component::OutputInterface target_temperature_; - double environment_temperature_ = 0; + double environment_temperature_ = 0; double body_temperature_from_forhead_ = 0; }; 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 7ab8a38f..1bc0ef8a 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 @@ -43,6 +43,7 @@ class LkMotor { rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) { status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/raw_angle", raw_angle_output_, 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); @@ -75,7 +76,7 @@ class LkMotor { switch (config.motor_type) { case Type::kMG5010Ei10: raw_angle_modulus_ = 1 << 16; - torque_constant = 0.90909; + torque_constant = 0.1; reduction_ratio = 10.0; // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. @@ -93,7 +94,7 @@ class LkMotor { break; case Type::kMG6012Ei8: raw_angle_modulus_ = 1 << 16; - torque_constant = 1.09; + torque_constant = 1.09 / 8.0; reduction_ratio = 8.0; max_torque_ = 16.0; break; @@ -193,6 +194,7 @@ class LkMotor { torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); *angle_output_ = angle(); + *raw_angle_output_ = last_raw_angle(); *velocity_output_ = velocity(); *torque_output_ = torque(); *temperature_output_ = temperature(); @@ -521,6 +523,7 @@ class LkMotor { double temperature_; rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface raw_angle_output_; rmcs_executor::Component::OutputInterface velocity_output_; rmcs_executor::Component::OutputInterface torque_output_; rmcs_executor::Component::OutputInterface temperature_output_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 3c01ac5a..69ddef42 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -2,12 +2,12 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -16,18 +16,18 @@ #include #include #include +#include #include #include #include -#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.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/remote_control.hpp" #include "hardware/device/supercap.hpp" -#include "hardware/device/vt13.hpp" namespace rmcs_core::hardware { @@ -40,7 +40,7 @@ class OmniInfantry : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::RmcsBoardLite(get_parameter("board_serial").as_string(), {true}) + , librmcs::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} , logger_(get_logger()) , infantry_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -55,8 +55,7 @@ class OmniInfantry , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , remote_control_(*this, dr16_, vt13_) - , bmi088_(1000, 0.2, 0.0) { + , dr16_{*this} { for (auto& motor : chassis_wheel_motors_) motor.configure( @@ -87,19 +86,20 @@ class OmniInfantry register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/gimbal/auto_aim/exposure_signal", camera_signal_output_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); 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); - }); + start_transmit().gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx, + { + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, + .capture_timestamp = true, + .pull = librmcs::data::GpioPull::kUp, + }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); @@ -145,8 +145,6 @@ class OmniInfantry update_motors(); update_imu(); dr16_.update_status(); - vt13_.update_status(); - remote_control_.update(); supercap_.update_status(); } @@ -221,14 +219,15 @@ class OmniInfantry } void update_imu() { - bmi088_.update_status(); - Eigen::Quaterniond const gimbal_imu_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + const auto snapshot = bmi088_.snapshot(); + if (!snapshot) + return; + tf_->set_transform( - gimbal_imu_pose.conjugate()); + snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); } void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { @@ -280,8 +279,19 @@ class OmniInfantry } } - void uart0_receive_callback(const librmcs::data::UartDataView& data) override { - vt13_.store_status(data.uart_data); + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio != librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx) + return; + if (!data.timestamp_quarter_us) + return; + + const auto timestamp = board_clock_lifter_.lift_timestamp(*data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + camera_signal_output_.emit(*timestamp); } void uart1_receive_callback(const librmcs::data::UartDataView& data) override { @@ -296,11 +306,21 @@ class OmniInfantry } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } private: @@ -331,12 +351,13 @@ class OmniInfantry device::DjiMotor gimbal_bullet_feeder_; device::Dr16 dr16_; - device::Vt13 vt13_; - device::RemoteControl remote_control_; - device::Bmi088 bmi088_; + device::Bmi088Ekf bmi088_; + device::BoardClockLifter board_clock_lifter_; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; + EventOutputInterface camera_signal_output_; + EventOutputInterface imu_snapshot_output_; OutputInterface tf_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp new file mode 100644 index 00000000..40150e60 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -0,0 +1,865 @@ +#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/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +class CanReceiveRateCounter { +public: + explicit CanReceiveRateCounter(rclcpp::Logger logger, std::string_view channel_name) + : logger_(std::move(logger)) + , channel_name_(channel_name) {} + + void record(std::uint32_t can_id) { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + auto& status = statuses_[can_id]; + ++status.receive_count; + status.last_receive_time = now; + + report_if_due(now); + } + + void report_if_due() { + const auto now = Clock::now(); + + std::lock_guard lock{mutex_}; + report_if_due(now); + } + +private: + using Clock = std::chrono::steady_clock; + + struct Status { + std::size_t receive_count{0}; + Clock::time_point last_receive_time{}; + }; + + void report_if_due(Clock::time_point now) { + if (statuses_.empty()) + return; + + if (last_report_time_ == Clock::time_point{}) { + last_report_time_ = now; + return; + } + + const auto elapsed = now - last_report_time_; + if (elapsed < kReportInterval) + return; + + const auto elapsed_seconds = std::chrono::duration(elapsed).count(); + for (auto& [can_id, status] : statuses_) { + const bool attached = now - status.last_receive_time <= kMissTimeout; + RCLCPP_INFO( + logger_, "[can rx] %.*s id=0x%03X rate=%.1fHz status=%s", + static_cast(channel_name_.size()), channel_name_.data(), + static_cast(can_id), + static_cast(status.receive_count) / elapsed_seconds, + attached ? "attach" : "miss"); + status.receive_count = 0; + } + + last_report_time_ = now; + } + + static constexpr std::chrono::milliseconds kReportInterval{1000}; + static constexpr std::chrono::milliseconds kMissTimeout{1000}; + + rclcpp::Logger logger_; + std::string_view channel_name_; + std::mutex mutex_; + Clock::time_point last_report_time_{}; + std::map statuses_; +}; + +class SteeringHeroLittle + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SteeringHeroLittle() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component( + get_component_name() + "_command", *this)) { + + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("serial_bottom_rmcs_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + + ~SteeringHeroLittle() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + + tf_->set_state( + bottom_board_->gimbal_bottom_yaw_motor_.angle() + + top_board_->gimbal_top_yaw_motor_.angle()); + tf_->set_state( + top_board_->gimbal_pitch_motor_.angle()); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New player viewer offset: %ld", + top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New bullet feeder offset: %ld", + top_board_->gimbal_bullet_feeder_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left front steering offset: %d", + bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right front steering offset: %d", + bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] left back steering offset: %d", + bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[chassis calibration] right back steering offset: %d", + bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); + } + + class SteeringHeroLittleCommand : public rmcs_executor::Component { + public: + explicit SteeringHeroLittleCommand(SteeringHeroLittle& hero) + : hero_(hero) {} + + void update() override { hero_.command_update(); } + + SteeringHeroLittle& hero_; + }; + std::shared_ptr command_component_; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit TopBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite(board_serial) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , tf_(steering_hero.tf_) + , imu_(1000, 0.2, 0.0) + , gimbal_top_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/top_yaw") + , gimbal_pitch_motor_(steering_hero, steering_hero_command, "/gimbal/pitch") + , gimbal_friction_wheels_( + {steering_hero, steering_hero_command, "/gimbal/first_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/first_left_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_right_friction"}, + {steering_hero, steering_hero_command, "/gimbal/second_left_friction"}) + , gimbal_bullet_feeder_(steering_hero, steering_hero_command, "/gimbal/bullet_feeder") + , putter_motor_(steering_hero, steering_hero_command, "/gimbal/putter") + , gimbal_scope_motor_(steering_hero, steering_hero_command, "/gimbal/scope") + , gimbal_player_viewer_motor_( + steering_hero, steering_hero_command, "/gimbal/player_viewer") { + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .enable_multi_turn_angle() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("top_yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("pitch_motor_zero_point").as_int())) + .enable_multi_turn_angle()); + gimbal_friction_wheels_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_friction_wheels_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_friction_wheels_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG5010Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bullet_feeder_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + putter_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .enable_multi_turn_angle()); + gimbal_scope_motor_.configure(device::DjiMotor::Config{device::DjiMotor::Type::kM2006}); + gimbal_player_viewer_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("viewer_motor_zero_point").as_int())) + .set_reversed() + .enable_multi_turn_angle()); + + steering_hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + steering_hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + steering_hero.register_output( + "/gimbal/photoelectric_sensor", photoelectric_sensor_status_, false); + steering_hero.register_output( + "/gimbal/grayscale_sensor", grayscale_sensor_status_, false); + steering_hero.register_output( + "/auto_aim/image_capturer/timestamp", camera_capturer_trigger_timestamp_, 0); + steering_hero.register_output( + "/auto_aim/image_capturer/trigger", camera_capturer_trigger_, 0); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. + + return std::make_tuple(-y, x, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + tf_->set_state( + gimbal_pitch_motor_.angle()); + + for (auto& motor : gimbal_friction_wheels_) + motor.update_status(); + + gimbal_bullet_feeder_.update_status(); + putter_motor_.update_status(); + + gimbal_player_viewer_motor_.update_status(); + tf_->set_state( + gimbal_player_viewer_motor_.angle()); + + gimbal_scope_motor_.update_status(); + + if (last_camera_capturer_trigger_timestamp_ != *camera_capturer_trigger_timestamp_) + *camera_capturer_trigger_ = true; + last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; + + *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + } + + void command_update() { + auto builder = start_transmit(); + + if (std::isfinite(gimbal_pitch_motor_.control_angle())) + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_ + .generate_angle_command(gimbal_pitch_motor_.control_angle()) + .as_bytes(), + }); + else + builder.can0_transmit({ + .can_id = 0x142, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); // Used to distinguish pitch encoder control from IMU control. + + builder.can0_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_friction_wheels_[0].generate_command(), + gimbal_friction_wheels_[1].generate_command(), + gimbal_friction_wheels_[2].generate_command(), + gimbal_friction_wheels_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + putter_motor_.generate_command(), + gimbal_scope_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x143, + .can_data = + gimbal_player_viewer_motor_ + .generate_velocity_command(gimbal_player_viewer_motor_.control_velocity()) + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x142, + .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + + builder.gpio_digital_read( + librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], + { + .period_ms = 20, + .pull = librmcs::data::GpioPull::kUp, + }); + } + + private: + void can0_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; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + 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; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + gimbal_friction_wheels_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + gimbal_friction_wheels_[1].store_status(data.can_data); + } else if (can_id == 0x203) { + gimbal_friction_wheels_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_friction_wheels_[3].store_status(data.can_data); + } else if (can_id == 0x205) { + putter_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + gimbal_scope_motor_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x143) { + gimbal_player_viewer_motor_.store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x142) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void gpio_digital_read_result_callback( + const librmcs::spec::rmcs_board_lite::GpioDescriptor& gpio, + const librmcs::data::GpioDigitalDataView& data) override { + if (gpio.channel_index == 2) { + photoelectric_sensor_status_atomic.store(data.high); + } else if (gpio.channel_index == 3) { + grayscale_sensor_status_atomic.store(!data.high); + } + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + OutputInterface& tf_; + + std::time_t last_camera_capturer_trigger_timestamp_{0}; + + device::Bmi088 imu_; + device::LkMotor gimbal_top_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + device::DjiMotor gimbal_friction_wheels_[4]; + device::LkMotor gimbal_bullet_feeder_; + device::DjiMotor putter_motor_; + device::DjiMotor gimbal_scope_motor_; + device::LkMotor gimbal_player_viewer_motor_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface photoelectric_sensor_status_; + OutputInterface grayscale_sensor_status_; + OutputInterface camera_capturer_trigger_; + OutputInterface camera_capturer_trigger_timestamp_; + std::atomic photoelectric_sensor_status_atomic{false}; + std::atomic grayscale_sensor_status_atomic{false}; + }; + + class BottomBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class SteeringHeroLittle; + explicit BottomBoard( + SteeringHeroLittle& steering_hero, SteeringHeroLittleCommand& steering_hero_command, + std::string_view board_serial = {}) + : librmcs::agent::RmcsBoardLite( + board_serial, {.dangerously_skip_version_checks = false}) + , logger_(steering_hero.get_logger()) + // , can0_receive_rate_counter_(logger_, "bottom/can0") + // , can1_receive_rate_counter_(logger_, "bottom/can1") + // , can2_receive_rate_counter_(logger_, "bottom/can2") + // , can3_receive_rate_counter_(logger_, "bottom/can3") + , imu_(1000, 0.2, 0.0) + , dr16_(steering_hero) + , supercap_(steering_hero, steering_hero_command) + , chassis_steering_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_front_steering"}, + {steering_hero, steering_hero_command, "/chassis/left_back_steering"}, + {steering_hero, steering_hero_command, "/chassis/right_back_steering"}) + , chassis_wheel_motors_( + {steering_hero, steering_hero_command, "/chassis/left_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_front_wheel"}, + {steering_hero, steering_hero_command, "/chassis/left_back_wheel"}, + {steering_hero, steering_hero_command, "/chassis/right_back_wheel"}) + , chassis_front_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_front_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_front_motor"}) + , chassis_back_climber_motor_( + {steering_hero, steering_hero_command, "/chassis/climber/left_back_motor"}, + {steering_hero, steering_hero_command, "/chassis/climber/right_back_motor"}) + , gimbal_bottom_yaw_motor_(steering_hero, steering_hero_command, "/gimbal/bottom_yaw") { + // + chassis_steering_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_front_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("left_back_zero_point").as_int())) + .set_reversed()); + chassis_steering_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("right_back_zero_point").as_int())) + .set_reversed()); + + chassis_wheel_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_wheel_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(2232. / 169.)); + chassis_front_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + chassis_front_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + chassis_back_climber_motor_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + chassis_back_climber_motor_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .enable_multi_turn_angle() + .set_reduction_ratio(19.)); + + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + steering_hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + steering_hero.register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart0_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + steering_hero.register_output( + "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); + steering_hero.register_output( + "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); + + steering_hero.register_output( + "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() final = default; + + void update() { + // can0_receive_rate_counter_.report_if_due(); + // can1_receive_rate_counter_.report_if_due(); + // can2_receive_rate_counter_.report_if_due(); + // can3_receive_rate_counter_.report_if_due(); + + imu_.update_status(); + dr16_.update_status(); + supercap_.update_status(); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_imu_ = -std::asin(2.0 * (imu_.q0() * imu_.q2() - imu_.q3() * imu_.q1())); + + chassis_front_climber_motor_[0].update_status(); + chassis_front_climber_motor_[1].update_status(); + chassis_back_climber_motor_[0].update_status(); + chassis_back_climber_motor_[1].update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steering_motors_) + motor.update_status(); + + gimbal_bottom_yaw_motor_.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + builder.can0_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steering_motors_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_steering_motors_[3].generate_command(), + chassis_steering_motors_[2].generate_command(), + supercap_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_back_climber_motor_[0].generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_front_climber_motor_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_front_climber_motor_[1].generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + } + + private: + void can0_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; + // can0_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(data.can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(data.can_data); + } else if (can_id == 0x205) { + chassis_steering_motors_[1].store_status(data.can_data); + } else if (can_id == 0x208) { + chassis_steering_motors_[0].store_status(data.can_data); + } + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can1_receive_rate_counter_.record(can_id); + if (can_id == 0x203) { + chassis_wheel_motors_[2].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_wheel_motors_[3].store_status(data.can_data); + } else if (can_id == 0x207) { + chassis_steering_motors_[2].store_status(data.can_data); + } else if (can_id == 0x206) { + chassis_steering_motors_[3].store_status(data.can_data); + } else if (can_id == 0x300) { + supercap_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) + return; + auto can_id = data.can_id; + // can2_receive_rate_counter_.record(can_id); + if (can_id == 0x201) { + chassis_front_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x203) { + chassis_front_climber_motor_[1].store_status(data.can_data); + } + } + + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + // can3_receive_rate_counter_.record(can_id); + if (can_id == 0x202) { + chassis_back_climber_motor_[1].store_status(data.can_data); + } else if (can_id == 0x204) { + chassis_back_climber_motor_[0].store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + } + + void uart0_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { *storage = *ptr++; }, data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + rclcpp::Logger logger_; + // CanReceiveRateCounter can0_receive_rate_counter_; + // CanReceiveRateCounter can1_receive_rate_counter_; + // CanReceiveRateCounter can2_receive_rate_counter_; + // CanReceiveRateCounter can3_receive_rate_counter_; + + device::Bmi088 imu_; + device::Dr16 dr16_; + device::Supercap supercap_; + + device::DjiMotor chassis_steering_motors_[4]; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_front_climber_motor_[2]; + device::DjiMotor chassis_back_climber_motor_[2]; + device::LkMotor gimbal_bottom_yaw_motor_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + + OutputInterface referee_serial_; + OutputInterface powermeter_control_enabled_; + OutputInterface powermeter_charge_power_limit_; + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_imu_; + }; + + OutputInterface tf_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + + std::shared_ptr top_board_; + std::shared_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHeroLittle, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index 738c32a3..e46c0ec4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -27,8 +27,10 @@ class DeformableInfantry , public rclcpp::Node { public: DeformableInfantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , crosshair_circle_(Shape::Color::WHITE, x_center - 26, y_center - 34, 8, 2) + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) , status_ring_(24.0, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp index 0f41b813..0bac8649 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/hero.cpp @@ -1,12 +1,16 @@ #include #include #include +#include #include #include #include #include +#include +#include #include +#include #include #include "referee/app/ui/shape/shape.hpp" @@ -21,20 +25,54 @@ class Hero , public rclcpp::Node { public: Hero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , status_ring_(26.5, 26.5, 600, 40) - , rangefinder_() - , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) - , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) { + // , chassis_direction_indicator_(Shape::Color::PINK, 8, x_center, y_center, 0, 0, 84, 84) + , chassis_left_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , chassis_right_wheel_indicator_( + Shape::Color::WHITE, wheel_indicator_width, x_center, y_center, 0, 0, + wheel_indicator_radius, wheel_indicator_radius) + , yaw_angle_number_(Shape::Color::YELLOW, 20, 5, x_center + 270, y_center + 95, 0.0, false) + , pitch_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 35, 0.0, false) + , bottom_yaw_angle_number_( + Shape::Color::YELLOW, 20, 5, x_center + 270, y_center - 65, 0.0, false) + , time_reminder_(Shape::Color::PINK, 50, 5, x_center + 150, y_center + 65, 0, false) + // , bullet_allowance_label_( + // Shape::Color::YELLOW, 18, 3, x_center - 300, y_center + 270, "bullet", false) + // , bullet_allowance_number_( + // Shape::Color::YELLOW, 20, 5, x_center - 170, y_center + 270, 0, false) + , bullet_allowance_number_( + Shape::Color::YELLOW, 20, 5, x_center - 220, y_center + 270, 0, false) + , friction_profile_number_( + Shape::Color::GREEN, friction_profile_number_font_size, 5, 0, 0, 12, false) + , friction_profile_indicator_{Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, false), Line(Shape::Color::WHITE, friction_profile_box_line_width, 0, 0, 0, 0, true)} + , center_green_line_( + Shape::Color::GREEN, green_line_width, x_center - green_line_half_length, + y_center - green_line_offset_y, x_center + green_line_half_length, + y_center - green_line_offset_y, true) + , tracking_pink_line_( + Shape::Color::PINK, pink_line_width, x_center - pink_line_half_length, + y_center + pink_line_offset_y, x_center + pink_line_half_length, + y_center + pink_line_offset_y, false) { chassis_control_direction_indicator_.set_x(x_center); chassis_control_direction_indicator_.set_y(y_center); + register_input("/gimbal/mode", gimbal_mode_); + register_input("/remote/keyboard", keyboard_); register_input("/chassis/control_mode", chassis_mode_); register_input("/chassis/angle", chassis_angle_); register_input("/chassis/control_angle", chassis_control_angle_); + register_input("/chassis/climber/left_front_motor/velocity", left_track_velocity_); + register_input("/chassis/climber/right_front_motor/velocity", right_track_velocity_); + register_input("/chassis/supercap/voltage", supercap_voltage_); // register_input("/chassis/supercap/control_enable", supercap_control_enabled_); @@ -42,56 +80,192 @@ class Hero register_input("/chassis/power", chassis_power_); register_input("/chassis/control_power_limit", chassis_control_power_limit_); register_input("/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); - - register_input("/referee/shooter/42mm_bullet_allowance", robot_bullet_allowance_); + register_input("/gimbal/control_bullet_allowance/limited_by_heat", robot_bullet_allowance_); register_input( "/gimbal/first_left_friction/control_velocity", left_friction_control_velocity_); register_input("/gimbal/first_left_friction/velocity", left_friction_velocity_); register_input("/gimbal/first_right_friction/velocity", right_friction_velocity_); + register_input("/gimbal/friction_profile_1_active", friction_profile_1_active_, false); + // register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/player_viewer/raw_angle", gimbal_player_viewer_raw_angle_); register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); - register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + register_input("/gimbal/pitch/raw_angle", gimbal_pitch_raw_angle_); + register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); + register_input("/gimbal/bottom_yaw/raw_angle", bottom_yaw_raw_angle_); + // register_input("/gimbal/auto_aim/laser_distance", laser_distance_); + + register_input("/gimbal/shooter/condiction", shoot_condiction_); register_input("/gimbal/shooter/mode", shoot_mode_); - register_input("/gimbal/scope/active", is_scope_active_); + + // register_input("/gimbal/scope/active", is_scope_active_); register_input("/remote/mouse", mouse_); register_input("/referee/game/stage", game_stage_); + + // register_input("/gimbal/auto_aim/fire_control", auto_aim_fire_control_, false); + // register_input("/gimbal/auto_aim/target_confidence", auto_aim_target_confidence_, false); } void update() override { update_normal_ui(); - update_sniper_ui(); - - if (*is_scope_active_) { - set_normal_ui_visible(false); - rangefinder_.set_visible(true); - } else { - set_normal_ui_visible(true); - rangefinder_.set_visible(false); - } + // update_bullet_allowance(); + // update_sniper_ui(); + // update_state_word(); + + // if (*is_scope_active_) { + // set_normal_ui_visible(false); + // rangefinder_.set_visible(true); + // } else { + set_normal_ui_visible(true); + // rangefinder_.set_visible(false); + // } } private: + static uint16_t count_digits(int32_t value) { + uint16_t digits = value <= 0 ? 1 : 0; + while (value != 0) { + value /= 10; + ++digits; + } + return digits; + } void set_normal_ui_visible(bool value) { status_ring_.set_visible(value); - chassis_direction_indicator_.set_visible(value); - chassis_control_direction_indicator_.set_visible(value); + // chassis_direction_indicator_.set_visible(value); + chassis_left_wheel_indicator_.set_visible(value); + chassis_right_wheel_indicator_.set_visible(value); + // chassis_control_direction_indicator_.set_visible(value); + yaw_angle_number_.set_visible(value); + pitch_angle_number_.set_visible(value); + bottom_yaw_angle_number_.set_visible(value); + // bullet_allowance_label_.set_visible(value); + bullet_allowance_number_.set_visible(value); + friction_profile_number_.set_visible(value); + center_green_line_.set_visible(value); + tracking_pink_line_.set_visible(value); + + const bool show_friction_profile_box = + value && friction_profile_1_active_.ready() && *friction_profile_1_active_; + for (auto& line : friction_profile_indicator_) + line.set_visible(show_friction_profile_box); + if (!value) + chassis_control_direction_indicator_.set_visible(false); } void update_normal_ui() { update_chassis_direction_indicator(); - - status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + yaw_angle_number_.set_value(static_cast(*gimbal_player_viewer_raw_angle_)); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + update_pitch_raw_angle_color(); + bottom_yaw_angle_number_.set_value(static_cast(*bottom_yaw_raw_angle_)); + update_bottom_yaw_tracking_lines(); + const int32_t bullet_allowance = + static_cast(std::max(0, *robot_bullet_allowance_)); + bullet_allowance_number_.set_value(bullet_allowance); + + const uint16_t yaw_right = + yaw_raw_angle_x + + count_digits(static_cast(*gimbal_player_viewer_raw_angle_)) + * raw_angle_font_size; + const uint16_t pitch_right = + pitch_raw_angle_x + + count_digits(static_cast(*gimbal_pitch_raw_angle_)) * raw_angle_font_size; + const uint16_t box_left = std::max(yaw_right, pitch_right) + friction_profile_box_gap; + const uint16_t box_right = box_left + friction_profile_box_visual_width; + + const uint16_t box_top = yaw_raw_angle_y + raw_angle_font_size; + const uint16_t box_bottom = pitch_raw_angle_y; + + const bool friction_profile_1_active = + friction_profile_1_active_.ready() && *friction_profile_1_active_; + const uint16_t box_center_x = (box_left + box_right) / 2; + const uint16_t profile_number_y = box_top + friction_profile_number_gap; + + for (auto& line : friction_profile_indicator_) + line.set_color(Shape::Color::GREEN); + + friction_profile_number_.set_value(friction_profile_1_active ? 16 : 12); + friction_profile_number_.set_color( + friction_profile_1_active ? Shape::Color::PINK : Shape::Color::GREEN); + friction_profile_number_.set_font_size(friction_profile_number_font_size); + friction_profile_number_.set_xy(box_left, profile_number_y); + friction_profile_number_.set_center_x(box_center_x); + + friction_profile_indicator_[0].set_x(box_left); + friction_profile_indicator_[0].set_y(box_top); + friction_profile_indicator_[0].set_x2(box_right); + friction_profile_indicator_[0].set_y2(box_top); + + friction_profile_indicator_[1].set_x(box_right); + friction_profile_indicator_[1].set_y(box_top); + friction_profile_indicator_[1].set_x2(box_right); + friction_profile_indicator_[1].set_y2(box_bottom); + + friction_profile_indicator_[2].set_x(box_left); + friction_profile_indicator_[2].set_y(box_bottom); + friction_profile_indicator_[2].set_x2(box_right); + friction_profile_indicator_[2].set_y2(box_bottom); + + friction_profile_indicator_[3].set_x(box_left); + friction_profile_indicator_[3].set_y(box_top); + friction_profile_indicator_[3].set_x2(box_left); + friction_profile_indicator_[3].set_y2(box_bottom); status_ring_.update_friction_wheel_speed( std::min(*left_friction_velocity_, *right_friction_velocity_), *left_friction_control_velocity_ > 0); status_ring_.update_supercap(*supercap_voltage_, true); status_ring_.update_battery_power(*chassis_voltage_); - update_static_status_ring(); + // const bool auto_aim_locked = auto_aim_fire_control_.ready() && *auto_aim_fire_control_; + // const double target_confidence_value = + // auto_aim_target_confidence_.ready() ? *auto_aim_target_confidence_ : 0.0; + + // status_ring_.update_auto_aim_feedback(auto_aim_locked, target_confidence_value); + // update_static_status_ring(); + last_keyboard_ = *keyboard_; + } + + void update_pitch_raw_angle_color() { + if (keyboard_.ready()) { + if (!last_keyboard_.e && keyboard_->e) { + last_e_triggered_with_ctrl_ = keyboard_->ctrl; + } + last_keyboard_ = *keyboard_; + } + + if (!gimbal_mode_.ready()) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + return; + } + + const bool is_encoder = *gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER; + const bool entering_encoder = + last_gimbal_mode_ != rmcs_msgs::GimbalMode::ENCODER && is_encoder; + const bool leaving_encoder = + last_gimbal_mode_ == rmcs_msgs::GimbalMode::ENCODER && !is_encoder; + + if (entering_encoder) { + pitch_encoder_by_ctrl_e_ = last_e_triggered_with_ctrl_; + } + + if (leaving_encoder) { + pitch_encoder_by_ctrl_e_ = false; + } + + if (!is_encoder) { + pitch_angle_number_.set_color(Shape::Color::YELLOW); + } else if (pitch_encoder_by_ctrl_e_) { + pitch_angle_number_.set_color(Shape::Color::PINK); + } else { + pitch_angle_number_.set_color(Shape::Color::GREEN); + } + + last_gimbal_mode_ = *gimbal_mode_; } void update_sniper_ui() { @@ -99,7 +273,7 @@ class Hero ? *gimbal_pitch_angle_ - 2 * std::numbers::pi : *gimbal_pitch_angle_; - rangefinder_.update_pitch_angle(-display_angle); + rangefinder_.update_pitch_angle(static_cast(*gimbal_pitch_raw_angle_)); double raw_height = -display_angle / 0.7 * static_cast(height_max); raw_height = std::clamp(raw_height, 0.0, static_cast(height_max)); @@ -107,6 +281,35 @@ class Hero lift_height = std::clamp(lift_height, height_min, height_max); rangefinder_.update_vertical_rangefinder(lift_height); + pitch_angle_number_.set_value(static_cast(*gimbal_pitch_raw_angle_)); + } + + void update_bottom_yaw_tracking_lines() { + const bool ctrl_e_pressed = keyboard_->ctrl && keyboard_->e; + if (ctrl_e_pressed) { + bottom_yaw_tracking_enabled_ = !bottom_yaw_tracking_enabled_; + bottom_yaw_anchor_angle_rad_ = *bottom_yaw_angle_; + } + + const double delta_bottom_yaw_rad = *bottom_yaw_angle_ - bottom_yaw_anchor_angle_rad_; + + const int min_center_x = static_cast(pink_line_half_length); + const int max_center_x = + static_cast(screen_width) - static_cast(pink_line_half_length); + + const int pink_center_x = std::clamp( + static_cast(std::lround( + static_cast(x_center) + + delta_bottom_yaw_rad * pink_line_pixels_per_radian)), + min_center_x, max_center_x); + + tracking_pink_line_.set_x( + static_cast(pink_center_x - static_cast(pink_line_half_length))); + tracking_pink_line_.set_y(y_center + pink_line_offset_y); + tracking_pink_line_.set_x2( + static_cast(pink_center_x + static_cast(pink_line_half_length))); + tracking_pink_line_.set_y2(y_center + pink_line_offset_y); + tracking_pink_line_.set_visible(true); } void update_time_reminder() { @@ -121,17 +324,78 @@ class Hero status_ring_.update_static_parts({auto_aim_enable, precise_enable}); } + // void update_bullet_allowance() { + + // std::string text = "BULLET : " + std::to_string(max(0,*robot_bullet_allowance_)); + // char* allow = text.data(); + // auto color = Shape::Color::YELLOW; + + // bullet_allowance_number_.set_value(allow); + // bullet_allowance_number_.set_font_size(14); + // bullet_allowance_number_.set_color(color); + // bullet_allowance_number_.set_visible(true); + // bullet_allowance_number_.set_xy(x_center - 240, y_center + 288); + // } + + void update_state_word() { + + const char* text = "OK"; + auto color = Shape::Color::GREEN; + + if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FRICTION_WAITING) { + text = " WAITING "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::SHOOT) { + text = " SHOOT "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::FIRED) { + text = " FIRED "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::JAM) { + text = " JAM "; + } else if (*shoot_condiction_ == rmcs_msgs::ShootCondiction::PRELOADING) { + text = "PRELOADING"; + } + + state_word_.set_value(text); + state_word_.set_font_size(30); + state_word_.set_color(color); + state_word_.set_visible(true); + state_word_.set_xy(x_center - 800, y_center + 200); + } + void update_chassis_direction_indicator() { auto chassis_mode = *chassis_mode_; auto to_referee_angle = [](double angle) { - return static_cast( - std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + // return static_cast( + // std::round((2 * std::numbers::pi - angle) / std::numbers::pi * 180)); + int degrees = static_cast( + std::lround((2.0 * std::numbers::pi - angle) / std::numbers::pi * 180.0)); + degrees %= 360; + if (degrees < 0) + degrees += 360; + return static_cast(degrees); }; - chassis_direction_indicator_.set_color( - chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN - : Shape::Color::PINK); - chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + // chassis_direction_indicator_.set_color( + // chassis_mode == rmcs_msgs::ChassisMode::SPIN ? Shape::Color::GREEN + // : Shape::Color::PINK); + // chassis_direction_indicator_.set_angle(to_referee_angle(*chassis_angle_), 30); + const bool left_track_active = + std::abs(*left_track_velocity_) > track_velocity_active_threshold; + const bool right_track_active = + std::abs(*right_track_velocity_) > track_velocity_active_threshold; + + chassis_left_wheel_indicator_.set_color( + left_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + chassis_right_wheel_indicator_.set_color( + right_track_active ? Shape::Color::GREEN : Shape::Color::WHITE); + + const double wheel_offset = wheel_indicator_offset_deg * std::numbers::pi / 180.0; + const double left_wheel_angle = *chassis_angle_ + wheel_offset; + const double right_wheel_angle = *chassis_angle_ - wheel_offset; + + chassis_left_wheel_indicator_.set_angle( + to_referee_angle(left_wheel_angle), wheel_indicator_half_angle); + chassis_right_wheel_indicator_.set_angle( + to_referee_angle(right_wheel_angle), wheel_indicator_half_angle); bool chassis_control_direction_indicator_visible = false; if (!std::isnan(*chassis_control_angle_)) { @@ -158,10 +422,48 @@ class Hero static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr uint16_t raw_angle_font_size = 20; + + static constexpr uint16_t yaw_raw_angle_x = x_center + 270; + static constexpr uint16_t yaw_raw_angle_y = y_center + 65; + + static constexpr uint16_t pitch_raw_angle_x = x_center + 270; + static constexpr uint16_t pitch_raw_angle_y = y_center - 65; + + static constexpr uint16_t friction_profile_box_gap = 18; + static constexpr uint16_t friction_profile_box_visual_width = 105; + static constexpr uint16_t friction_profile_box_line_width = 6; + static constexpr uint16_t friction_profile_number_font_size = 24; + static constexpr uint16_t friction_profile_number_gap = 12; + static constexpr uint16_t height_min = 0, height_max = 500; + static constexpr uint16_t wheel_indicator_radius = 110; + static constexpr uint16_t wheel_indicator_width = 12; + static constexpr uint16_t wheel_indicator_half_angle = 10; + static constexpr double wheel_indicator_offset_deg = 28.0; + static constexpr double track_velocity_active_threshold = 1.0; + static constexpr uint16_t green_line_half_length = 8; + static constexpr uint16_t green_line_width = 40; + static constexpr uint16_t green_line_offset_y = 0; + + static constexpr uint16_t pink_line_half_length = 40; + static constexpr uint16_t pink_line_width = 8; + static constexpr uint16_t pink_line_offset_y = 0; + + static constexpr double pink_line_pixels_per_radian = 400.0; + + InputInterface gimbal_mode_; + InputInterface keyboard_; + + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::GimbalMode last_gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; + + bool last_e_triggered_with_ctrl_ = false; + bool pitch_encoder_by_ctrl_e_ = false; InputInterface chassis_mode_; InputInterface chassis_angle_, chassis_control_angle_; + InputInterface left_track_velocity_, right_track_velocity_; InputInterface supercap_voltage_; InputInterface supercap_control_enabled_; @@ -171,33 +473,60 @@ class Hero InputInterface chassis_control_power_limit_; InputInterface supercap_charge_power_limit_; - InputInterface robot_bullet_allowance_; + InputInterface robot_bullet_allowance_; InputInterface left_friction_control_velocity_; InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; + InputInterface friction_profile_1_active_; InputInterface mouse_; InputInterface game_stage_; + InputInterface gimbal_yaw_angle_; InputInterface gimbal_pitch_angle_; InputInterface gimbal_player_viewer_angle_; - InputInterface laser_distance_; + InputInterface gimbal_player_viewer_raw_angle_; + InputInterface gimbal_pitch_raw_angle_; + InputInterface bottom_yaw_raw_angle_; + InputInterface bottom_yaw_angle_; + // InputInterface laser_distance_; InputInterface shoot_mode_; - InputInterface is_scope_active_; + InputInterface shoot_condiction_; + // InputInterface is_scope_active_; StatusRing status_ring_; Rangefinder rangefinder_; - Arc chassis_direction_indicator_, chassis_control_direction_indicator_; + Arc chassis_left_wheel_indicator_; + Arc chassis_right_wheel_indicator_; + Arc chassis_control_direction_indicator_; + + Float yaw_angle_number_; + Float pitch_angle_number_; + Float bottom_yaw_angle_number_; + Text state_word_; Integer time_reminder_; + + // Text bullet_allowance_label_; + Integer bullet_allowance_number_; + Integer friction_profile_number_; + Line friction_profile_indicator_[4]; + Line center_green_line_; + Line tracking_pink_line_; + + bool bottom_yaw_tracking_enabled_ = false; + double bottom_yaw_anchor_angle_rad_ = 0.0; + + // InputInterface auto_aim_fire_control_; + // InputInterface auto_aim_target_confidence_; }; } // namespace rmcs_core::referee::app::ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Hero, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp index 9e2dd8a3..e2916919 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/infantry.cpp @@ -22,7 +22,9 @@ class Infantry , public rclcpp::Node { public: Infantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_(Shape::Color::WHITE, x_center - 12, y_center - 37) , status_ring_(26.5, 26.5, 600, 300) , horizontal_center_guidelines_( diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp index b387b972..e73e297d 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp @@ -48,7 +48,7 @@ class CfsScheduler { private: bool operator<(const Entity& obj) const { return vruntime_ < obj.vruntime_; } uint64_t vruntime_ : 48 = 65536; - uint16_t priority_ = 0; + uint16_t priority_ = 0; }; // class T : public Entity {}; @@ -58,10 +58,10 @@ class CfsScheduler { UpdateIterator() : current_(run_queue_.first()) , ignored_(nullptr) {} - UpdateIterator(const UpdateIterator&) = delete; + UpdateIterator(const UpdateIterator&) = delete; UpdateIterator& operator=(const UpdateIterator&) = delete; - UpdateIterator(UpdateIterator&&) = default; - UpdateIterator& operator=(UpdateIterator&&) = default; + UpdateIterator(UpdateIterator&&) = default; + UpdateIterator& operator=(UpdateIterator&&) = default; T* get() const { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) @@ -74,12 +74,12 @@ class CfsScheduler { auto update() { min_vruntime_ = current_->vruntime_; - int shift = 65536 - current_->priority_; + int shift = 65536 - current_->priority_; current_->vruntime_ += shift; run_queue_.erase(*current_); auto result = get()->update(); - current_ = ignored_ ? ignored_->next() : run_queue_.first(); + current_ = ignored_ ? ignored_->next() : run_queue_.first(); return result; } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp index b95766a9..ff7bed62 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp @@ -181,7 +181,7 @@ class BasicRedBlackTree { */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -209,7 +209,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->right; + tmp = node->right; } /* @@ -234,7 +234,7 @@ class BasicRedBlackTree { /* Case 1 - color flips */ tmp->set_parent_and_color(gparent, Color::BLACK); parent->set_parent_and_color(gparent, Color::BLACK); - node = gparent; + node = gparent; parent = node->parent(); node->set_parent_and_color(parent, Color::RED); continue; @@ -250,7 +250,7 @@ class BasicRedBlackTree { tmp->set_parent_and_color(parent, Color::BLACK); parent->set_parent_and_color(node, Color::RED); parent = node; - tmp = node->left; + tmp = node->left; } /* Case 3 - left rotate at gparent */ @@ -315,7 +315,7 @@ class BasicRedBlackTree { * - old gets assigned new as a parent and 'color' as a color. */ void rotate_set_parents(Node* old_node, Node* new_node, Color color) { - Node* parent = old_node->parent(); + Node* parent = old_node->parent(); new_node->parent_and_color = old_node->parent_and_color; old_node->set_parent_and_color(new_node, color); change_child(old_node, new_node, parent); @@ -323,7 +323,7 @@ class BasicRedBlackTree { Node* __erase(Node* node) { Node* child = node->right; - Node* tmp = node->left; + Node* tmp = node->left; Node *parent, *rebalance; uintptr_t pc; @@ -335,22 +335,22 @@ class BasicRedBlackTree { * and node must be black due to 4). We adjust colors locally * so as to bypass __rb_erase_color() later on. */ - pc = node->parent_and_color; + pc = node->parent_and_color; parent = ((Node*)(pc & ~3)); change_child(node, child, parent); if (child) { child->parent_and_color = pc; - rebalance = nullptr; + rebalance = nullptr; } else rebalance = ((pc) & 1) ? parent : nullptr; tmp = parent; } else if (!child) { /* Still case 1, but this time the child is node->rb_left */ tmp->parent_and_color = pc = node->parent_and_color; - parent = ((Node*)(pc & ~3)); + parent = ((Node*)(pc & ~3)); change_child(node, tmp, parent); rebalance = nullptr; - tmp = parent; + tmp = parent; } else { Node *successor = child, *child2; @@ -384,9 +384,9 @@ class BasicRedBlackTree { * (c) */ do { - parent = successor; + parent = successor; successor = tmp; - tmp = tmp->left; + tmp = tmp->left; } while (tmp); child2 = successor->right; WRITE_ONCE(parent->left, child2); @@ -398,7 +398,7 @@ class BasicRedBlackTree { WRITE_ONCE(successor->left, tmp); tmp->set_parent(successor); - pc = node->parent_and_color; + pc = node->parent_and_color; tmp = ((Node*)(pc & ~3)); change_child(node, successor, tmp); @@ -409,7 +409,7 @@ class BasicRedBlackTree { rebalance = successor->is_black() ? parent : nullptr; } successor->parent_and_color = pc; - tmp = successor; + tmp = successor; } return rebalance; @@ -468,7 +468,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -508,7 +508,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->right, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* @@ -551,7 +551,7 @@ class BasicRedBlackTree { if (parent->is_red()) parent->set_black(); else { - node = parent; + node = parent; parent = node->parent(); if (parent) continue; @@ -565,7 +565,7 @@ class BasicRedBlackTree { WRITE_ONCE(parent->left, tmp2); if (tmp1) tmp1->set_parent_and_color(sibling, Color::BLACK); - tmp1 = sibling; + tmp1 = sibling; sibling = tmp2; } /* Case 4 - right rotate at parent + color flips */ diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp index 7fd0d851..f2305314 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp @@ -13,11 +13,11 @@ class RemoteShape { friend class RemoteShape; friend class RedBlackTree; - Descriptor() = default; - Descriptor(const Descriptor&) = delete; + Descriptor() = default; + Descriptor(const Descriptor&) = delete; Descriptor& operator=(const Descriptor&) = delete; - Descriptor(Descriptor&&) = delete; - Descriptor& operator=(Descriptor&& obj) = delete; + Descriptor(Descriptor&&) = delete; + Descriptor& operator=(Descriptor&& obj) = delete; [[nodiscard]] bool has_id() const { return id_; } [[nodiscard]] bool try_assign_id() @@ -79,9 +79,9 @@ class RemoteShape { private: /* Swap requirement: !this->id_ && victim.id_ */ void swap_id(Descriptor& victim) { - id_ = victim.id_; + id_ = victim.id_; assigned_list_[id_ - 1] = this; - existence_confidence_ = victim.existence_confidence_; + existence_confidence_ = victim.existence_confidence_; victim.revoke_id(); } @@ -94,7 +94,7 @@ class RemoteShape { } void revoke_id() { - id_ = 0; + id_ = 0; existence_confidence_ = 0; static_cast(this)->id_revoked(); @@ -104,7 +104,7 @@ class RemoteShape { return existence_confidence_ < obj.existence_confidence_; } - uint8_t id_ = 0; + uint8_t id_ = 0; uint8_t existence_confidence_ = 0; }; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index e0ac7365..37be26ae 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -87,18 +87,26 @@ class Shape set_modified(); } + void set_xy(uint16_t x, uint16_t y) { + if (part2_.x == x && part2_.y == y) + return; + part2_.x = x; + part2_.y = y; + set_modified(); + } + bool is_text_shape() const { return is_text_shape_; } enum class Operation : uint8_t { NO_OPERATION = 0, - ADD = 1, - MODIFY = 2, - DELETE = 3, + ADD = 1, + MODIFY = 2, + DELETE = 3, }; Operation predict_update() const { uint8_t predict_existence = existence_confidence(); - uint8_t predict_sync = sync_confidence_; + uint8_t predict_sync = sync_confidence_; if (!has_id() && !predict_try_assign_id(predict_existence)) { return Operation::NO_OPERATION; @@ -120,34 +128,37 @@ class Shape constexpr static inline command::Field no_operation_description() { return command::Field{[](std::byte* buffer) { - auto& description = *new (buffer) DescriptionField{}; + auto& description = *new (buffer) DescriptionField{}; description.part1.operation_type = Operation::NO_OPERATION; return sizeof(DescriptionField); }}; } enum class Color : uint8_t { - SELF = 0, + SELF = 0, YELLOW = 1, - GREEN = 2, + GREEN = 2, ORANGE = 3, PURPLE = 4, - PINK = 5, - CYAN = 6, - BLACK = 7, - WHITE = 8, + PINK = 5, + CYAN = 6, + BLACK = 7, + WHITE = 8, }; protected: + explicit Shape(bool is_text_shape = false) + : is_text_shape_(is_text_shape) {} + enum class ShapeType : uint8_t { - LINE = 0, + LINE = 0, RECTANGLE = 1, - CIRCLE = 2, - ELLIPSE = 3, - ARC = 4, - FLOAT = 5, - INTEGER = 6, - TEXT = 7, + CIRCLE = 2, + ELLIPSE = 3, + ARC = 4, + FLOAT = 5, + INTEGER = 6, + TEXT = 7, }; struct DescriptionField { @@ -173,7 +184,7 @@ class Shape }; void set_modified() { - // Optimization: Assume the modification not exist when invisible. + // Optimization: Assume the modification does not exist when invisible. if (!visible_) return; @@ -187,7 +198,7 @@ class Shape private: void enter_run_queue() { - uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); + uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); uint16_t weighted_priority = (priority_ - 256) << (4 * min_confidence); CfsScheduler::Entity::enter_run_queue(weighted_priority); } @@ -200,7 +211,7 @@ class Shape // Re-enter the update queue to try to get a new id. set_modified(); } else { - // Leave run_queue when shape was hidden. + // Leave run_queue when the shape is hidden. leave_run_queue(); } } @@ -210,10 +221,9 @@ class Shape // Called by CfsScheduler. if (!has_id() && !try_assign_id()) { - // TODO: Print error message. sync_confidence_ = max_update_times; - visible_ = false; - // Do nothing when failed + visible_ = false; + // Do nothing when the update fails. return no_operation_description(); } @@ -224,14 +234,14 @@ class Shape command::Field field; - // Optimization1: Stop adding when shape is invisible. - // Optimization2: Prevent continuous modification. + // Optimization 1: Stop adding when the shape is invisible. + // Optimization 2: Prevent continuous modification. if (visible_ && (existence_confidence() <= sync_confidence_ || (last_time_modified_ && existence_confidence() < max_update_times))) { // Send add packet last_time_modified_ = false; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::ADD); }}; if (increase_existence_confidence() < max_update_times @@ -240,7 +250,7 @@ class Shape } else { // Send modify packet last_time_modified_ = true; - field = command::Field{[this](std::byte* buffer) { + field = command::Field{[this](std::byte* buffer) { return write_full_description_field(buffer, Operation::MODIFY); }}; // No need to compare existence_confidence here. @@ -275,11 +285,11 @@ class Shape auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = Color::WHITE; + description.part1.color = Color::WHITE; description.part2.width = 0; - description.part2.x = 0; - description.part2.y = 0; + description.part2.x = 0; + description.part2.y = 0; description.part3.details_c = 0; description.part3.details_d = 0; @@ -290,7 +300,7 @@ class Shape static constexpr uint8_t max_update_times = 4; - uint8_t priority_ = 15; + uint8_t priority_ = 15; uint8_t sync_confidence_ : 5 = max_update_times; bool is_text_shape_ : 1 = false; bool last_time_modified_ : 1 = false; @@ -305,10 +315,10 @@ class Line : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -342,7 +352,7 @@ class Line : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::LINE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -370,10 +380,10 @@ class Circle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.rx = rx; - part3_.ry = ry; + part2_.x = x; + part2_.y = y; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -412,7 +422,7 @@ class Circle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ELLIPSE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -438,10 +448,10 @@ class Rectangle : public Shape { bool visible = true) { part3_.color = color; part2_.width = width; - part2_.x = x; - part2_.y = y; - part3_.x2 = x2; - part3_.y2 = y2; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; set_visible(visible); } @@ -475,7 +485,7 @@ class Rectangle : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::RECTANGLE; - description.part1.color = part3_.color; + description.part1.color = part3_.color; description.part2 = part2_; @@ -497,18 +507,17 @@ class Arc : public Shape { public: Arc() = default; Arc(Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t angle_start, - uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) - : Arc() { + uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) { angle_start_ = angle_start; - angle_end_ = angle_end; + angle_end_ = angle_end; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; part3_.color = color; - part3_.rx = rx; - part3_.ry = ry; + part3_.rx = rx; + part3_.ry = ry; set_visible(visible); } @@ -577,9 +586,9 @@ class Arc : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::ARC; - description.part1.color = part3_.color; - description.part1.details_a = angle_start_; - description.part1.details_b = angle_end_; + description.part1.color = part3_.color; + description.part1.details_a = angle_start_; + description.part1.details_b = angle_end_; description.part2 = part2_; @@ -604,14 +613,13 @@ class Integer : public Shape { Integer() = default; Integer( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, int32_t value, - bool visible = true) - : Integer() { - color_ = color; + bool visible = true) { + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -627,7 +635,7 @@ class Integer : public Shape { } void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value <= 0 ? 1 : 0; for (; value != 0; number_of_digits++) value /= 10; @@ -656,8 +664,8 @@ class Integer : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::INTEGER; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -676,7 +684,7 @@ class Float : public Integer { using Integer::Integer; void set_center_x(uint16_t x) { - int value = value_; + int value = value_; int number_of_digits = value < 0 ? 1 : 0; int integer_part = value / 1000; @@ -701,8 +709,8 @@ class Float : public Integer { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::FLOAT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; @@ -714,17 +722,18 @@ class Float : public Integer { class Text : public Shape { public: - Text() { value_ = nullptr; }; + Text() + : Shape(true) {} Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) : Text() { - color_ = color; + color_ = color; font_size_ = font_size; part2_.width = width; - part2_.x = x; - part2_.y = y; + part2_.x = x; + part2_.y = y; value_ = value; @@ -760,14 +769,14 @@ class Text : public Shape { auto& description = *new (buffer) DescriptionField{}; description.part1.shape_type = ShapeType::TEXT; - description.part1.color = color_; - description.part1.details_a = font_size_; + description.part1.color = color_; + description.part1.details_a = font_size_; description.part2 = part2_; constexpr size_t data_part_size = 30; - auto str_length = std::min(strlen(value_), data_part_size); - description.part1.details_b = str_length; + auto str_length = std::min(strlen(value_), data_part_size); + description.part1.details_b = str_length; std::memcpy(buffer + sizeof(DescriptionField), value_, str_length); return sizeof(DescriptionField) + data_part_size; @@ -775,8 +784,8 @@ class Text : public Shape { uint16_t font_size_; Color color_; - const char* value_; + const char* value_ = nullptr; }; } // namespace app::ui -} // namespace rmcs_core::referee \ No newline at end of file +} // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp index 15823c6f..a45a58c4 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair_circle.hpp @@ -10,8 +10,7 @@ class CrossHairCircle { : CrossHairCircle(color, x, y, 12, 2, visible) {} CrossHairCircle( - Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, - bool visible = true) + Shape::Color color, uint16_t x, uint16_t y, uint16_t r, uint16_t width, bool visible = true) : circle_(color, width, x, y, r, r, visible) {} void set_visible(bool value) { circle_.set_visible(value); } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index 9c08074d..0b6bd794 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -67,15 +67,13 @@ class DeformableChassisLegArcs { static constexpr double default_min_angle_deg_ = 8.0; static constexpr double default_max_angle_deg_ = 58.0; - static constexpr double deg_to_rad_(double degrees) { - return degrees * degrees_to_radians_; - } + static constexpr double deg_to_rad_(double degrees) { return degrees * degrees_to_radians_; } static constexpr std::array leg_base_mid_angles_ = { front_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v - rear_pair_offset_deg_ * degrees_to_radians_, - std::numbers::pi_v + rear_pair_offset_deg_ * degrees_to_radians_, - -front_pair_offset_deg_ * degrees_to_radians_, + std::numbers::pi_v - rear_pair_offset_deg_* degrees_to_radians_, + std::numbers::pi_v + rear_pair_offset_deg_* degrees_to_radians_, + -front_pair_offset_deg_* degrees_to_radians_, }; static constexpr std::array leg_radii_near_ = { @@ -93,9 +91,8 @@ class DeformableChassisLegArcs { }; static uint16_t to_referee_angle_(double angle) { - const auto degrees = - static_cast(std::lround((2.0 * std::numbers::pi_v - angle) - / std::numbers::pi_v * 180.0)); + const auto degrees = static_cast(std::lround( + (2.0 * std::numbers::pi_v - angle) / std::numbers::pi_v * 180.0)); int wrapped = degrees % 360; if (wrapped < 0) wrapped += 360; @@ -118,14 +115,13 @@ class DeformableChassisLegArcs { double leg_angle, bool active_suspension) const { const double normalized_extension = normalized_leg_extension_(leg_angle); // Min angle looks like a thin leg stretching radially outward from the center ring. - const uint16_t radius = static_cast(std::lround( - far_radius - normalized_extension * (far_radius - near_radius))); + const uint16_t radius = static_cast( + std::lround(far_radius - normalized_extension * (far_radius - near_radius))); const uint16_t half_angle = static_cast(std::lround( prone_leg_half_angle_ - normalized_extension * (prone_leg_half_angle_ - upright_leg_half_angle_))); const uint16_t width = static_cast(std::lround( - prone_leg_width_ - + normalized_extension * (upright_leg_width_ - prone_leg_width_))); + prone_leg_width_ + normalized_extension * (upright_leg_width_ - prone_leg_width_))); leg.set_x(center_x_); leg.set_y(center_y_); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp index f3361b58..efb9f766 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/rangefinder.hpp @@ -151,16 +151,16 @@ class Rangefinder { } private: - constexpr static uint16_t x_center_ = 960; - constexpr static uint16_t y_center_ = 540; + constexpr static uint16_t x_center_ = 960; + constexpr static uint16_t y_center_ = 540; constexpr static uint16_t x_side_center_ = x_center_ - 110; - constexpr static uint16_t horizontal_unit_scale = 15; + constexpr static uint16_t horizontal_unit_scale = 15; constexpr static uint16_t horizontal_scale_accuracy_ = 40; - constexpr static uint16_t vertical_unit_scale_ = 5; + constexpr static uint16_t vertical_unit_scale_ = 5; constexpr static uint16_t vertical_side_unit_scale_ = 15; - constexpr static uint16_t vertical_scale_accuracy_ = 26; + constexpr static uint16_t vertical_scale_accuracy_ = 26; Float pitch_angle_{Shape::Color::BLACK, 17, 2, x_center_ - 100, y_center_ + 170, 0, false}; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 35882689..36c8c8b1 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -133,7 +133,7 @@ class StatusRing { void update_static_parts(std::tuple enable) { auto& [auto_aim_enable, precise_enable] = enable; - auto static_enable = auto_aim_enable || precise_enable; + auto static_enable = auto_aim_enable || precise_enable; static auto color{Shape::Color::WHITE}; @@ -331,9 +331,9 @@ class StatusRing { void set_limits( double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { supercap_limit_ = supercap_limit; - battery_limit_ = battery_limit; + battery_limit_ = battery_limit; friction_limit_ = friction_limit; - bullet_limit_ = bullet_limit; + bullet_limit_ = bullet_limit; int scale_angle = 5; for (auto& bullet_scale : bullet_scales_) { @@ -351,13 +351,13 @@ class StatusRing { } double value = 0; - scale_angle = 5; + scale_angle = 5; for (auto& number : bullet_scales_number_) { scale_angle += (visible_angle) / 4; value += static_cast(bullet_limit_) / 4; - const auto r = visible_radius - width_ring + 30; + const auto r = visible_radius - width_ring + 30; const auto angle = static_cast(-scale_angle) * std::numbers::pi / 180; number.set_x(x_center + static_cast(r * std::cos(angle))); @@ -370,11 +370,11 @@ class StatusRing { } } - constexpr static uint16_t x_center = 960; - constexpr static uint16_t y_center = 540; - constexpr static uint16_t width_ring = 15; + constexpr static uint16_t x_center = 960; + constexpr static uint16_t y_center = 540; + constexpr static uint16_t width_ring = 15; constexpr static uint16_t visible_radius = 400; - constexpr static uint16_t visible_angle = 40; + constexpr static uint16_t visible_angle = 40; double supercap_limit_; double battery_limit_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/command.cpp b/rmcs_ws/src/rmcs_core/src/referee/command.cpp index 95326d34..34c55849 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command.cpp @@ -16,7 +16,9 @@ class Command , public rclcpp::Node { public: Command() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , next_sent_(std::chrono::steady_clock::time_point::min()) , interaction_next_sent_(std::chrono::steady_clock::time_point::min()) , map_marker_next_sent_(std::chrono::steady_clock::time_point::min()) diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index dc1e3271..cea7953c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -45,7 +45,7 @@ class Ui resetting_ = 4; } last_game_stage_ = *game_stage_; - last_keyboard_ = *keyboard_; + last_keyboard_ = *keyboard_; if (resetting_) { *ui_field_ = Field{[this](std::byte* buffer) { @@ -67,10 +67,10 @@ class Ui size_t write_resetting_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; - header.command_id = 0x0100; // Clear shapes + auto& header = *new (buffer + written) Header{}; + header.command_id = 0x0100; // Clear shapes auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); @@ -79,7 +79,7 @@ class Ui uint8_t layer; }; auto& command = *new (buffer + written) Command{}; - command.type = 2; // Clear all layers + command.type = 2; // Clear all layers command.layer = 0; written += sizeof(Command); @@ -89,9 +89,9 @@ class Ui size_t write_updating_field(std::byte* buffer) const { size_t written = 0; - auto& header = *new (buffer + written) Header{}; + auto& header = *new (buffer + written) Header{}; auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; - header.sender_id = full_robot_id; + header.sender_id = full_robot_id; header.receiver_id = full_robot_id.client(); written += sizeof(Header); diff --git a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp index d8a406e4..4d719add 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee { -constexpr uint8_t sof_value = 0xa5; +constexpr uint8_t sof_value = 0xa5; constexpr size_t frame_data_max_length = 1024; struct __attribute__((packed)) FrameHeader { diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index de3911af..ad321125 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -5,7 +5,7 @@ namespace rmcs_core::referee::status { struct __attribute__((packed)) GameStatus { - uint8_t game_type : 4; + uint8_t game_type : 4; uint8_t game_progress : 4; uint16_t stage_remain_time; uint64_t sync_timestamp; diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index bfb10288..bd9ed067 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -1,22 +1,56 @@ #pragma once +#include #include +#include +#include +#include #include #include +#include #include #include #include +#include #include #include #include +#include #include +#include +#include + namespace rmcs_executor { +enum class InterfaceKind { + Normal, + Event, +}; + +inline const char* interface_kind_name(InterfaceKind kind) { + switch (kind) { + case InterfaceKind::Normal: return "Normal"; + case InterfaceKind::Event: return "Event"; + } + + return "Unknown"; +} + +using EventState = std::uint32_t; +constexpr EventState EVENT_DISABLED_BIT = EventState{1} << 31; +constexpr EventState EVENT_ACTIVE_MASK = ~EVENT_DISABLED_BIT; + class Component { public: friend class Executor; + struct OutputInfo { + std::reference_wrapper type; + InterfaceKind kind; + }; + using OutputInfoMap = std::map; + Component(const Component&) = delete; Component& operator=(const Component&) = delete; Component(Component&&) = delete; @@ -24,9 +58,7 @@ class Component { virtual ~Component() = default; - virtual void before_pairing(const std::map& output_map) { - (void)output_map; - } + virtual void before_pairing(const OutputInfoMap& output_map) { (void)output_map; } virtual void before_updating() {} virtual void update() = 0; @@ -79,9 +111,9 @@ class Component { const T& operator*() const { return *data_pointer_; } private: - void** activate() { + void* activate() { activated = true; - return reinterpret_cast(&data_pointer_); + return reinterpret_cast(&data_pointer_); } T* data_pointer_ = nullptr; @@ -90,6 +122,139 @@ class Component { bool delete_data_when_deconstruct = false; }; + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventInputInterface { + public: + friend class Component; + + template + requires std::invocable + explicit EventInputInterface(Callback&& callback) + : callback_(std::forward(callback)) {} + + EventInputInterface(const EventInputInterface&) = delete; + EventInputInterface& operator=(const EventInputInterface&) = delete; + EventInputInterface(EventInputInterface&&) = delete; + EventInputInterface& operator=(EventInputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + [[nodiscard]] bool ready() const { return static_cast(callback_); } + + private: + void* activate() { + if (!ready()) + throw std::runtime_error( + "The event input interface requires a callback before registration"); + + activated = true; + return reinterpret_cast(&callback_); + } + + std::function callback_; + bool activated = false; + }; + + template + requires( + !std::is_reference_v && !std::is_unbounded_array_v + && std::is_nothrow_copy_constructible_v && std::is_nothrow_destructible_v) + class QueuedEventInputInterface final : public EventInputInterface { + public: + template + requires std::invocable + QueuedEventInputInterface(size_t queue_depth, Callback&& callback) + : EventInputInterface([this](const T& event) { enqueue(event); }) + , user_callback_(std::forward(callback)) + , queue_(queue_depth) + , worker_(&QueuedEventInputInterface::worker_main, this) {} + + ~QueuedEventInputInterface() { + stop_requested_.store(true, std::memory_order::relaxed); + notify_event(); + if (worker_.joinable()) + worker_.join(); + } + + QueuedEventInputInterface(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface& operator=(const QueuedEventInputInterface&) = delete; + QueuedEventInputInterface(QueuedEventInputInterface&&) = delete; + QueuedEventInputInterface& operator=(QueuedEventInputInterface&&) = delete; + + private: + void enqueue(const T& event) { + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + auto guard = std::scoped_lock{enqueue_mutex_}; + if (stop_requested_.load(std::memory_order::relaxed)) + return; + + if (!queue_.push_back(event)) { + const auto dropped_count = dropped_event_count_++; + if (dropped_count == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface started dropping events because the queue is " + "full"); + } + return; + } + + const auto dropped_count = dropped_event_count_; + dropped_event_count_ = 0; + if (dropped_count != 0) { + RCLCPP_WARN( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface resumed enqueueing after dropping %u events", + dropped_count); + } + + notify_event(); + } + + void notify_event() { + event_count_.fetch_add(1, std::memory_order::release); + event_count_.notify_one(); + } + + void worker_main() { + while (!stop_requested_.load(std::memory_order::relaxed)) { + if (auto* event = queue_.peek_front()) { + try { + user_callback_(std::move(*event)); + } catch (const std::exception& exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by exception: %s", + exception.what()); + return; + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rmcs_executor"), + "QueuedEventInputInterface worker terminated by unknown exception"); + return; + } + + if (!queue_.pop_front([](T&&) noexcept {})) + std::terminate(); + continue; + } + + const auto old = event_count_.load(std::memory_order::relaxed); + if (!queue_.readable() && !stop_requested_.load(std::memory_order::relaxed)) + event_count_.wait(old, std::memory_order::acquire); + } + } + + std::function user_callback_; + rmcs_utility::RingBuffer queue_; + std::atomic stop_requested_{false}; + std::uint32_t dropped_event_count_ = 0; + std::atomic event_count_{0}; + std::mutex enqueue_mutex_; + std::thread worker_; + }; + template requires(!std::is_reference_v && !std::is_unbounded_array_v) class OutputInterface { public: @@ -104,25 +269,111 @@ class Component { ~OutputInterface() { if (active()) - std::destroy_at(std::launder(reinterpret_cast(&data_))); + std::destroy_at(storage_pointer()); }; [[nodiscard]] bool active() const { return activated; } - T* operator->() { return reinterpret_cast(&data_); } - const T* operator->() const { return reinterpret_cast(&data_); } - T& operator*() { return *reinterpret_cast(&data_); } - const T& operator*() const { return *reinterpret_cast(&data_); } + T* operator->() { return storage_pointer(); } + const T* operator->() const { return storage_pointer(); } + T& operator*() { return *storage_pointer(); } + const T& operator*() const { return *storage_pointer(); } private: template void* activate(Args&&... args) { - ::new (&data_) T(std::forward(args)...); + std::construct_at(raw_storage_pointer(), std::forward(args)...); + activated = true; + return data_; + } + + [[nodiscard]] T* raw_storage_pointer() { return reinterpret_cast(data_); } + + [[nodiscard]] T* storage_pointer() { return std::launder(raw_storage_pointer()); } + + [[nodiscard]] const T* storage_pointer() const { + return std::launder(reinterpret_cast(data_)); + } + + alignas(T) std::byte data_[sizeof(T)]; + bool activated = false; + }; + + template + requires(!std::is_reference_v && !std::is_unbounded_array_v) class EventOutputInterface { + public: + friend class Component; + + EventOutputInterface() = default; + + EventOutputInterface(const EventOutputInterface&) = delete; + EventOutputInterface& operator=(const EventOutputInterface&) = delete; + EventOutputInterface(EventOutputInterface&&) = delete; + EventOutputInterface& operator=(EventOutputInterface&&) = delete; + + [[nodiscard]] bool active() const { return activated; } + + void emit(const T& event) { + if (!active()) + throw std::runtime_error("The event output interface has not been activated"); + if (!try_enter_emit()) + return; + + struct LeaveEmitGuard { + EventOutputInterface& interface; + + ~LeaveEmitGuard() { interface.leave_emit(); } + } leave_emit_guard{*this}; + + for (const auto* callback : callback_list_) + (*callback)(event); + } + + private: + using EventCallback = std::function; + + void* activate() { activated = true; - return reinterpret_cast(&data_); + return this; } - std::aligned_storage_t data_; + void add_listener(EventCallback* callback) { callback_list_.emplace_back(callback); } + + bool try_enter_emit() { + auto state = state_.load(std::memory_order_relaxed); + while (true) { + if (state & EVENT_DISABLED_BIT) + return false; + if ((state & EVENT_ACTIVE_MASK) == EVENT_ACTIVE_MASK) + throw std::runtime_error("Too many active event emissions"); + + if (state_.compare_exchange_weak( + state, state + 1, std::memory_order_relaxed, std::memory_order_relaxed)) + return true; + } + } + + void leave_emit() { + const auto previous_state = state_.fetch_sub(1, std::memory_order_release); + const auto new_state = previous_state - 1; + if ((new_state & EVENT_DISABLED_BIT) != 0 && (new_state & EVENT_ACTIVE_MASK) == 0) + state_.notify_one(); + } + + void disable() { state_.fetch_or(EVENT_DISABLED_BIT, std::memory_order_relaxed); } + + void enable() { state_.fetch_and(EVENT_ACTIVE_MASK, std::memory_order_relaxed); } + + void wait_idle() { + auto state = state_.load(std::memory_order_acquire); + while ((state & EVENT_ACTIVE_MASK) != 0) { + state_.wait(state, std::memory_order_acquire); + state = state_.load(std::memory_order_acquire); + } + } + + std::vector callback_list_; + std::atomic state_{EVENT_DISABLED_BIT}; bool activated = false; }; @@ -133,7 +384,25 @@ class Component { const std::string& name, InputInterface& interface, bool required = true) { if (interface.active()) throw std::runtime_error("The interface has been activated"); - input_list_.emplace_back(typeid(T), name, required, interface.activate()); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Normal, required, interface.activate(), + &bind_input_interface); + } + + template + void register_input( + const std::string& name, EventInputInterface& interface, bool required = true) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Input); + input_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, required, interface.activate(), + &bind_event_input_interface); } template @@ -141,14 +410,31 @@ class Component { void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { if (interface.active()) throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Normal, RegistrationDirection::Output); output_list_.emplace_back( - typeid(T), name, interface.activate(std::forward(args)...), this); + typeid(T), name, InterfaceKind::Normal, interface.activate(std::forward(args)...), + nullptr, nullptr, nullptr, this); + } + + template + void register_output(const std::string& name, EventOutputInterface& interface) { + if (interface.active()) + throw std::runtime_error("The interface has been activated"); + + ensure_registration_name_is_available( + name, InterfaceKind::Event, RegistrationDirection::Output); + output_list_.emplace_back( + typeid(T), name, InterfaceKind::Event, interface.activate(), + &enable_event_output_interface, &disable_event_output_interface, + &wait_event_output_interface_idle, this); } template requires std::constructible_from std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { - initializing_component_name = name.c_str(); + initializing_component_name = name; auto component = std::make_shared(std::forward(args)...); partner_component_list_.emplace_back(component); @@ -156,26 +442,104 @@ class Component { return component; } - static const char* initializing_component_name; + static std::string initializing_component_name; protected: Component() : component_name_(initializing_component_name) {} private: + enum class RegistrationDirection { + Input, + Output, + }; + + using BindFunction = void (*)(void* input_binding, void* output_binding); + using OutputLifecycleHook = void (*)(void* output_binding); + + static const char* registration_direction_name(RegistrationDirection direction) { + switch (direction) { + case RegistrationDirection::Input: return "input"; + case RegistrationDirection::Output: return "output"; + } + + return "interface"; + } + + void ensure_registration_name_is_available( + const std::string& name, InterfaceKind kind, RegistrationDirection direction) const { + auto check_declarations = [&](const auto& declarations, + RegistrationDirection existing_direction) { + for (const auto& declaration : declarations) { + if (declaration.name != name) + continue; + + if (declaration.kind != kind) { + throw std::runtime_error( + "Component [" + component_name_ + "] cannot register " + + interface_kind_name(kind) + " " + registration_direction_name(direction) + + " \"" + name + "\" because \"" + name + + "\" has already been registered as " + + interface_kind_name(declaration.kind) + " " + + registration_direction_name(existing_direction) + " in this component."); + } + + throw std::runtime_error( + "Component [" + component_name_ + "] registered " + interface_kind_name(kind) + + " " + registration_direction_name(direction) + " \"" + name + + "\" more than once."); + } + }; + + check_declarations(input_list_, RegistrationDirection::Input); + check_declarations(output_list_, RegistrationDirection::Output); + } + + template + static void bind_input_interface(void* input_binding, void* output_binding) { + *reinterpret_cast(input_binding) = reinterpret_cast(output_binding); + } + + template + static void bind_event_input_interface(void* input_binding, void* output_binding) { + auto& callback = *reinterpret_cast*>(input_binding); + reinterpret_cast*>(output_binding)->add_listener(&callback); + } + + template + static void enable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->enable(); + } + + template + static void disable_event_output_interface(void* output_binding) { + reinterpret_cast*>(output_binding)->disable(); + } + + template + static void wait_event_output_interface_idle(void* output_binding) { + reinterpret_cast*>(output_binding)->wait_idle(); + } + std::string component_name_; struct InputDeclaration { const std::type_info& type; std::string name; + InterfaceKind kind; bool required; - void** pointer_to_data_pointer; + void* binding; + BindFunction bind; }; struct OutputDeclaration { const std::type_info& type; std::string name; - void* data_pointer; + InterfaceKind kind; + void* binding; + OutputLifecycleHook enable; + OutputLifecycleHook disable; + OutputLifecycleHook wait_idle; Component* component; }; diff --git a/rmcs_ws/src/rmcs_executor/package.xml b/rmcs_ws/src/rmcs_executor/package.xml index a287fb49..5ed7bcd6 100644 --- a/rmcs_ws/src/rmcs_executor/package.xml +++ b/rmcs_ws/src/rmcs_executor/package.xml @@ -11,6 +11,7 @@ rclcpp pluginlib + rmcs_utility ament_cmake diff --git a/rmcs_ws/src/rmcs_executor/src/component.cpp b/rmcs_ws/src/rmcs_executor/src/component.cpp index 561ac7a2..181ae975 100644 --- a/rmcs_ws/src/rmcs_executor/src/component.cpp +++ b/rmcs_ws/src/rmcs_executor/src/component.cpp @@ -2,6 +2,6 @@ namespace rmcs_executor { -const char* Component::initializing_component_name; +std::string Component::initializing_component_name; -} // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/src/executor.hpp index a818d50a..eb454fd0 100644 --- a/rmcs_ws/src/rmcs_executor/src/executor.hpp +++ b/rmcs_ws/src/rmcs_executor/src/executor.hpp @@ -2,8 +2,10 @@ #include #include +#include #include #include +#include #include #include @@ -21,18 +23,22 @@ class Executor final : public rclcpp::Node { public: explicit Executor( const std::string& node_name, rclcpp::executors::SingleThreadedExecutor& rcl_executor) - : Node{node_name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} + : Node{ + node_name, + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} , rcl_executor_(rcl_executor) { Component::initializing_component_name = "predefined_msg_provider"; - predefined_msg_provider_ = std::make_shared(); + predefined_msg_provider_ = std::make_shared(); add_component(predefined_msg_provider_); } - ~Executor() { + ~Executor() override { + disable_event_outputs(); + wait_event_outputs_idle(); if (thread_.joinable()) thread_.join(); }; - void add_component(std::shared_ptr component) { + void add_component(const std::shared_ptr& component) { component_list_.emplace_back(component); if (auto node = std::dynamic_pointer_cast(component)) rcl_executor_.add_node(node); @@ -50,35 +56,195 @@ class Executor final : public rclcpp::Node { if (!get_parameter("update_rate", update_rate)) throw std::runtime_error{"Unable to get parameter update_rate"}; predefined_msg_provider_->set_update_rate(update_rate); + enable_event_outputs(); thread_ = std::thread{[update_rate, this]() { const auto period = std::chrono::nanoseconds( static_cast(std::round(1'000'000'000.0 / update_rate))); auto next_iteration_time = std::chrono::steady_clock::now(); - while (rclcpp::ok()) { - predefined_msg_provider_->set_timestamp(next_iteration_time); - next_iteration_time += period; - for (const auto& component : updating_order_) { - component->update(); + try { + while (rclcpp::ok()) { + predefined_msg_provider_->set_timestamp(next_iteration_time); + next_iteration_time += period; + for (const auto& component : updating_order_) { + component->update(); + } + std::this_thread::sleep_until(next_iteration_time); } - std::this_thread::sleep_until(next_iteration_time); + } catch (const std::exception& exception) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by exception: %s", + exception.what()); + rclcpp::shutdown(); + } catch (...) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by unknown exception"); + rclcpp::shutdown(); } }}; } private: + void enable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.enable == nullptr) + continue; + output.enable(output.binding); + } + } + } + + void disable_event_outputs() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.disable == nullptr) + continue; + output.disable(output.binding); + } + } + } + + void wait_event_outputs_idle() { + for (const auto& component : component_list_) { + for (const auto& output : component->output_list_) { + if (output.wait_idle == nullptr) + continue; + output.wait_idle(output.binding); + } + } + } + + struct InterfaceKey { + std::string name; + InterfaceKind kind; + + bool operator==(const InterfaceKey& other) const { + return name == other.name && kind == other.kind; + } + }; + + struct InterfaceKeyHash { + std::size_t operator()(const InterfaceKey& key) const { + return std::hash{}(key.name) + ^ (std::hash{}(static_cast(key.kind)) << 1U); + } + }; + + using OutputRecord = Component::OutputDeclaration*; + + struct NameKindRecord { + InterfaceKind kind; + std::string component_name; + const char* direction; + }; + + static std::string describe_output( + const std::string& name, InterfaceKind kind, const std::type_info& type, + const std::string& component_name) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << " output \"" << name << "\" with type \"" << type.name() << "\""; + return stream.str(); + } + + static std::string describe_output(const Component::OutputDeclaration& output) { + return describe_output( + output.name, output.kind, output.type, output.component->get_component_name()); + } + + static std::string describe_declaration( + const std::string& name, InterfaceKind kind, const std::string& component_name, + const char* direction) { + std::ostringstream stream; + stream << "Component [" << component_name << "] registered " << interface_kind_name(kind) + << ' ' << direction << " \"" << name << "\""; + return stream.str(); + } + void init() { updating_order_.clear(); - auto output_map = std::unordered_map{}; - auto user_output_map = std::map{}; + auto output_map = std::unordered_map{}; + auto output_by_name = std::unordered_map{}; + auto declaration_kind_map = std::unordered_map{}; + auto user_output_map = Component::OutputInfoMap{}; for (const auto& component : component_list_) { component->dependency_count_ = 0; component->wanted_by_.clear(); for (auto& output : component->output_list_) { - if (!output_map.emplace(output.name, &output).second) - throw std::runtime_error{"Duplicate names of output"}; - user_output_map.emplace(output.name, output.type); + auto declaration_iter = declaration_kind_map.find(output.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != output.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + output.name + + "\": " + + describe_declaration( + output.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + output.name, output.kind, component->get_component_name(), "output") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + output.name, + NameKindRecord{output.kind, component->get_component_name(), "output"}); + + auto existing_output_iter = output_by_name.find(output.name); + if (existing_output_iter != output_by_name.end() + && existing_output_iter->second->kind != output.kind) { + const auto& existing_output = *existing_output_iter->second; + const auto message = std::string{"Conflicting output kinds for name \""} + + output.name + "\": " + describe_output(existing_output) + + "; " + describe_output(output) + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + output_by_name.emplace(output.name, &output); + + const auto [output_iter, inserted] = + output_map.emplace(InterfaceKey{output.name, output.kind}, &output); + if (!inserted) { + const auto& existing_output = *output_iter->second; + const auto message = + std::string{"Duplicate "} + interface_kind_name(output.kind) + + " output name \"" + output.name + + "\": " + describe_output(existing_output) + "; " + describe_output(output) + + ". Only one output may be registered for each (name, kind)."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + + user_output_map.emplace( + output.name, Component::OutputInfo{std::cref(output.type), output.kind}); + } + + for (const auto& input : component->input_list_) { + auto declaration_iter = declaration_kind_map.find(input.name); + if (declaration_iter != declaration_kind_map.end() + && declaration_iter->second.kind != input.kind) { + const auto message = + std::string{"Conflicting interface kinds for name \""} + input.name + "\": " + + describe_declaration( + input.name, declaration_iter->second.kind, + declaration_iter->second.component_name, + declaration_iter->second.direction) + + "; " + + describe_declaration( + input.name, input.kind, component->get_component_name(), "input") + + ". A name can only belong to one interface kind."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + declaration_kind_map.emplace( + input.name, + NameKindRecord{input.kind, component->get_component_name(), "input"}); } } @@ -88,36 +254,54 @@ class Executor final : public rclcpp::Node { for (const auto& component : component_list_) { for (const auto& input : component->input_list_) { - auto output_iter = output_map.find(input.name); + auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); if (output_iter == output_map.end()) { + auto output_by_name_iter = output_by_name.find(input.name); + if (output_by_name_iter != output_by_name.end()) { + const auto& available_output = *output_by_name_iter->second; + const auto message = + std::string{"Component ["} + component->get_component_name() + + "] requested " + interface_kind_name(input.kind) + " input \"" + + input.name + "\", but " + describe_output(available_output) + "."; + + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; + } + if (!input.required) continue; - RCLCPP_FATAL( - get_logger(), - "Cannot find the corresponding output of input \"%s\" declared by " - "component [%s]", - input.name.c_str(), component->get_component_name().c_str()); - throw std::runtime_error{"Cannot find the corresponding output"}; + const auto message = std::string{"Cannot find "} + + interface_kind_name(input.kind) + " output \"" + input.name + + "\" required by component [" + + component->get_component_name() + "]."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); + throw std::runtime_error{message}; } const auto& output = *output_iter->second; if (input.type != output.type) { - RCLCPP_FATAL(get_logger(), "With message \"%s\":", input.name.c_str()); + const auto message = std::string{"Type mismatch for "} + + interface_kind_name(input.kind) + " interface \"" + + input.name + "\": component [" + + output.component->get_component_name() + + "] declared output type \"" + output.type.name() + + "\", but component [" + component->get_component_name() + + "] requested input type \"" + input.type.name() + "\"."; + RCLCPP_FATAL(get_logger(), "%s", message.c_str()); RCLCPP_FATAL( get_logger(), " Component [%s] declared the output with type \"%s\"", output.component->get_component_name().c_str(), output.type.name()); RCLCPP_FATAL( get_logger(), " Component [%s] requested the input with type \"%s\"", component->get_component_name().c_str(), input.type.name()); - RCLCPP_FATAL(get_logger(), "Type not match."); - throw std::runtime_error{"Type not match"}; + throw std::runtime_error{message}; } if (output.component->wanted_by_.emplace(component.get()).second) component->dependency_count_++; - *input.pointer_to_data_pointer = output.data_pointer; + input.bind(input.binding, output.binding); } } @@ -140,12 +324,17 @@ class Executor final : public rclcpp::Node { RCLCPP_FATAL( get_logger(), "Component [%s]:", component->get_component_name().c_str()); for (const auto& input : component->input_list_) { - const auto& output = output_map[input.name]; + const auto output_iter = output_map.find(InterfaceKey{input.name, input.kind}); + if (output_iter == output_map.end()) + continue; + + const auto* output = output_iter->second; if (output->component->dependency_count_ == 0) continue; RCLCPP_FATAL( - get_logger(), " Depends on [%s] because requesting \"%s\"", - output->component->component_name_.c_str(), output->name.c_str()); + get_logger(), " Depends on [%s] because requesting %s interface \"%s\"", + output->component->component_name_.c_str(), interface_kind_name(input.kind), + output->name.c_str()); } } throw std::runtime_error{"Circular dependency found"}; @@ -182,4 +371,4 @@ class Executor final : public rclcpp::Node { size_t dependency_recursive_level_ = 0; }; -}; // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index 99311f24..da4c29fb 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv) { plugin_name = component_name = component_description; } - rmcs_executor::Component::initializing_component_name = component_name.c_str(); + rmcs_executor::Component::initializing_component_name = component_name; auto component = component_loader.createSharedInstance(plugin_name); executor->add_component(component); } @@ -70,4 +70,4 @@ int main(int argc, char** argv) { rcl_executor.spin(); rclcpp::shutdown(); -} \ No newline at end of file +} diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp new file mode 100644 index 00000000..e96fa76c --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/board_clock.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct BoardClock { + using rep = std::int64_t; + using period = std::ratio<1, 4'000'000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + + static constexpr bool is_steady = true; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp new file mode 100644 index 00000000..526b79f2 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/camera_frame_raw.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace rmcs_msgs { + +struct CameraFrameRaw { + static constexpr std::uint32_t kWidth = 1440; + static constexpr std::uint32_t kHeight = 1080; + static constexpr std::size_t kFrameSize = + static_cast(kWidth) * static_cast(kHeight); + + std::array data; + int opencv_cvt_color_code; + + Eigen::Quaterniond imu_snapshot; + + std::chrono::steady_clock::time_point exposure_timestamp; + std::chrono::steady_clock::time_point image_reception_timestamp; + std::chrono::steady_clock::time_point sync_publish_timestamp; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index e534ce0b..92d391b5 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -5,9 +5,9 @@ namespace rmcs_msgs { enum class ChassisMode : uint8_t { - AUTO = 0, - SPIN = 1, - STEP_DOWN = 2, + AUTO = 0, + SPIN = 1, + STEP_DOWN = 2, LAUNCH_RAMP = 3, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp index b0ec852c..4cdc6d5d 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp @@ -12,43 +12,43 @@ class FullRobotId { enum Value : uint16_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, - - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, + + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, - - RED_HERO_CLIENT = 0x0101, - RED_ENGINEER_CLIENT = 0x0102, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, + + RED_HERO_CLIENT = 0x0101, + RED_ENGINEER_CLIENT = 0x0102, RED_INFANTRY_III_CLIENT = 0x0103, - RED_INFANTRY_IV_CLIENT = 0x0104, - RED_INFANTRY_V_CLIENT = 0x0105, - RED_AERIAL_CLIENT = 0x0106, + RED_INFANTRY_IV_CLIENT = 0x0104, + RED_INFANTRY_V_CLIENT = 0x0105, + RED_AERIAL_CLIENT = 0x0106, - BLUE_HERO_CLIENT = 0x0165, - BLUE_ENGINEER_CLIENT = 0x0166, + BLUE_HERO_CLIENT = 0x0165, + BLUE_ENGINEER_CLIENT = 0x0166, BLUE_INFANTRY_III_CLIENT = 0x0167, - BLUE_INFANTRY_IV_CLIENT = 0x0168, - BLUE_INFANTRY_V_CLIENT = 0x0169, - BLUE_AERIAL_CLIENT = 0x016A, + BLUE_INFANTRY_IV_CLIENT = 0x0168, + BLUE_INFANTRY_V_CLIENT = 0x0169, + BLUE_AERIAL_CLIENT = 0x016A, REFEREE_SERVER = 0x8080, }; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp index 622a171d..d94e04dc 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp @@ -5,13 +5,13 @@ namespace rmcs_msgs { enum class GameStage : uint8_t { - NOT_START = 0, - PREPARATION = 1, + NOT_START = 0, + PREPARATION = 1, REFEREE_CHECK = 2, - COUNTDOWN = 3, - STARTED = 4, - SETTLING = 5, - UNKNOWN = UINT8_MAX, + COUNTDOWN = 3, + STARTED = 4, + SETTLING = 5, + UNKNOWN = UINT8_MAX, }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp new file mode 100644 index 00000000..1c0747b3 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/imu_snapshot.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include +#include + +namespace rmcs_msgs { + +struct ImuSnapshot { + Eigen::Quaterniond orientation; + Eigen::Vector3d gyro_body; + rmcs_msgs::BoardClock::time_point timestamp; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp index 09925e7b..b2ec7c37 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/rmcs_msgs.hpp @@ -9,18 +9,21 @@ # endif #endif -#include "chassis_mode.hpp" -#include "full_robot_id.hpp" -#include "game_stage.hpp" -#include "gimbal_mode.hpp" -#include "keyboard.hpp" -#include "mouse.hpp" -#include "robot_color.hpp" -#include "robot_id.hpp" -#include "serial_interface.hpp" -#include "shoot_mode.hpp" -#include "shoot_status.hpp" -#include "switch.hpp" +#include "board_clock.hpp" // IWYU pragma: export +#include "camera_frame_raw.hpp" // IWYU pragma: export +#include "chassis_mode.hpp" // IWYU pragma: export +#include "full_robot_id.hpp" // IWYU pragma: export +#include "game_stage.hpp" // IWYU pragma: export +#include "gimbal_mode.hpp" // IWYU pragma: export +#include "imu_snapshot.hpp" // IWYU pragma: export +#include "keyboard.hpp" // IWYU pragma: export +#include "mouse.hpp" // IWYU pragma: export +#include "robot_color.hpp" // IWYU pragma: export +#include "robot_id.hpp" // IWYU pragma: export +#include "serial_interface.hpp" // IWYU pragma: export +#include "shoot_mode.hpp" // IWYU pragma: export +#include "shoot_status.hpp" // IWYU pragma: export +#include "switch.hpp" // IWYU pragma: export namespace rmcs_msgs { diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp index dc7ee694..c5517f2d 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp @@ -6,8 +6,8 @@ namespace rmcs_msgs { enum class RobotColor : uint8_t { UNKNOWN = 0, - RED = 1, - BLUE = 2, + RED = 1, + BLUE = 2, }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index 622d5fdc..61e063e2 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -6,18 +6,18 @@ namespace rmcs_msgs { enum class ArmorID : uint16_t { - Unknown = 0, - Hero = 1, - Engineer = 2, + Unknown = 0, + Hero = 1, + Engineer = 2, InfantryIII = 3, - InfantryIV = 4, - InfantryV = 5, - Aerial = 6, - Sentry = 7, - Dart = 8, - Radar = 9, - Outpost = 10, - Base = 11, + InfantryIV = 4, + InfantryV = 5, + Aerial = 6, + Sentry = 7, + Dart = 8, + Radar = 9, + Outpost = 10, + Base = 11, }; class RobotId { @@ -25,29 +25,29 @@ class RobotId { enum Value : uint8_t { UNKNOWN = 0, - RED_HERO = 1, - RED_ENGINEER = 2, + RED_HERO = 1, + RED_ENGINEER = 2, RED_INFANTRY_III = 3, - RED_INFANTRY_IV = 4, - RED_INFANTRY_V = 5, - RED_AERIAL = 6, - RED_SENTRY = 7, - RED_DART = 8, - RED_RADAR = 9, - RED_OUTPOST = 10, - RED_BASE = 11, + RED_INFANTRY_IV = 4, + RED_INFANTRY_V = 5, + RED_AERIAL = 6, + RED_SENTRY = 7, + RED_DART = 8, + RED_RADAR = 9, + RED_OUTPOST = 10, + RED_BASE = 11, - BLUE_HERO = 101, - BLUE_ENGINEER = 102, + BLUE_HERO = 101, + BLUE_ENGINEER = 102, BLUE_INFANTRY_III = 103, - BLUE_INFANTRY_IV = 104, - BLUE_INFANTRY_V = 105, - BLUE_AERIAL = 106, - BLUE_SENTRY = 107, - BLUE_DART = 108, - BLUE_RADAR = 109, - BLUE_OUTPOST = 110, - BLUE_BASE = 111, + BLUE_INFANTRY_IV = 104, + BLUE_INFANTRY_V = 105, + BLUE_AERIAL = 106, + BLUE_SENTRY = 107, + BLUE_DART = 108, + BLUE_RADAR = 109, + BLUE_OUTPOST = 110, + BLUE_BASE = 111, }; constexpr RobotId() @@ -67,7 +67,9 @@ class RobotId { constexpr bool operator==(const Value value) const { return value_ == value; } constexpr bool operator!=(const Value value) const { return value_ != value; } - constexpr RobotColor color() const { return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } + constexpr RobotColor color() const { + return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; + } constexpr ArmorID id() const { return value_ > 100 ? static_cast(value_ - 100) : static_cast(value_); diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp index a198e5a8..52d736a4 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp @@ -7,7 +7,7 @@ namespace rmcs_msgs { struct SerialInterface { std::function read; - std::function write; + std::function write; }; -} \ No newline at end of file +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp new file mode 100644 index 00000000..95dd4090 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_condiction.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ShootCondiction : uint8_t { + FRICTION_WAITING = 0, + PRELOADING = 1, + SHOOT = 2, + JAM = 3, + FIRED = 4, +}; +} diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp new file mode 100644 index 00000000..78ed4062 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/atomic_futex.hpp @@ -0,0 +1,160 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_utility { +namespace detail { + +inline auto atomic_futex_address(const std::atomic& atomic) noexcept -> uint32_t* { + return const_cast(reinterpret_cast(&atomic)); +} + +inline bool atomic_futex_wait_until_steady( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, std::memory_order order) noexcept { + + if (atomic.load(order) != old_val) + return true; + + timespec abs_time; + clock_gettime(CLOCK_MONOTONIC, &abs_time); + + const auto remaining = + std::chrono::ceil(deadline - std::chrono::steady_clock::now()); + + if (remaining <= std::chrono::nanoseconds::zero()) + return atomic.load(order) != old_val; + + const auto secs = std::chrono::duration_cast(remaining); + const auto nsecs = std::chrono::duration_cast(remaining - secs); + + abs_time.tv_sec += static_cast(secs.count()); + abs_time.tv_nsec += static_cast(nsecs.count()); + + if (abs_time.tv_nsec >= 1'000'000'000) { + abs_time.tv_sec += 1; + abs_time.tv_nsec -= 1'000'000'000; + } + + auto* const ptr = atomic_futex_address(atomic); + + while (atomic.load(order) == old_val) { + if (syscall( + SYS_futex, ptr, FUTEX_WAIT_BITSET_PRIVATE, old_val, &abs_time, nullptr, + FUTEX_BITSET_MATCH_ANY) + == 0) + continue; + + if (errno == EAGAIN || errno == EINTR) + continue; + + if (errno == ETIMEDOUT) + return atomic.load(order) != old_val; + + return atomic.load(order) != old_val; + } + + return true; +} + +} // namespace detail + +/** + * @brief Waits until a 32-bit atomic value changes or the timeout expires. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param timeout Maximum time to wait. A non-positive timeout performs a non-blocking check. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the timeout + * expires. + */ +inline bool atomic_futex_wait_for( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::duration timeout, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + if (atomic.load(order) != old_val) + return true; + + if (timeout <= std::chrono::steady_clock::duration::zero()) + return false; + + return detail::atomic_futex_wait_until_steady( + atomic, old_val, std::chrono::steady_clock::now() + timeout, order); +} + +/** + * @brief Waits until a 32-bit atomic value changes or the steady-clock deadline is reached. + * + * This helper uses Linux futex syscalls directly and must be paired with + * rmcs_utility::atomic_futex_notify_one() or rmcs_utility::atomic_futex_notify_all() + * on the same atomic object. + * + * @warning This implementation is not compatible with `std::atomic::notify_one()` or + * `std::atomic::notify_all()`. The standard library may track waiters in an internal pool, + * so mixing these APIs on the same atomic object can miss wakeups. + * + * @param atomic Atomic word used as the futex key. + * @param old_val Expected value observed before waiting. + * @param deadline Steady-clock deadline after which the wait times out. + * @param order Memory order used for polling loads around the futex wait. + * @return `true` if `atomic` no longer equals `old_val`, otherwise `false` when the deadline + * is reached. + */ +inline bool atomic_futex_wait_until( + const std::atomic& atomic, uint32_t old_val, + std::chrono::steady_clock::time_point deadline, + std::memory_order order = std::memory_order_seq_cst) noexcept { + + return detail::atomic_futex_wait_until_steady(atomic, old_val, deadline, order); +} + +/** + * @brief Wakes one thread blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_one(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, 1, nullptr, + nullptr, FUTEX_BITSET_MATCH_ANY); +} + +/** + * @brief Wakes all threads blocked in rmcs_utility::atomic_futex_wait_for() or + * rmcs_utility::atomic_futex_wait_until(). + * + * @warning This must not be mixed with `std::atomic::notify_one()` or + * `std::atomic::notify_all()` on the same atomic object. + * + * @param atomic Atomic word used as the futex key. + */ +inline void atomic_futex_notify_all(const std::atomic& atomic) noexcept { + syscall( + SYS_futex, detail::atomic_futex_address(atomic), FUTEX_WAKE_BITSET_PRIVATE, INT_MAX, + nullptr, nullptr, FUTEX_BITSET_MATCH_ANY); +} + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp index 620c6023..be552b7e 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp @@ -11,7 +11,7 @@ inline auto& get_tail(T& data) { return *reinterpret_cast(reinterpret_cast(&data) + sizeof(T) - sizeof(uint8_t)); } -constexpr uint8_t crc8_init = 0xff; +constexpr uint8_t crc8_init = 0xff; constexpr uint8_t crc8_table[256] = { 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, @@ -31,7 +31,7 @@ constexpr uint8_t crc8_table[256] = { 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, }; -constexpr uint16_t crc16_init = 0xffff; +constexpr uint16_t crc16_init = 0xffff; constexpr uint16_t crc16_table[256] = { 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, @@ -59,7 +59,7 @@ constexpr uint16_t crc16_table[256] = { } // namespace internal inline uint8_t calculate_crc8(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); auto result = internal::crc8_init; while (length--) result = internal::crc8_table[result ^ (*p++)]; @@ -87,7 +87,7 @@ inline void append_crc8(T& package) { } inline uint16_t calculate_crc16(const void* data, size_t length) { - auto* p = reinterpret_cast(data); + auto* p = reinterpret_cast(data); uint16_t result = internal::crc16_init; while (length--) { result = (result >> 8) ^ internal::crc16_table[(result ^ (*p++)) & 0x00ff]; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp index 67edfce9..9e473d50 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp @@ -21,7 +21,7 @@ requires(std::is_trivially_copyable_v && std::is_trivially_destructible_v) /// @param data The data to be written to the buffer. void write(const T& data) noexcept { const uint64_t current = current_.load(std::memory_order_relaxed); - const uint64_t next = current + 1; + const uint64_t next = current + 1; std::memcpy(&buffers_[next & 1], &data, sizeof(T)); current_.store(next, std::memory_order_release); } diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp index 2c5ff84f..05af4814 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp @@ -14,11 +14,11 @@ class FpsCounter { bool count() { ++count_; - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); auto elapsed_time = now - start_; if (elapsed_time >= measurement_window_) { start_ = now; - fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); + fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); count_ = 0; return true; } @@ -33,7 +33,7 @@ class FpsCounter { std::chrono::steady_clock::time_point start_; int64_t count_ = 0; - double fps_ = 0; + double fps_ = 0; }; } // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp new file mode 100644 index 00000000..2044f4a1 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/memory_pool.hpp @@ -0,0 +1,196 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_utility { + +template +class MemoryPool { + static_assert(Size > 0, "MemoryPool slot size must be greater than zero"); + static_assert(Align > 0, "MemoryPool slot alignment must be greater than zero"); + static_assert((Align & (Align - 1)) == 0, "MemoryPool slot alignment must be a power of two"); + +public: + /*! + * @brief Construct a fixed-capacity raw storage pool + * @param capacity Number of storage slots to allocate + * @note This container is not thread-safe. + */ + explicit MemoryPool(size_t capacity) + : capacity_(capacity) + , storage_(new Storage[capacity]) + , free_stack_(new size_t[capacity]) + , occupied_(new bool[capacity]{}) { + reset_free_stack(); + } + + MemoryPool(const MemoryPool&) = delete; + MemoryPool& operator=(const MemoryPool&) = delete; + MemoryPool(MemoryPool&&) = delete; + MemoryPool& operator=(MemoryPool&&) = delete; + + /*! + * @brief Destructor + * @note All allocated slots must be returned to the pool before destruction. + * Violating this lifetime contract terminates the program. + */ + ~MemoryPool() noexcept { + if (!empty()) { + std::fprintf( + stderr, + "rmcs_utility::MemoryPool %p destroyed with outstanding slots: size=%zu, " + "capacity=%zu\n", + static_cast(this), size_, capacity_); + std::fflush(stderr); + std::terminate(); + } + } + + /*! + * @brief Payload size of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_size() noexcept { return Size; } + + /*! + * @brief Alignment guarantee of each storage slot in bytes + */ + [[nodiscard]] static constexpr size_t slot_align() noexcept { return Align; } + + /*! + * @brief Capacity of the memory pool + * @return Total number of storage slots + */ + [[nodiscard]] size_t max_size() const noexcept { return capacity_; } + + /*! + * @brief Number of currently allocated storage slots + */ + [[nodiscard]] size_t size() const noexcept { return size_; } + + /*! + * @brief Number of free storage slots remaining + */ + [[nodiscard]] size_t available() const noexcept { return capacity_ - size_; } + + /*! + * @brief Check whether the pool contains no allocated slot + */ + [[nodiscard]] bool empty() const noexcept { return size_ == 0; } + + /*! + * @brief Check whether the pool has no free slot + */ + [[nodiscard]] bool full() const noexcept { return size_ == capacity_; } + + /*! + * @brief Allocate one raw storage slot from the pool + * @return Pointer to the slot's raw storage, or nullptr if the pool is full + */ + [[nodiscard]] void* allocate() noexcept { + if (full()) + return nullptr; + + const auto index = free_stack_[top_ - 1]; + occupied_[index] = true; + --top_; + ++size_; + + return static_cast(slot_pointer(index)); + } + + /*! + * @brief Release a raw storage slot back to the pool + * @param pointer Pointer previously returned by this pool + * @return true if the slot was released, false if the pointer was invalid + * @note The pointer must refer to the start address of a live slot. + */ + bool free(void* pointer) noexcept { + const auto index = pointer_to_index(pointer); + + if (index == capacity_ || !occupied_[index]) + return false; + + occupied_[index] = false; + free_stack_[top_] = index; + ++top_; + --size_; + + return true; + } + + /*! + * @brief Check whether a pointer refers to a live slot owned by this pool + */ + [[nodiscard]] bool contains(const void* pointer) const noexcept { + const auto index = pointer_to_index(pointer); + return index != capacity_ && occupied_[index]; + } + + /*! + * @brief Release every live slot and reset the pool + * @return Number of slots that were released + */ + size_t clear() noexcept { + size_t count = 0; + + for (size_t i = 0; i < capacity_; i++) { + if (occupied_[i]) { + occupied_[i] = false; + ++count; + } + } + + size_ = 0; + reset_free_stack(); + + return count; + } + +private: + struct Storage { + alignas(Align) std::byte data[Size]; + }; + + [[nodiscard]] std::byte* slot_pointer(size_t index) noexcept { return storage_[index].data; } + + [[nodiscard]] const std::byte* slot_pointer(size_t index) const noexcept { + return storage_[index].data; + } + + [[nodiscard]] size_t pointer_to_index(const void* pointer) const noexcept { + if (pointer == nullptr) + return capacity_; + + const auto begin = reinterpret_cast(storage_.get()); + const auto end = begin + sizeof(Storage) * capacity_; + const auto target = reinterpret_cast(pointer); + + if (target < begin || target >= end) + return capacity_; + + const auto offset = target - begin; + if (offset % sizeof(Storage) != 0) + return capacity_; + + return static_cast(offset / sizeof(Storage)); + } + + void reset_free_stack() noexcept { + for (size_t i = 0; i < capacity_; i++) + free_stack_[capacity_ - 1 - i] = i; + top_ = capacity_; + } + + size_t capacity_; + std::unique_ptr storage_; + std::unique_ptr free_stack_; + std::unique_ptr occupied_; + + size_t top_{0}; + size_t size_{0}; +}; + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp index 60571f8e..f9678c60 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp @@ -10,8 +10,8 @@ namespace rmcs_utility { enum class ReceiveResult : uint8_t { - SUCCESS = 0, - TIMEOUT = 1, + SUCCESS = 0, + TIMEOUT = 1, HEADER_INVALID = 2, VERIFY_INVALID = 3 }; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp new file mode 100644 index 00000000..c2e65a19 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/pooled_shared_factory.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rmcs_utility/memory_pool.hpp" + +namespace rmcs_utility { + +template +requires std::is_nothrow_destructible_v class PooledSharedFactory { +public: + explicit PooledSharedFactory(size_t capacity) + : pool_(std::make_shared(capacity)) {} + + template + [[nodiscard]] std::shared_ptr make(Args&&... args) { + auto ptr = try_make(std::forward(args)...); + if (!ptr) + throw std::bad_alloc{}; + return ptr; + } + + template + [[nodiscard]] std::shared_ptr try_make(Args&&... args) { + void* storage = nullptr; + { + auto guard = std::scoped_lock{pool_->mutex}; + storage = pool_->allocate(); + } + + if (!storage) + return nullptr; + + try { + return std::shared_ptr( + std::construct_at(static_cast(storage), std::forward(args)...), + [pool = pool_](T* ptr) { + std::destroy_at(ptr); + auto guard = std::scoped_lock{pool->mutex}; + if (!pool->free(ptr)) { + std::fprintf( + stderr, + "rmcs_utility::PooledSharedFactory failed to return slot %p to pool " + "%p\n", + static_cast(ptr), static_cast(pool.get())); + std::fflush(stderr); + std::terminate(); + } + }); + } catch (...) { + auto guard = std::scoped_lock{pool_->mutex}; + pool_->free(storage); + throw; + } + } + + [[nodiscard]] size_t max_size() const noexcept { return pool_->max_size(); } + +private: + class PoolImpl final : public MemoryPool { + public: + using MemoryPool::MemoryPool; + + mutable std::mutex mutex; + }; + + std::shared_ptr pool_; +}; + +} // 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 82a93fba..5f9f065b 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 @@ -2,10 +2,13 @@ #include #include +#include #include +#include #include #include #include +#include #include namespace rmcs_utility { @@ -15,6 +18,179 @@ namespace rmcs_utility { template class RingBuffer { public: + template + class ReadableView; + + template + class BasicIterator { + using Buffer = std::conditional_t; + + public: + using iterator_category = std::random_access_iterator_tag; + using iterator_concept = std::random_access_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using reference = std::conditional_t; + using pointer = std::conditional_t; + + BasicIterator() = default; + BasicIterator(const BasicIterator&) = default; + BasicIterator(BasicIterator&&) = default; + BasicIterator& operator=(const BasicIterator&) = default; + BasicIterator& operator=(BasicIterator&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + BasicIterator(const BasicIterator& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , offset_(other.offset_) {} + + reference operator*() const { return *ptr(); } + pointer operator->() const { return ptr(); } + reference operator[](difference_type n) const { return *(*this + n); } + + BasicIterator& operator++() { + offset_++; + return *this; + } + + BasicIterator operator++(int) { + auto old = *this; + ++*this; + return old; + } + + BasicIterator& operator--() { + offset_--; + return *this; + } + + BasicIterator operator--(int) { + auto old = *this; + --*this; + return old; + } + + BasicIterator& operator+=(difference_type n) { + if (n >= 0) + offset_ += static_cast(n); + else + offset_ -= static_cast(-(n + 1)) + 1; + return *this; + } + + BasicIterator& operator-=(difference_type n) { + if (n >= 0) + offset_ -= static_cast(n); + else + offset_ += static_cast(-(n + 1)) + 1; + return *this; + } + + friend BasicIterator operator+(BasicIterator it, difference_type n) { + it += n; + return it; + } + + friend BasicIterator operator+(difference_type n, BasicIterator it) { + it += n; + return it; + } + + friend BasicIterator operator-(BasicIterator it, difference_type n) { + it -= n; + return it; + } + + friend difference_type operator-(BasicIterator lhs, BasicIterator rhs) { + if (lhs.offset_ >= rhs.offset_) + return static_cast(lhs.offset_ - rhs.offset_); + return -static_cast(rhs.offset_ - lhs.offset_); + } + + friend bool operator==(BasicIterator lhs, BasicIterator rhs) { + return lhs.buffer_ == rhs.buffer_ && lhs.origin_ == rhs.origin_ + && lhs.offset_ == rhs.offset_; + } + + friend std::strong_ordering operator<=>(BasicIterator lhs, BasicIterator rhs) { + return lhs.offset_ <=> rhs.offset_; + } + + private: + friend class RingBuffer; + template + friend class BasicIterator; + template + friend class ReadableView; + + BasicIterator(Buffer* buffer, size_t origin, size_t offset) + : buffer_(buffer) + , origin_(origin) + , offset_(offset) {} + + pointer ptr() const { + return std::launder( + reinterpret_cast( + buffer_->storage_[(origin_ + offset_) & buffer_->mask_].data)); + } + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t offset_ = 0; + }; + + using iterator = BasicIterator; + using const_iterator = BasicIterator; + + template + class ReadableView { + using Buffer = std::conditional_t; + + public: + using iterator = BasicIterator; + using value_type = T; + using difference_type = std::ptrdiff_t; + using size_type = size_t; + using reference = std::conditional_t; + + ReadableView() = default; + ReadableView(const ReadableView&) = default; + ReadableView(ReadableView&&) = default; + ReadableView& operator=(const ReadableView&) = default; + ReadableView& operator=(ReadableView&&) = default; + + // NOLINTNEXTLINE(google-explicit-constructor) + ReadableView(const ReadableView& other) requires is_const + : buffer_(other.buffer_) + , origin_(other.origin_) + , size_(other.size_) {} + + iterator begin() const { return iterator{buffer_, origin_, 0}; } + iterator end() const { return iterator{buffer_, origin_, size_}; } + + [[nodiscard]] bool empty() const { return size_ == 0; } + [[nodiscard]] size_type size() const { return size_; } + + reference front() const { return *begin(); } + reference back() const { return *(end() - 1); } + reference operator[](size_type index) const { return begin()[index]; } + + private: + friend class RingBuffer; + template + friend class ReadableView; + + ReadableView(Buffer* buffer, size_t origin, size_t size) + : buffer_(buffer) + , origin_(origin) + , size_(size) {} + + Buffer* buffer_ = nullptr; + size_t origin_ = 0; + size_t size_ = 0; + }; + /*! * @brief Construct an SPSC ring buffer * @param size Minimum capacity requested. Actual capacity is rounded up @@ -75,6 +251,31 @@ class RingBuffer { return max_size() - (in - out); } + /*! + * @brief Snapshot view of elements currently readable by the consumer + * @note Captures [out, in) once. Producer pushes do not extend the returned view. + * Consumer pops invalidate iterators to erased elements. + */ + ReadableView readable_view() noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Const snapshot view of elements currently readable by the consumer + */ + ReadableView readable_view() const noexcept { + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + return {this, out, in - out}; + } + + /*! + * @brief Explicit const snapshot view for non-const buffers + */ + ReadableView const_readable_view() const noexcept { return readable_view(); } + /*! * @brief Peek the first element (consumer side) * @return Pointer to the first element, or nullptr if empty @@ -277,6 +478,24 @@ class RingBuffer { return pop_front_n(std::forward(callback_functor), 1); } + /*! + * @brief Pop readable elements before a snapshot iterator + * @return Number of elements erased from the front + * @note Consumer-only. `pos` must come from this buffer's current readable range. + */ + size_t pop_front_until(const_iterator pos) { + if (pos.buffer_ != this) + return 0; + + const auto in = in_.load(std::memory_order::acquire); + const auto out = out_.load(std::memory_order::relaxed); + const auto count = pos.origin_ + pos.offset_ - out; + if (count > in - out) + return 0; + + return pop_front_n([](const T&) noexcept {}, count); + } + /*! * @brief Clear the buffer by consuming all elements * @return Number of elements that were erased From 90ceab4b417c175aeba2e26a2c5762d763c7979a Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 16:22:15 +0800 Subject: [PATCH 74/90] chore(plugins): remove DeformableInfantryV2 class --- rmcs_ws/src/rmcs_core/plugins.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 8d30c5cf..560cf036 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -5,7 +5,6 @@ - From 3f419b8f7e3df796511dc69265b43f7b5348204a Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 16:22:15 +0800 Subject: [PATCH 75/90] chore(plugins): remove DeformableInfantryV2 class --- rmcs_ws/src/rmcs_core/plugins.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 8d30c5cf..560cf036 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -5,7 +5,6 @@ - From 166addb0366ad8ca4631008bf6175b74cbbeb939 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 18:52:50 +0800 Subject: [PATCH 76/90] Squashed commit of the following: commit 2c63c7d9218fc005f984b6d92b43404271a9d7f8 Author: creeper5820 <131014151+creeper5820@users.noreply.github.com> Date: Tue Jul 7 00:03:30 2026 +0800 feat: Integrate flight support (#85) - Add flight.yaml parameter config for Flight hardware, gimbal, friction wheels, feeder, referee interaction, auto-aim, and odin_ros_driver. - Add rmcs_core::hardware::Flight hardware plugin supporting flight controller, IMU, remote control, CAN/UART comms, TF, and referee serial. - Add rmcs_core::referee::app::ui::Flight UI plugin for ammo, friction wheel status, and auto-aim mode display. - Replace/add Flight-related hardware, controller, and UI components in plugin registry; remove some legacy chassis/controller registrations. - Add auto-aim toggle, yaw limit support, and direction clamping based on current angle to gimbal control. - Switch shooting control to /auto_aim/should_shoot input and refine auto/manual shooting logic. - Add kMHF7015 motor type support. - Add supercap/battery display toggle to StatusRing. - Minor CMake, description file, and export formatting/cleanup. Co-authored-by: heyeuu <2829004293@qq.com> Co-authored-by: zlq040222 <1542498005@qq.com> commit 1e99fd9191b7cc74125234dff105878e6b9aeb70 Author: Zihan Qin Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab65867b13d54858f7d0938b309ad619d3fe02 Author: Zihan Qin Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect commit b5ae6cb717b64b300086ec6cbc434b48561d450c Author: Zihan Qin Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d36a9a4e1de2e54cb452352ef2d16263ff Author: qzhhhi Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files --- .script/template/entrypoint | 15 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 153 ++++++++++ rmcs_ws/src/rmcs_core/CMakeLists.txt | 11 +- rmcs_ws/src/rmcs_core/plugins.xml | 15 +- .../gimbal/simple_gimbal_controller.cpp | 30 +- .../gimbal/two_axis_gimbal_solver.hpp | 38 +++ .../src/hardware/device/lk_motor.hpp | 15 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 274 ++++++++++++++++++ .../rmcs_core/src/referee/app/ui/flight.cpp | 96 ++++++ .../src/referee/app/ui/widget/status_ring.hpp | 20 +- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 3 +- .../rmcs_description/sentry_description.hpp | 20 +- .../rmcs_description/tf_description.hpp | 18 +- 13 files changed, 660 insertions(+), 48 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp diff --git a/.script/template/entrypoint b/.script/template/entrypoint index e8660e7a..49290c5c 100755 --- a/.script/template/entrypoint +++ b/.script/template/entrypoint @@ -1,5 +1,18 @@ #!/usr/bin/bash +USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}" +USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb" + +if [ -w "$USBFS_MEMORY_MB_PATH" ]; then + if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then + echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2 + fi +elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then + echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2 +else + echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2 +fi + # Remove all files in /tmp rm -rf /tmp/* @@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then service avahi-daemon start fi -sleep infinity \ No newline at end of file +sleep infinity diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 00000000..2291e12a --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,153 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + + - 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::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::Command -> referee_command + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs::AutoAimComponent + +odin_ros_driver: + ros__parameters: + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/velocity_imu + - /gimbal/bullet_feeder/velocity + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" + yaw_motor_zero_point: 11720 + pitch_motor_zero_point: 18578 + +referee_status: + ros__parameters: + path: /dev/tty0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° + lower_limit: 0.7 # 0.7 rad ≈ 40.1° + yaw_lower_limit: 0.1745 + yaw_upper_limit: 2.5708 + +yaw_angle_pid_controller: + ros__parameters: + 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: 8.0 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 620.0 + - 620.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 15000 + +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.2 + 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.00 + 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.00 + 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: 0.583 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d275c6db..49ce42d2 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,11 +8,12 @@ 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) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() 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( @@ -24,13 +25,13 @@ FetchContent_Declare( FetchContent_MakeAvailable(librmcs) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.c + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.c ) ament_auto_add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 560cf036..08264072 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,5 +1,5 @@ - + @@ -20,14 +20,14 @@ - + - + @@ -36,18 +36,23 @@ + + + + + + - - + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..dfd36131 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -20,9 +20,11 @@ class SimpleGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , two_axis_gimbal_solver{ + *this, + get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + } { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -30,10 +32,15 @@ class SimpleGimbalController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { @@ -45,18 +52,22 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; - auto mouse = *mouse_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) + const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = + auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite(); + + if (auto_aim_requested && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); + } if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); @@ -67,7 +78,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); @@ -82,6 +93,7 @@ class SimpleGimbalController InputInterface mouse_velocity_; InputInterface mouse_; + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; TwoAxisGimbalSolver two_axis_gimbal_solver; @@ -94,4 +106,4 @@ class SimpleGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 89ac5a09..17562eb8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,7 +1,9 @@ #pragma once +#include #include #include +#include #include #include @@ -28,6 +30,13 @@ class TwoAxisGimbalSolver { component.register_input("/tf", tf_); } + void enable_yaw_limit( + rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + yaw_cw_max_ = yaw_upper_limit; + yaw_cw_min_ = yaw_lower_limit; + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + } + class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; @@ -138,6 +147,8 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; + clamp_yaw_limit(control_direction_yaw_link); + control_direction_ = fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); return calculate_control_errors(control_direction_yaw_link, pitch); @@ -194,6 +205,29 @@ class TwoAxisGimbalSolver { *control_direction << lower_limit_.x() * projection, lower_limit_.y(); } + void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { + if (!gimbal_yaw_angle_.ready()) + return; + + constexpr double two_pi = 2 * std::numbers::pi; + double cw = std::fmod(two_pi - *gimbal_yaw_angle_, two_pi); + if (cw < 0) + cw += two_pi; + + const auto& [x, y, z] = *control_direction; + const double err = std::atan2(y, x); + + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); + if (clamped_cw == target_cw) + return; + + // delta = err_new - err = (cw - clamped_cw) - err + const double delta = (cw - clamped_cw) - err; + const double c = std::cos(delta), s = std::sin(delta); + *control_direction << c * x - s * y, s * x + c * y, z; + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -213,6 +247,10 @@ class TwoAxisGimbalSolver { rmcs_executor::Component::InputInterface tf_; + double yaw_cw_min_ = 0.; + double yaw_cw_max_ = 0.; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; bool control_enabled_ = false; 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 1bc0ef8a..60348bb6 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 @@ -23,7 +23,14 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; + enum class Type : uint8_t { + kMG5010Ei10, + kMG4010Ei10, + kMG6012Ei8, + kMG4005Ei10, + kMG5010Ei36, + kMHF7015, + }; struct Config { explicit Config(Type type) @@ -110,6 +117,12 @@ class LkMotor { reduction_ratio = 36.0; max_torque_ = 25.0; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 00000000..c35bb8df --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,274 @@ +#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 "librmcs/agent/rmcs_board_lite.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::RmcsBoardLite { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , RmcsBoardLite{get_parameter("board_serial").as_string()} { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .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::kMG4010Ei10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.0)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::tuple{y, z, x}; }); + + using namespace rmcs_description; + + constexpr auto kCameraPostionX = 0.10238; + constexpr auto kCameraPostionZ = 0.05286; + tf_->set_transform( + Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + status_service_ = create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + } + + void command_update() { + auto builder = start_transmit(); + builder + .can0_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()}) + .can1_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes()}) + .can2_transmit( + {.can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) + .can3_transmit( + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); + } + +private: + void update_motors() { + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + using namespace rmcs_description; + + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + } + + void update_imu() { + using namespace rmcs_description; + + bmi088_.update_status(); + const auto gimbal_imu_pose = + Eigen::Quaterniond{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 + status_service_callback(const std::shared_ptr& response) { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_pitch_motor_.last_raw_angle()); + + response->message = feedback_message.str(); + } + +protected: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, + data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + std::shared_ptr command_component_{ + create_partner_component(get_component_name() + "_command", *this), + }; + std::shared_ptr> status_service_; + + device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; + device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; + device::DjiMotor gimbal_left_friction_{*this, *command_component_, "/gimbal/left_friction"}; + device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; + device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; + + device::Dr16 dr16_{*this}; + device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface tf_; + OutputInterface referee_serial_; + + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp new file mode 100644 index 00000000..88367c26 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -0,0 +1,96 @@ +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeOutpostOnly = 1; + static constexpr uint8_t kUiModeEngineer = 2; + + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , status_ring_(26.5, 26.5, 600, 300, false, false) + , horizontal_center_guidelines_( + {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, + {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) + , vertical_center_guidelines_( + {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , mode_indicator_( + Shape::Color::YELLOW, 30, x_center + 360, y_center + 220, x_center + 400, + y_center + 220) { + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/auto_aim/ui_mode", auto_aim_mode_, false); + + register_input("/remote/mouse", mouse_); + } + + void update() override { + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_auto_aim_enable(mouse_->right == 1); + + update_mode_indicator(); + } + +private: + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + auto update_mode_indicator() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + + switch (mode_value) { + case kUiModeOutpostOnly: mode_indicator_.set_color(Shape::Color::GREEN); break; + case kUiModeEngineer: mode_indicator_.set_color(Shape::Color::PINK); break; + case kUiModeCombat: + default: mode_indicator_.set_color(Shape::Color::YELLOW); break; + } + } + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface auto_aim_mode_; + + InputInterface mouse_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; + + Line mode_indicator_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 36c8c8b1..2b9c5da0 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include #include - -#include #include #include @@ -17,7 +16,11 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: StatusRing( - double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, + bool supercap = true, bool battery = true) + : supercap_ui_{supercap} + , battery_ui_{battery} { + supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -25,7 +28,7 @@ class StatusRing { supercap_status_.set_angle_end(275 + visible_angle); supercap_status_.set_width(width_ring); supercap_status_.set_color(Shape::Color::PINK); - supercap_status_.set_visible(true); + supercap_status_.set_visible(supercap); battery_status_.set_x(x_center); battery_status_.set_y(y_center); @@ -34,7 +37,7 @@ class StatusRing { battery_status_.set_angle_end(265); battery_status_.set_width(width_ring); battery_status_.set_color(Shape::Color::PINK); - battery_status_.set_visible(true); + battery_status_.set_visible(battery); friction_wheel_speed_.set_x(x_center); friction_wheel_speed_.set_y(y_center); @@ -112,8 +115,8 @@ class StatusRing { void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value); - battery_status_.set_visible(value); + supercap_status_.set_visible(value && supercap_ui_); + battery_status_.set_visible(value && battery_ui_); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -381,6 +384,9 @@ class StatusRing { double friction_limit_; int16_t bullet_limit_; + bool supercap_ui_ = true; + bool battery_ui_ = true; + // Dynamic part Arc supercap_status_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index b655bfff..a5467aba 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -301,7 +301,8 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { // NOLINT return; } diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 2fc9d17b..1ac5d4ee 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp index 650987f5..4a46051c 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp @@ -73,7 +73,7 @@ struct OmniLinkRight : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -101,25 +101,25 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; @@ -135,7 +135,7 @@ struct fast_tf::Joint : fast_tf::ModificationTrack }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -146,7 +146,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -157,7 +157,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -168,7 +168,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} From af885645459d4c0147eee4e5b6e880d83aaa09b6 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 18:59:20 +0800 Subject: [PATCH 77/90] Squashed commit of the following: commit 2c63c7d9218fc005f984b6d92b43404271a9d7f8 Author: creeper5820 <131014151+creeper5820@users.noreply.github.com> Date: Tue Jul 7 00:03:30 2026 +0800 feat: Integrate flight support (#85) - Add flight.yaml parameter config for Flight hardware, gimbal, friction wheels, feeder, referee interaction, auto-aim, and odin_ros_driver. - Add rmcs_core::hardware::Flight hardware plugin supporting flight controller, IMU, remote control, CAN/UART comms, TF, and referee serial. - Add rmcs_core::referee::app::ui::Flight UI plugin for ammo, friction wheel status, and auto-aim mode display. - Replace/add Flight-related hardware, controller, and UI components in plugin registry; remove some legacy chassis/controller registrations. - Add auto-aim toggle, yaw limit support, and direction clamping based on current angle to gimbal control. - Switch shooting control to /auto_aim/should_shoot input and refine auto/manual shooting logic. - Add kMHF7015 motor type support. - Add supercap/battery display toggle to StatusRing. - Minor CMake, description file, and export formatting/cleanup. Co-authored-by: heyeuu <2829004293@qq.com> Co-authored-by: zlq040222 <1542498005@qq.com> commit 1e99fd9191b7cc74125234dff105878e6b9aeb70 Author: Zihan Qin Date: Fri Jun 12 11:48:28 2026 +0800 feat: Add event interfaces and board-clocked IMU pipeline (#83) This commit lays the groundwork for upcoming IMU-camera hardware synchronization. - Add event input/output support to rmcs_executor - Add board clock, IMU snapshot, and raw camera frame message types - Integrate board-timestamped BMI088 EKF outputs in OmniInfantry - Add utility primitives used by the sync pipeline Known limitation: - OmniInfantry does not preserve the old BMI088 body-to-sensor mapping. This is intentional because that platform is retired and its original mechanical layout no longer exists. It is kept only as the remaining single-board hardware target for sync pipeline testing, so preserving its legacy IMU axis alignment is no longer necessary. commit c0ab65867b13d54858f7d0938b309ad619d3fe02 Author: Zihan Qin Date: Wed Jun 10 18:00:35 2026 +0800 feat: Fold climbable-infantry into maintained steering-infantry (#82) - Convert the active climbable-infantry line into the maintained steering-infantry variant. Remove the stair-climbing control chain and dedicated climber motors, revert the temporary VT13 / merged-remote stack back to pure DR16, and rename the cleaned result back to steering-infantry because the historical steering-infantry is no longer maintained. - Update SteeringInfantry to the rmcs-board-lite split top/bottom board path and refresh its bringup config for the current robot. Also restore the original /gimbal/calibrate and /steers/calibrate semantics, remove the temporary yaw watchdog path, make pitch control explicitly use LK velocity commands, and recover accidentally removed mainline content while keeping only the required steering-infantry merge delta. Update librmcs from v3.2.0b0 to v3.2.0 for the required board-lite SDK. Co-authored-by: Palejoker <2797572751@qq.com> Co-authored-by: Fin_Resect commit b5ae6cb717b64b300086ec6cbc434b48561d450c Author: Zihan Qin Date: Tue Jun 9 22:20:27 2026 +0800 feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> commit 8407c1d36a9a4e1de2e54cb452352ef2d16263ff Author: qzhhhi Date: Sat May 30 23:17:41 2026 +0800 style: Apply clang-format across all C++ source files --- .script/template/entrypoint | 15 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 153 ++++++++++ rmcs_ws/src/rmcs_core/CMakeLists.txt | 11 +- rmcs_ws/src/rmcs_core/plugins.xml | 15 +- .../gimbal/simple_gimbal_controller.cpp | 30 +- .../gimbal/two_axis_gimbal_solver.hpp | 38 +++ .../bullet_feeder_controller_17mm.cpp | 3 +- .../src/hardware/device/lk_motor.hpp | 15 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 274 ++++++++++++++++++ .../referee/app/ui/deformable_infantry_ui.cpp | 6 +- .../rmcs_core/src/referee/app/ui/flight.cpp | 96 ++++++ .../ui/widget/deformable_chassis_top_view.hpp | 2 +- .../src/referee/app/ui/widget/status_ring.hpp | 20 +- .../src/referee/command/interaction/ui.cpp | 2 +- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 3 +- .../rmcs_description/sentry_description.hpp | 20 +- .../rmcs_description/tf_description.hpp | 18 +- 17 files changed, 667 insertions(+), 54 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp diff --git a/.script/template/entrypoint b/.script/template/entrypoint index e8660e7a..49290c5c 100755 --- a/.script/template/entrypoint +++ b/.script/template/entrypoint @@ -1,5 +1,18 @@ #!/usr/bin/bash +USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}" +USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb" + +if [ -w "$USBFS_MEMORY_MB_PATH" ]; then + if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then + echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2 + fi +elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then + echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2 +else + echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2 +fi + # Remove all files in /tmp rm -rf /tmp/* @@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then service avahi-daemon start fi -sleep infinity \ No newline at end of file +sleep infinity diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 00000000..2291e12a --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,153 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + + - 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::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::Command -> referee_command + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs::AutoAimComponent + +odin_ros_driver: + ros__parameters: + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/velocity_imu + - /gimbal/bullet_feeder/velocity + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" + yaw_motor_zero_point: 11720 + pitch_motor_zero_point: 18578 + +referee_status: + ros__parameters: + path: /dev/tty0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° + lower_limit: 0.7 # 0.7 rad ≈ 40.1° + yaw_lower_limit: 0.1745 + yaw_upper_limit: 2.5708 + +yaw_angle_pid_controller: + ros__parameters: + 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: 8.0 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 620.0 + - 620.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 15000 + +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.2 + 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.00 + 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.00 + 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: 0.583 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d275c6db..49ce42d2 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,11 +8,12 @@ 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) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() 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( @@ -24,13 +25,13 @@ FetchContent_Declare( FetchContent_MakeAvailable(librmcs) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.c + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.c ) ament_auto_add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 560cf036..08264072 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,5 +1,5 @@ - + @@ -20,14 +20,14 @@ - + - + @@ -36,18 +36,23 @@ + + + + + + - - + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..dfd36131 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -20,9 +20,11 @@ class SimpleGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , two_axis_gimbal_solver{ + *this, + get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + } { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -30,10 +32,15 @@ class SimpleGimbalController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { @@ -45,18 +52,22 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; - auto mouse = *mouse_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) + const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = + auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite(); + + if (auto_aim_requested && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); + } if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); @@ -67,7 +78,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); @@ -82,6 +93,7 @@ class SimpleGimbalController InputInterface mouse_velocity_; InputInterface mouse_; + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; TwoAxisGimbalSolver two_axis_gimbal_solver; @@ -94,4 +106,4 @@ class SimpleGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 89ac5a09..17562eb8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,7 +1,9 @@ #pragma once +#include #include #include +#include #include #include @@ -28,6 +30,13 @@ class TwoAxisGimbalSolver { component.register_input("/tf", tf_); } + void enable_yaw_limit( + rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + yaw_cw_max_ = yaw_upper_limit; + yaw_cw_min_ = yaw_lower_limit; + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + } + class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; @@ -138,6 +147,8 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; + clamp_yaw_limit(control_direction_yaw_link); + control_direction_ = fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); return calculate_control_errors(control_direction_yaw_link, pitch); @@ -194,6 +205,29 @@ class TwoAxisGimbalSolver { *control_direction << lower_limit_.x() * projection, lower_limit_.y(); } + void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { + if (!gimbal_yaw_angle_.ready()) + return; + + constexpr double two_pi = 2 * std::numbers::pi; + double cw = std::fmod(two_pi - *gimbal_yaw_angle_, two_pi); + if (cw < 0) + cw += two_pi; + + const auto& [x, y, z] = *control_direction; + const double err = std::atan2(y, x); + + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); + if (clamped_cw == target_cw) + return; + + // delta = err_new - err = (cw - clamped_cw) - err + const double delta = (cw - clamped_cw) - err; + const double c = std::cos(delta), s = std::sin(delta); + *control_direction << c * x - s * y, s * x + c * y, z; + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -213,6 +247,10 @@ class TwoAxisGimbalSolver { rmcs_executor::Component::InputInterface tf_; + double yaw_cw_min_ = 0.; + double yaw_cw_max_ = 0.; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; bool control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 1c8f4d50..49969c0f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -100,8 +100,7 @@ class BulletFeederController17mm if (shoot_mode == ShootMode::AUTOMATIC) { auto aiming_enable = mouse_->right || (switch_right == Switch::UP); auto attack_intent = mouse_->left || (switch_left == Switch::DOWN); - auto triggered = - aiming_enable ? (*should_shoot_ && attack_intent) : attack_intent; + auto triggered = aiming_enable ? (*should_shoot_ && attack_intent) : attack_intent; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { 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 1bc0ef8a..60348bb6 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 @@ -23,7 +23,14 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; + enum class Type : uint8_t { + kMG5010Ei10, + kMG4010Ei10, + kMG6012Ei8, + kMG4005Ei10, + kMG5010Ei36, + kMHF7015, + }; struct Config { explicit Config(Type type) @@ -110,6 +117,12 @@ class LkMotor { reduction_ratio = 36.0; max_torque_ = 25.0; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 00000000..c35bb8df --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,274 @@ +#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 "librmcs/agent/rmcs_board_lite.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::RmcsBoardLite { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , RmcsBoardLite{get_parameter("board_serial").as_string()} { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .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::kMG4010Ei10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.0)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::tuple{y, z, x}; }); + + using namespace rmcs_description; + + constexpr auto kCameraPostionX = 0.10238; + constexpr auto kCameraPostionZ = 0.05286; + tf_->set_transform( + Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + status_service_ = create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + } + + void command_update() { + auto builder = start_transmit(); + builder + .can0_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()}) + .can1_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes()}) + .can2_transmit( + {.can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) + .can3_transmit( + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); + } + +private: + void update_motors() { + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + using namespace rmcs_description; + + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + } + + void update_imu() { + using namespace rmcs_description; + + bmi088_.update_status(); + const auto gimbal_imu_pose = + Eigen::Quaterniond{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 + status_service_callback(const std::shared_ptr& response) { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text(" yaw_motor_zero_point: {}", gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_pitch_motor_.last_raw_angle()); + + response->message = feedback_message.str(); + } + +protected: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, + data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + std::shared_ptr command_component_{ + create_partner_component(get_component_name() + "_command", *this), + }; + std::shared_ptr> status_service_; + + device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; + device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; + device::DjiMotor gimbal_left_friction_{*this, *command_component_, "/gimbal/left_friction"}; + device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; + device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; + + device::Dr16 dr16_{*this}; + device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface tf_; + OutputInterface referee_serial_; + + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index aea13c51..1d70f647 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -26,8 +26,10 @@ class DeformableInfantry , public rclcpp::Node { public: DeformableInfantry() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , crosshair_circle_(Shape::Color::WHITE, x_center - 26, y_center - 34, 8, 2) + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) , status_ring_(24.0, 26.5, 600, 300) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp new file mode 100644 index 00000000..88367c26 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -0,0 +1,96 @@ +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeOutpostOnly = 1; + static constexpr uint8_t kUiModeEngineer = 2; + + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , status_ring_(26.5, 26.5, 600, 300, false, false) + , horizontal_center_guidelines_( + {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, + {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) + , vertical_center_guidelines_( + {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , mode_indicator_( + Shape::Color::YELLOW, 30, x_center + 360, y_center + 220, x_center + 400, + y_center + 220) { + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/auto_aim/ui_mode", auto_aim_mode_, false); + + register_input("/remote/mouse", mouse_); + } + + void update() override { + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_auto_aim_enable(mouse_->right == 1); + + update_mode_indicator(); + } + +private: + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + auto update_mode_indicator() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + + switch (mode_value) { + case kUiModeOutpostOnly: mode_indicator_.set_color(Shape::Color::GREEN); break; + case kUiModeEngineer: mode_indicator_.set_color(Shape::Color::PINK); break; + case kUiModeCombat: + default: mode_indicator_.set_color(Shape::Color::YELLOW); break; + } + } + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface auto_aim_mode_; + + InputInterface mouse_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; + + Line mode_indicator_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp index f8c84704..80ff5e7c 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/deformable_chassis_top_view.hpp @@ -73,7 +73,7 @@ class DeformableChassisLegArcs { front_pair_offset_deg_ * degrees_to_radians_, std::numbers::pi_v - rear_pair_offset_deg_ * degrees_to_radians_, std::numbers::pi_v + rear_pair_offset_deg_ * degrees_to_radians_, - -front_pair_offset_deg_* degrees_to_radians_, + -front_pair_offset_deg_ * degrees_to_radians_, }; static constexpr std::array leg_radii_near_ = { diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 36c8c8b1..2b9c5da0 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include #include - -#include #include #include @@ -17,7 +16,11 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: StatusRing( - double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, + bool supercap = true, bool battery = true) + : supercap_ui_{supercap} + , battery_ui_{battery} { + supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -25,7 +28,7 @@ class StatusRing { supercap_status_.set_angle_end(275 + visible_angle); supercap_status_.set_width(width_ring); supercap_status_.set_color(Shape::Color::PINK); - supercap_status_.set_visible(true); + supercap_status_.set_visible(supercap); battery_status_.set_x(x_center); battery_status_.set_y(y_center); @@ -34,7 +37,7 @@ class StatusRing { battery_status_.set_angle_end(265); battery_status_.set_width(width_ring); battery_status_.set_color(Shape::Color::PINK); - battery_status_.set_visible(true); + battery_status_.set_visible(battery); friction_wheel_speed_.set_x(x_center); friction_wheel_speed_.set_y(y_center); @@ -112,8 +115,8 @@ class StatusRing { void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value); - battery_status_.set_visible(value); + supercap_status_.set_visible(value && supercap_ui_); + battery_status_.set_visible(value && battery_ui_); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -381,6 +384,9 @@ class StatusRing { double friction_limit_; int16_t bullet_limit_; + bool supercap_ui_ = true; + bool battery_ui_ = true; + // Dynamic part Arc supercap_status_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index cea7953c..57167144 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -165,4 +165,4 @@ class Ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index b655bfff..a5467aba 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -301,7 +301,8 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { // NOLINT return; } diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 2fc9d17b..1ac5d4ee 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp index 650987f5..4a46051c 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp @@ -73,7 +73,7 @@ struct OmniLinkRight : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -101,25 +101,25 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; @@ -135,7 +135,7 @@ struct fast_tf::Joint : fast_tf::ModificationTrack }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -146,7 +146,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -157,7 +157,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -168,7 +168,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} From f0e7022038fdd2e5edc6215f182889b685cf08d1 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 19:54:39 +0800 Subject: [PATCH 78/90] fix: update dr16 initialization and store_status call --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 4 ++-- rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp | 3 +-- rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp | 3 +-- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index c35bb8df..e577715f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -220,7 +220,7 @@ class Flight } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { @@ -253,7 +253,7 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_{*this}; + device::Dr16 dr16_; device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 69ddef42..a5ac2792 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -54,8 +54,7 @@ class OmniInfantry , 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} { + , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") { for (auto& motor : chassis_wheel_motors_) motor.configure( diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp index 40150e60..c4b873fa 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -526,7 +526,6 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -813,7 +812,7 @@ class SteeringHeroLittle } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback( From 9523181eb2e06b26e05db2e6bd36c0f54846d42d Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Tue, 7 Jul 2026 21:02:23 +0800 Subject: [PATCH 79/90] feat(gimbal): add pitch gravity feedforward --- .../config/deformable-infantry-omni-b.yaml | 2 ++ .../config/deformable-infantry-omni.yaml | 2 ++ .../deformable_infantry_gimbal_controller.cpp | 19 ++++++++++++++++--- 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 0b0b59e9..09d7a4ae 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -113,6 +113,8 @@ gimbal_controller: pitch_velocity_kd: 0.0 pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_phase: 0.66 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a1cabd8a..742bf86f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -111,6 +111,8 @@ gimbal_controller: pitch_velocity_kd: 0.0 pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_phase: 0.66 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 76a1511e..a0b1a33f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -35,6 +35,8 @@ class DeformableInfantryGimbalController get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); + get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); + get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); get_parameter_or( "ctrl_hold_chassis_yaw_velocity_max", ctrl_hold_chassis_yaw_velocity_max_, 30.0); @@ -93,7 +95,7 @@ class DeformableInfantryGimbalController && std::isfinite(*input_.auto_aim_pitch_acc) ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc : 0.0; - + const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); @@ -104,7 +106,8 @@ class DeformableInfantryGimbalController *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_acc_ff; + + pitch_acc_ff + + pitch_gravity_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -247,6 +250,13 @@ class DeformableInfantryGimbalController return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + auto pitch_gravity_feedforward() const -> double { + if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) + return 0.0; + else + return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + } + auto activate_ctrl_hold() -> void { ctrl_hold_active_ = true; locked_yaw_angle_ = (input_.yaw_angle.ready() && std::isfinite(*input_.yaw_angle)) @@ -298,7 +308,8 @@ class DeformableInfantryGimbalController if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu); + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_gravity_feedforward(); } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -365,6 +376,8 @@ class DeformableInfantryGimbalController double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; double ctrl_hold_chassis_yaw_velocity_max_ = 30.0; + double pitch_gravity_ff_gain_ = 0.0; + double pitch_gravity_ff_phase_ = 0.0; bool suspension_on_by_switch_ = false; bool ctrl_hold_active_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; From 9437147f91cb588b1a636dfdca7a85fef5794caa Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Tue, 7 Jul 2026 22:38:26 +0800 Subject: [PATCH 80/90] style: format code for consistency and readability --- .../config/deformable-infantry-omni-b.yaml | 1 - .../config/deformable-infantry-omni.yaml | 1 - .../controller/chassis/deformable_chassis.cpp | 256 ++++++---------- .../deformable_chassis_joint_manager.hpp | 126 ++++---- .../deformable_chassis_mode_manager.hpp | 246 +++++---------- .../chassis/deformable_chassis_suspension.hpp | 283 ++++++++---------- .../deformable_omni_wheel_controller.cpp | 189 +++++------- .../deformable_infantry_gimbal_controller.cpp | 88 +++--- .../hardware/deformable-infantry-omni-b.cpp | 196 ++++++------ .../src/hardware/deformable-infantry-omni.cpp | 160 +++++----- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 8 +- .../rmcs_core/src/hardware/omni_infantry.cpp | 25 +- .../src/hardware/steering-hero-little.cpp | 39 ++- .../referee/app/ui/deformable_infantry_ui.cpp | 46 +-- 14 files changed, 683 insertions(+), 981 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 402a2297..76768d83 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -111,7 +111,6 @@ gimbal_controller: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 03a46e23..8f02e7e8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -109,7 +109,6 @@ gimbal_controller: upper_limit: -0.65 # -35 deg lower_limit: 0.05 # 6 deg ctrl_hold_pitch_target_angle: 0.0 - ctrl_hold_chassis_yaw_velocity_max: 3.0 yaw_angle_kp: 30.0 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index e12e684d..395fd4d8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -31,22 +31,12 @@ class DeformableChassis : public rmcs_executor::Component , public rclcpp::Node { public: - enum JointIndex : size_t { - kLeftFront = 0, - kLeftBack = 1, - kRightBack = 2, - kRightFront = 3, - kJointCount = 4, - }; - DeformableChassis() : Node( get_component_name(), - rclcpp::NodeOptions{} - .automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) - , spin_ratio_( - std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) + , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) , mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; @@ -63,67 +53,39 @@ class DeformableChassis register_input("/predefined/update_rate", update_rate_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); - register_input( - "/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, - false); - register_input( - "/chassis/manual_yaw_velocity_override", - manual_yaw_velocity_override_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input( - "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input( - "/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); for (size_t i = 0; i < kJointCount; ++i) { register_input( - fmt::format( - "/chassis/{}_joint/physical_angle", kJointName[i]), + fmt::format("/chassis/{}_joint/physical_angle", kJointName[i]), joint_physical_angle_[i], false); - register_output( - fmt::format( - "/chassis/{}_joint/target_physical_angle", - kJointName[i]), + fmt::format("/chassis/{}_joint/target_physical_angle", kJointName[i]), joint_target_angle_[i], nan_); - register_output( - fmt::format( - "/chassis/{}_joint/target_physical_velocity", - kJointName[i]), + fmt::format("/chassis/{}_joint/target_physical_velocity", kJointName[i]), joint_target_velocity_[i], nan_); - register_output( - fmt::format( - "/chassis/{}_joint/target_physical_acceleration", - kJointName[i]), + fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointName[i]), joint_target_acceleration_[i], nan_); - register_output( - fmt::format( - "/chassis/{}_joint/control_angle_error", - kJointName[i]), + fmt::format("/chassis/{}_joint/control_angle_error", kJointName[i]), joint_angle_error_[i], nan_); } - register_output( - "/gimbal/scope/control_torque", scope_motor_control_torque, - nan_); + register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); register_output("/chassis/angle", chassis_angle_, nan_); - register_output( - "/chassis/control_angle", chassis_control_angle_, nan_); - + register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); - register_output( - "/chassis/control_velocity", chassis_control_velocity_); - register_output( - "/chassis/ctrl_hold_active", ctrl_hold_active_, false); - register_output( - "/chassis/active_suspension/active", - active_suspension_active_, false); + register_output("/chassis/control_velocity", chassis_control_velocity_); + register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); + register_output("/chassis/active_suspension/active", active_suspension_active_, false); *mode_ = rmcs_msgs::ChassisMode::AUTO; *ctrl_hold_active_ = false; @@ -133,19 +95,14 @@ class DeformableChassis void before_updating() override { if (!gimbal_yaw_angle_.ready()) { gimbal_yaw_angle_.make_and_bind_directly(0.0); - RCLCPP_WARN( - get_logger(), - "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); RCLCPP_WARN( - get_logger(), - "Failed to fetch \"/gimbal/yaw/control_angle_error\". " - "Set to 0.0."); + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " + "Set to 0.0."); } - if (!manual_yaw_velocity_override_.ready()) - manual_yaw_velocity_override_.make_and_bind_directly(nan_); if (!chassis_imu_pitch_.ready()) chassis_imu_pitch_.make_and_bind_directly(0.0); if (!chassis_imu_roll_.ready()) @@ -165,25 +122,20 @@ class DeformableChassis const auto keyboard = *keyboard_; do { - if ((switch_left == Switch::UNKNOWN - || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN - && switch_right == Switch::DOWN)) { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); break; } - double rotary_knob = - rotary_knob_.ready() ? *rotary_knob_ : 0.0; + double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - mode_mgr_.update( - switch_left, switch_right, keyboard, - rotary_knob, update_dt()); + mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); - const auto& mm = mode_mgr_.result(); - *mode_ = mm.mode; - *ctrl_hold_active_ = mm.ctrl_hold_active; - *active_suspension_active_ = mm.suspension_active; + const auto& mode_result = mode_mgr_.result(); + *mode_ = mode_result.mode; + *ctrl_hold_active_ = mode_result.ctrl_hold_active; + *active_suspension_active_ = mode_result.suspension_active; update_velocity_control(); run_joint_intent_pipeline_(); @@ -191,12 +143,8 @@ class DeformableChassis } private: - static constexpr const char* kJointName[] = { - "left_front", "left_back", "right_back", "right_front", - }; - - static constexpr double nan_ = - std::numeric_limits::quiet_NaN(); + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; static constexpr double angular_velocity_max_ = 30.0; static constexpr double default_dt_ = 1e-3; @@ -228,28 +176,23 @@ class DeformableChassis } double update_dt() const { - if (update_rate_.ready() && std::isfinite(*update_rate_) - && *update_rate_ > 1e-6) + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) return 1.0 / *update_rate_; return default_dt_; } void update_velocity_control() { - Eigen::Vector2d translational_velocity = - update_translational_velocity_control(); - double angular_velocity = update_angular_velocity_control(); - chassis_control_velocity_->vector << translational_velocity, - angular_velocity; + Eigen::Vector2d translational_velocity = update_translational_velocity_control(); + double angular_velocity = update_angular_velocity_control(); + chassis_control_velocity_->vector << translational_velocity, angular_velocity; } Eigen::Vector2d update_translational_velocity_control() { const auto keyboard = *keyboard_; - Eigen::Vector2d keyboard_move{ - keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; Eigen::Vector2d translational_velocity = - Eigen::Rotation2Dd{*gimbal_yaw_angle_} - * (*joystick_right_ + keyboard_move); + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); @@ -268,81 +211,73 @@ class DeformableChassis case rmcs_msgs::ChassisMode::SPIN: { bool forward = mode_mgr_.result().spinning_forward; angular_velocity = - spin_ratio_ - * (forward ? angular_velocity_max_ - : -angular_velocity_max_); - angular_velocity = std::clamp( - angular_velocity, -angular_velocity_max_, - angular_velocity_max_); + spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); + angular_velocity = + std::clamp(angular_velocity, -angular_velocity_max_, angular_velocity_max_); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { - double err = calculate_unsigned_chassis_angle_error( - chassis_control_angle); + double chassis_angle_error = + calculate_unsigned_chassis_angle_error(chassis_control_angle); constexpr double alignment = std::numbers::pi; - while (err > alignment / 2) { + while (chassis_angle_error > alignment / 2) { chassis_control_angle -= alignment; if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; - err -= alignment; + chassis_angle_error -= alignment; } - angular_velocity = - following_velocity_controller_.update(err); + angular_velocity = following_velocity_controller_.update(chassis_angle_error); } break; default: break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; } - double calculate_unsigned_chassis_angle_error( - double& chassis_control_angle) { + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { chassis_control_angle = *gimbal_yaw_angle_error_; if (chassis_control_angle < 0) chassis_control_angle += 2 * std::numbers::pi; - double err = chassis_control_angle + *gimbal_yaw_angle_; - if (err >= 2 * std::numbers::pi) - err -= 2 * std::numbers::pi; + double unsigned_angle_error = chassis_control_angle + *gimbal_yaw_angle_; + if (unsigned_angle_error >= 2 * std::numbers::pi) + unsigned_angle_error -= 2 * std::numbers::pi; - return err; + return unsigned_angle_error; } - static double deg_to_rad(double deg) { - return deg * std::numbers::pi / 180.0; - } + static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } std::array read_feedback_() const { std::array angles; angles.fill(nan_); for (size_t i = 0; i < kJointCount; ++i) - if (joint_physical_angle_[i].ready() - && std::isfinite(*joint_physical_angle_[i])) + if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) angles[i] = *joint_physical_angle_[i]; return angles; } std::array compute_target_angles_rad_() const { - const auto& mm = mode_mgr_.result(); - std::array targets_deg = mm.joint_target_deg; + const auto& mode_result = mode_mgr_.result(); + std::array targets_deg = mode_result.joint_target_deg; std::array targets_rad; for (size_t i = 0; i < kJointCount; ++i) targets_rad[i] = deg_to_rad(targets_deg[i]); - if (mm.suspension_active) { - double base = mm.low_prone - ? mode_mgr_.active_suspension_min_angle_rad() - : deg_to_rad(mm.base_angle_deg); - targets_rad.fill(base); + if (mode_result.suspension_active) { + double base_target_angle_rad = mode_result.low_prone + ? mode_mgr_.active_suspension_min_angle_rad() + : deg_to_rad(mode_result.base_angle_deg); + targets_rad.fill(base_target_angle_rad); } return targets_rad; @@ -350,45 +285,38 @@ class DeformableChassis void run_joint_intent_pipeline_() { const auto current_physical_angles = read_feedback_(); - const auto& mm = mode_mgr_.result(); + const auto& mode_result = mode_mgr_.result(); if (!joint_mgr_.init_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); return; } - std::array target_angles_rad = - compute_target_angles_rad_(); + std::array target_angles_rad = compute_target_angles_rad_(); suspension_.calibrate( - *chassis_imu_pitch_, *chassis_imu_roll_, - mm.symmetric_target, update_dt()); + *chassis_imu_pitch_, *chassis_imu_roll_, mode_result.symmetric_target, update_dt()); *scope_motor_control_torque = suspension_.scope_torque( - mm.suspension_active, - mm.mode == rmcs_msgs::ChassisMode::SPIN); + mode_result.suspension_active, mode_result.mode == rmcs_msgs::ChassisMode::SPIN); auto corrections = suspension_.update( *chassis_imu_pitch_ - suspension_.pitch_offset(), - *chassis_imu_roll_ - suspension_.roll_offset(), - *chassis_imu_pitch_rate_, *chassis_imu_roll_rate_, - mm.suspension_active, mm.low_prone, - mode_mgr_.min_angle(), mode_mgr_.max_angle(), - mm.base_angle_deg, mode_mgr_.correction_inverted(), - joint_mgr_.angle_states(), update_dt()); + *chassis_imu_roll_ - suspension_.roll_offset(), *chassis_imu_pitch_rate_, + *chassis_imu_roll_rate_, mode_result.suspension_active, mode_result.low_prone, + mode_mgr_.min_angle(), mode_mgr_.max_angle(), mode_result.base_angle_deg, + mode_mgr_.correction_inverted(), joint_mgr_.angle_states(), update_dt()); - joint_mgr_.run_trajectory( - target_angles_rad, mm.suspension_active, update_dt()); + joint_mgr_.run_trajectory(target_angles_rad, mode_result.suspension_active, update_dt()); publish_joint_targets_(current_physical_angles, corrections); } void publish_joint_targets_( const std::array& feedback_angles, - const DeformableChassisActiveSuspension::Corrections& corr) { + const DeformableChassisActiveSuspension::Corrections& corrections) { - double min_angle_rad = - mode_mgr_.active_suspension_min_angle_rad(); + double min_angle_rad = mode_mgr_.active_suspension_min_angle_rad(); double max_angle_rad = mode_mgr_.max_angle_rad(); if (!joint_mgr_.any_active()) { @@ -405,19 +333,15 @@ class DeformableChassis continue; } - double target = - joint_mgr_.angle_state(i) + corr.angle[i]; - *joint_target_angle_[i] = std::clamp( - target, min_angle_rad, max_angle_rad); + double target = joint_mgr_.angle_state(i) + corrections.joint_angle_correction[i]; + *joint_target_angle_[i] = std::clamp(target, min_angle_rad, max_angle_rad); *joint_target_velocity_[i] = - joint_mgr_.velocity_state(i) + corr.velocity[i]; + joint_mgr_.velocity_state(i) + corrections.joint_velocity_correction[i]; *joint_target_acceleration_[i] = - joint_mgr_.acceleration_state(i) - + corr.acceleration[i]; - *joint_angle_error_[i] = - std::isfinite(feedback_angles[i]) - ? feedback_angles[i] - *joint_target_angle_[i] - : nan_; + joint_mgr_.acceleration_state(i) + corrections.joint_acceleration_correction[i]; + *joint_angle_error_[i] = std::isfinite(feedback_angles[i]) + ? feedback_angles[i] - *joint_target_angle_[i] + : nan_; } } @@ -433,6 +357,13 @@ class DeformableChassis } } + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + InputInterface joystick_right_; InputInterface switch_right_; InputInterface switch_left_; @@ -441,12 +372,10 @@ class DeformableChassis InputInterface update_rate_; InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; - InputInterface manual_yaw_velocity_override_; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; - OutputInterface - chassis_control_velocity_; + OutputInterface chassis_control_velocity_; OutputInterface ctrl_hold_active_; OutputInterface active_suspension_active_; @@ -457,16 +386,11 @@ class DeformableChassis OutputInterface scope_motor_control_torque; - std::array, kJointCount> - joint_physical_angle_; - std::array, kJointCount> - joint_target_angle_; - std::array, kJointCount> - joint_target_velocity_; - std::array, kJointCount> - joint_target_acceleration_; - std::array, kJointCount> - joint_angle_error_; + std::array, kJointCount> joint_physical_angle_; + std::array, kJointCount> joint_target_angle_; + std::array, kJointCount> joint_target_velocity_; + std::array, kJointCount> joint_target_acceleration_; + std::array, kJointCount> joint_angle_error_; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; @@ -480,6 +404,4 @@ class DeformableChassis #include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableChassis, - rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::DeformableChassis, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp index 75c8b077..953fa60d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include @@ -13,28 +12,28 @@ namespace rmcs_core::controller::chassis { class DeformableChassisJointManager { public: static constexpr size_t kJointCount = 4; - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); DeformableChassisJointManager() = default; void configure(rclcpp::Node& node) { target_vel_limit_ = std::max( - deg_to_rad_(std::abs( - node.get_parameter_or("target_physical_velocity_limit", 180.0))), + deg_to_rad_(std::abs(node.get_parameter_or("target_physical_velocity_limit", 180.0))), 1e-6); target_acc_limit_ = std::max( - deg_to_rad_(std::abs( - node.get_parameter_or("target_physical_acceleration_limit", 720.0))), + deg_to_rad_( + std::abs(node.get_parameter_or("target_physical_acceleration_limit", 720.0))), 1e-6); suspension_vel_limit_ = std::max( - deg_to_rad_(std::abs(node.get_parameter_or( - "active_suspension_target_velocity_limit_deg", - node.get_parameter_or("target_physical_velocity_limit", 180.0)))), + deg_to_rad_( + std::abs(node.get_parameter_or( + "active_suspension_target_velocity_limit_deg", + node.get_parameter_or("target_physical_velocity_limit", 180.0)))), 1e-6); suspension_acc_limit_ = std::max( - deg_to_rad_(std::abs(node.get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - node.get_parameter_or("target_physical_acceleration_limit", 720.0)))), + deg_to_rad_( + std::abs(node.get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + node.get_parameter_or("target_physical_acceleration_limit", 720.0)))), 1e-6); } @@ -45,8 +44,7 @@ class DeformableChassisJointManager { target_acceleration_state_rad_.fill(0.0); } - bool init_from_feedback( - const std::array& physical_angles) { + bool init_from_feedback(const std::array& physical_angles) { bool any_active = false; for (size_t i = 0; i < kJointCount; ++i) { @@ -54,7 +52,7 @@ class DeformableChassisJointManager { target_angle_state_rad_[i] = physical_angles[i]; target_velocity_state_rad_[i] = 0.0; target_acceleration_state_rad_[i] = 0.0; - joint_target_active_[i] = true; + joint_target_active_[i] = true; } any_active = any_active || joint_target_active_[i]; } @@ -69,93 +67,71 @@ class DeformableChassisJointManager { } void run_trajectory( - const std::array& target_angles_rad, - bool suspension_active, + const std::array& target_angles_rad, bool suspension_active, double dt) { for (size_t i = 0; i < kJointCount; ++i) { if (!joint_target_active_[i]) continue; - double& angle_st = target_angle_state_rad_[i]; - double& vel_st = target_velocity_state_rad_[i]; - double& acc_st = target_acceleration_state_rad_[i]; - double target = target_angles_rad[i]; + double& angle_state = target_angle_state_rad_[i]; + double& velocity_state = target_velocity_state_rad_[i]; + double& acceleration_state = target_acceleration_state_rad_[i]; + double target = target_angles_rad[i]; - double vel_limit = suspension_active - ? suspension_vel_limit_ - : target_vel_limit_; - double acc_limit = suspension_active - ? suspension_acc_limit_ - : target_acc_limit_; + double vel_limit = suspension_active ? suspension_vel_limit_ : target_vel_limit_; + double acc_limit = suspension_active ? suspension_acc_limit_ : target_acc_limit_; - if (!std::isfinite(target) || !std::isfinite(angle_st)) + if (!std::isfinite(target) || !std::isfinite(angle_state)) continue; - double position_error = target - angle_st; - double stopping_distance = - vel_st * vel_st / (2.0 * acc_limit); + double position_error = target - angle_state; + double stopping_distance = velocity_state * velocity_state / (2.0 * acc_limit); double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 - && std::abs(position_error) > stopping_distance) { - desired_velocity = - std::copysign(vel_limit, position_error); + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { + desired_velocity = std::copysign(vel_limit, position_error); } - double velocity_error = desired_velocity - vel_st; - acc_st = std::clamp( - velocity_error / dt, -acc_limit, acc_limit); + double velocity_error = desired_velocity - velocity_state; + acceleration_state = std::clamp(velocity_error / dt, -acc_limit, acc_limit); - vel_st += acc_st * dt; - vel_st = std::clamp(vel_st, -vel_limit, vel_limit); - angle_st += vel_st * dt; + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -vel_limit, vel_limit); + angle_state += velocity_state * dt; - double next_error = target - angle_st; + double next_error = target - angle_state; if ((position_error > 0.0 && next_error < 0.0) || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 - && std::abs(vel_st) < 1e-3)) { - angle_st = target; - vel_st = 0.0; - acc_st = 0.0; + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; } } } - double angle_state(size_t i) const { - return target_angle_state_rad_[i]; - } - double velocity_state(size_t i) const { - return target_velocity_state_rad_[i]; - } - double acceleration_state(size_t i) const { - return target_acceleration_state_rad_[i]; - } + double angle_state(size_t i) const { return target_angle_state_rad_[i]; } + double velocity_state(size_t i) const { return target_velocity_state_rad_[i]; } + double acceleration_state(size_t i) const { return target_acceleration_state_rad_[i]; } bool joint_active(size_t i) const { return joint_target_active_[i]; } - std::array angle_states() const { - return target_angle_state_rad_; - } + std::array angle_states() const { return target_angle_state_rad_; } private: - static double deg_to_rad_(double deg) { - return deg * std::numbers::pi / 180.0; - } + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + std::array joint_target_active_ = {false, false, false, false}; + std::array target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array target_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array target_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array joint_target_active_ = { - false, false, false, false}; - std::array target_angle_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array target_velocity_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array target_acceleration_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - - double target_vel_limit_ = 0.0; - double target_acc_limit_ = 0.0; - double suspension_vel_limit_ = 0.0; - double suspension_acc_limit_ = 0.0; + double target_vel_limit_ = 0.0; + double target_acc_limit_ = 0.0; + double suspension_vel_limit_ = 0.0; + double suspension_acc_limit_ = 0.0; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp index 3df2f7d6..82d3a07a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp @@ -14,27 +14,25 @@ namespace rmcs_core::controller::chassis { class DeformableChassisModeManager { public: - static constexpr size_t kJointCount = 4; - struct Result { - rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; - bool ctrl_hold_active = false; - bool suspension_active = false; - bool low_prone = false; - bool symmetric_target = true; - bool spinning_forward = true; - std::array joint_target_deg = {58.0, 58.0, 58.0, 58.0}; - double base_angle_deg = 58.0; + rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; + bool ctrl_hold_active = false; + bool suspension_active = false; + bool low_prone = false; + bool symmetric_target = true; + bool spinning_forward = true; + std::array joint_target_deg = {58.0, 58.0, 58.0, 58.0}; + double base_angle_deg = 58.0; }; explicit DeformableChassisModeManager(rclcpp::Node& node) : min_angle_(node.get_parameter_or("min_angle", 7.0)) , max_angle_(node.get_parameter_or("max_angle", 58.0)) - , base_angle_deg_(std::clamp( - node.get_parameter_or("active_suspension_base_angle", max_angle_), - min_angle_ - 5.0, max_angle_)) - , suspension_enable_( - node.get_parameter_or("active_suspension_enable", false)) { + , base_angle_deg_( + std::clamp( + node.get_parameter_or("active_suspension_base_angle", max_angle_), + min_angle_ - 5.0, max_angle_)) + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { result_.base_angle_deg = base_angle_deg_; result_.joint_target_deg.fill(max_angle_); current_target_angle_ = max_angle_; @@ -49,19 +47,15 @@ class DeformableChassisModeManager { result_.symmetric_target = true; result_.spinning_forward = true; result_.joint_target_deg.fill(max_angle_); - result_.base_angle_deg = max_angle_; + result_.base_angle_deg = max_angle_; - current_target_angle_ = max_angle_; - base_angle_deg_ = max_angle_; + current_target_angle_ = max_angle_; + base_angle_deg_ = max_angle_; joint_current_target_angle_.fill(max_angle_); - apply_symmetric_target_ = true; - complex_spin_active_ = false; - complex_spin_elapsed_ = 0.0; - qe_complex_spin_active_ = false; - qe_front_high_rear_low_ = true; - qe_last_toggle_elapsed_ = 0.0; - suspension_on_by_keyboard_ = false; - suspension_on_by_remote_ = false; + apply_symmetric_target_ = true; + complex_spin_active_ = false; + complex_spin_elapsed_ = 0.0; + suspension_on_by_dr16_switch_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -70,27 +64,22 @@ class DeformableChassisModeManager { } void update( - rmcs_msgs::Switch switch_left, - rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard, - double rotary_knob, - double dt) { + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { update_mode_from_inputs_(switch_left, switch_right, keyboard); update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); - result_.low_prone = keyboard.ctrl; - result_.ctrl_hold_active = - keyboard.ctrl || suspension_on_by_remote_ || suspension_on_by_keyboard_; - result_.suspension_active = - suspension_enable_ && result_.ctrl_hold_active; + result_.low_prone = keyboard.ctrl; + result_.ctrl_hold_active = keyboard.ctrl || suspension_on_by_dr16_switch_; + result_.suspension_active = suspension_enable_ && result_.ctrl_hold_active; result_.symmetric_target = symmetric_joint_target_requested_(); update_lift_target_toggle_(keyboard, rotary_knob, dt); result_.joint_target_deg = joint_current_target_angle_; - result_.base_angle_deg = base_angle_deg_; + result_.base_angle_deg = base_angle_deg_; last_switch_right_ = switch_right; last_switch_left_ = switch_left; @@ -103,9 +92,7 @@ class DeformableChassisModeManager { double max_angle() const { return max_angle_; } double max_angle_rad() const { return deg_to_rad_(max_angle_); } - double active_suspension_min_angle_rad() const { - return deg_to_rad_(min_angle_ - 5.0); - } + double active_suspension_min_angle_rad() const { return deg_to_rad_(min_angle_ - 5.0); } bool correction_inverted() const { double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; @@ -113,72 +100,43 @@ class DeformableChassisModeManager { } private: - static double deg_to_rad_(double deg) { - return deg * std::numbers::pi / 180.0; - } + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } void update_mode_from_inputs_( - rmcs_msgs::Switch switch_left, - rmcs_msgs::Switch switch_right, + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { - auto mode = result_.mode; - bool q_pressed = keyboard.q; - bool e_pressed = keyboard.e; - bool last_q_pressed = last_keyboard_.q; - bool last_e_pressed = last_keyboard_.e; - bool last_c_pressed = last_keyboard_.c; - bool qe_combo_pressed = q_pressed && e_pressed; - bool last_qe_combo_pressed = last_q_pressed && last_e_pressed; - bool c_rising = !last_c_pressed && keyboard.c; - bool qe_combo_rising = !last_qe_combo_pressed && qe_combo_pressed; - + auto mode = result_.mode; if (switch_left == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); result_.mode = mode; return; } - if (qe_complex_spin_active_) { - if (c_rising) { - deactivate_qe_complex_spin_(); - apply_symmetric_target_ = true; - joint_current_target_angle_.fill(current_target_angle_); - mode = rmcs_msgs::ChassisMode::SPIN; - } - } else if (qe_combo_rising) { - deactivate_complex_spin_(); - activate_qe_complex_spin_(mode); - } else if ( - last_switch_right_ == rmcs_msgs::Switch::MIDDLE + if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; result_.spinning_forward = !result_.spinning_forward; } } else if (!last_keyboard_.c && keyboard.c) { deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; result_.spinning_forward = !result_.spinning_forward; } } else if (!last_keyboard_.z && keyboard.z) { deactivate_complex_spin_(); - deactivate_qe_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; + mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; } - if (complex_spin_active_ || qe_complex_spin_active_) + if (complex_spin_active_) mode = rmcs_msgs::ChassisMode::SPIN; result_.mode = mode; @@ -189,7 +147,7 @@ class DeformableChassisModeManager { complex_spin_elapsed_ = 0.0; apply_symmetric_target_ = true; if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; result_.spinning_forward = !result_.spinning_forward; } } @@ -199,83 +157,38 @@ class DeformableChassisModeManager { complex_spin_elapsed_ = 0.0; } - void activate_qe_complex_spin_(rmcs_msgs::ChassisMode& mode) { - qe_complex_spin_active_ = true; - qe_last_toggle_elapsed_ = 0.0; - qe_front_high_rear_low_ = true; - apply_front_high_rear_low_target_(); - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - result_.spinning_forward = !result_.spinning_forward; - } - } - - void deactivate_qe_complex_spin_() { - qe_complex_spin_active_ = false; - qe_last_toggle_elapsed_ = 0.0; - } - void apply_front_high_rear_low_target_() { - joint_current_target_angle_[0] = max_angle_; - joint_current_target_angle_[3] = max_angle_; - joint_current_target_angle_[1] = min_angle_; - joint_current_target_angle_[2] = min_angle_; - apply_symmetric_target_ = false; - qe_front_high_rear_low_ = true; + joint_current_target_angle_[kLeftFront] = max_angle_; + joint_current_target_angle_[kRightFront] = max_angle_; + joint_current_target_angle_[kLeftBack] = min_angle_; + joint_current_target_angle_[kRightBack] = min_angle_; + apply_symmetric_target_ = false; } void apply_front_low_rear_high_target_() { - joint_current_target_angle_[0] = min_angle_; - joint_current_target_angle_[3] = min_angle_; - joint_current_target_angle_[1] = max_angle_; - joint_current_target_angle_[2] = max_angle_; - apply_symmetric_target_ = false; - qe_front_high_rear_low_ = false; + joint_current_target_angle_[kLeftFront] = min_angle_; + joint_current_target_angle_[kRightFront] = min_angle_; + joint_current_target_angle_[kLeftBack] = max_angle_; + joint_current_target_angle_[kRightBack] = max_angle_; + apply_symmetric_target_ = false; } void toggle_bg_target_() { - if (qe_front_high_rear_low_) + if (joint_current_target_angle_[kLeftFront] > joint_current_target_angle_[kLeftBack]) apply_front_low_rear_high_target_(); else apply_front_high_rear_low_target_(); } - void toggle_qe_complex_spin_target_() { - toggle_bg_target_(); - } - - void update_qe_complex_spin_toggle_(double dt) { - constexpr double qe_complex_spin_toggle_period = 1.0; - qe_last_toggle_elapsed_ += dt; - size_t toggle_count = 0; - while (qe_last_toggle_elapsed_ >= qe_complex_spin_toggle_period) { - qe_last_toggle_elapsed_ -= qe_complex_spin_toggle_period; - ++toggle_count; - } - if ((toggle_count % 2) == 1) - toggle_qe_complex_spin_target_(); - } - void update_suspension_toggle_from_inputs_( - rmcs_msgs::Switch switch_left, - rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard) { + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard&) { if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) - suspension_on_by_remote_ = !suspension_on_by_remote_; - if (suspension_toggle_requested_by_keyboard_(keyboard)) - suspension_on_by_keyboard_ = !suspension_on_by_keyboard_; - } - - bool suspension_toggle_requested_by_keyboard_( - const rmcs_msgs::Keyboard& keyboard) const { - return !last_keyboard_.e && keyboard.e && !keyboard.q; + suspension_on_by_dr16_switch_ = !suspension_on_by_dr16_switch_; } bool suspension_toggle_requested_by_switch_( - rmcs_msgs::Switch switch_left, - rmcs_msgs::Switch switch_right) const { - return switch_left == rmcs_msgs::Switch::DOWN - && switch_right == rmcs_msgs::Switch::UP + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { + return switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; } @@ -283,37 +196,28 @@ class DeformableChassisModeManager { constexpr double epsilon = 1e-6; return std::all_of( joint_current_target_angle_.begin() + 1, joint_current_target_angle_.end(), - [&](double v) { - return std::abs(v - joint_current_target_angle_.front()) <= epsilon; - }); + [&](double v) { return std::abs(v - joint_current_target_angle_.front()) <= epsilon; }); } void update_lift_target_toggle_( - const rmcs_msgs::Keyboard& keyboard, - double rotary_knob, - double dt) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { constexpr double rotary_knob_symmetric_edge_threshold = 0.7; constexpr double rotary_knob_bg_edge_threshold = -0.9; constexpr double complex_spin_toggle_period = 0.5; - const bool keyboard_toggle_condition = - !qe_complex_spin_active_ && !last_keyboard_.q && keyboard.q - && !keyboard.e; + const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; const bool rotary_knob_toggle_condition = last_rotary_knob_ < rotary_knob_symmetric_edge_threshold && rotary_knob >= rotary_knob_symmetric_edge_threshold; const bool rotary_knob_bg_toggle_condition = - !qe_complex_spin_active_ - && last_rotary_knob_ > rotary_knob_bg_edge_threshold + last_rotary_knob_ > rotary_knob_bg_edge_threshold && rotary_knob <= rotary_knob_bg_edge_threshold; - const bool front_high_rear_low = - !qe_complex_spin_active_ && !last_keyboard_.b && keyboard.b; - const bool front_low_rear_high = - !qe_complex_spin_active_ && !last_keyboard_.g && keyboard.g; + const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; + const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; bool complex_spin_toggle_condition = false; if (complex_spin_active_) { @@ -323,13 +227,9 @@ class DeformableChassisModeManager { complex_spin_elapsed_ -= complex_spin_toggle_period; ++complex_spin_toggle_count; } - complex_spin_toggle_condition = - (complex_spin_toggle_count % 2) == 1; + complex_spin_toggle_condition = (complex_spin_toggle_count % 2) == 1; } - if (qe_complex_spin_active_) - update_qe_complex_spin_toggle_(dt); - if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); @@ -337,14 +237,10 @@ class DeformableChassisModeManager { || complex_spin_toggle_condition) { if (result_.suspension_active && keyboard_toggle_condition) { base_angle_deg_ = - (std::abs(base_angle_deg_ - max_angle_) < 1e-6) - ? min_angle_ - : max_angle_; + (std::abs(base_angle_deg_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; } else { current_target_angle_ = - (std::abs(current_target_angle_ - max_angle_) < 1e-6) - ? min_angle_ - : max_angle_; + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; apply_symmetric_target_ = true; } } else if (rotary_knob_bg_toggle_condition) { @@ -358,6 +254,12 @@ class DeformableChassisModeManager { last_rotary_knob_ = rotary_knob; } + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr size_t kJointCount = 4; + Result result_; double min_angle_; @@ -367,19 +269,15 @@ class DeformableChassisModeManager { double current_target_angle_; std::array joint_current_target_angle_; - bool apply_symmetric_target_ = true; - bool complex_spin_active_ = false; - double complex_spin_elapsed_ = 0.0; - bool qe_complex_spin_active_ = false; - bool qe_front_high_rear_low_ = true; - double qe_last_toggle_elapsed_ = 0.0; - bool suspension_on_by_keyboard_ = false; - bool suspension_on_by_remote_ = false; + bool apply_symmetric_target_ = true; + bool complex_spin_active_ = false; + double complex_spin_elapsed_ = 0.0; + bool suspension_on_by_dr16_switch_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - double last_rotary_knob_ = 0.0; + double last_rotary_knob_ = 0.0; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp index b72139a3..c14fd014 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp @@ -3,8 +3,6 @@ #include #include #include -#include -#include #include #include @@ -15,14 +13,11 @@ namespace rmcs_core::controller::chassis { class DeformableChassisActiveSuspension { public: - static constexpr size_t kJointCount = 4; - static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; - struct Corrections { - std::array angle = {0.0, 0.0, 0.0, 0.0}; - std::array velocity = {0.0, 0.0, 0.0, 0.0}; - std::array acceleration = {0.0, 0.0, 0.0, 0.0}; - std::array active = {false, false, false, false}; + std::array joint_angle_correction = {0.0, 0.0, 0.0, 0.0}; + std::array joint_velocity_correction = {0.0, 0.0, 0.0, 0.0}; + std::array joint_acceleration_correction = {0.0, 0.0, 0.0, 0.0}; + std::array joint_correction_active = {false, false, false, false}; }; void configure(rclcpp::Node& node) { @@ -33,30 +28,30 @@ class DeformableChassisActiveSuspension { node, "active_suspension_pitch_inner_", pitch_inner_pid_, // 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); load_pid_( - node, "active_suspension_roll_outer_", roll_outer_pid_, // + node, "active_suspension_roll_outer_", roll_outer_pid_, // 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, 3.0); load_pid_( - node, "active_suspension_roll_inner_", roll_inner_pid_, // + node, "active_suspension_roll_inner_", roll_inner_pid_, // 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); correction_vel_limit_ = std::max( - deg_to_rad_(std::abs(node.get_parameter_or( - "active_suspension_correction_velocity_limit_deg", 720.0))), + deg_to_rad_( + std::abs(node.get_parameter_or( + "active_suspension_correction_velocity_limit_deg", 720.0))), 1e-6); correction_acc_limit_ = std::max( - deg_to_rad_(std::abs(node.get_parameter_or( - "active_suspension_correction_acceleration_limit_deg", 3600.0))), + deg_to_rad_( + std::abs(node.get_parameter_or( + "active_suspension_correction_acceleration_limit_deg", 3600.0))), 1e-6); - calibration_wait_time_ = std::max( - node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); - calibration_sample_time_ = std::max( - node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + calibration_wait_time_ = + std::max(node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_sample_time_ = + std::max(node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); } - void calibrate( - double pitch, double roll, - bool symmetric_target, double dt) { + void calibrate(double pitch, double roll, bool symmetric_target, double dt) { if (calibrated_once_) return; @@ -73,8 +68,7 @@ class DeformableChassisActiveSuspension { if (calibration_hold_elapsed_ < calibration_wait_time_) return; - double calibration_end = - calibration_wait_time_ + calibration_sample_time_; + double calibration_end = calibration_wait_time_ + calibration_sample_time_; if (calibration_hold_elapsed_ < calibration_end) { pitch_sum_ += pitch; roll_sum_ += roll; @@ -90,11 +84,9 @@ class DeformableChassisActiveSuspension { return; pitch_offset_ = std::clamp( - pitch_sum_ / static_cast(sample_count_), - -offset_limit_rad_, offset_limit_rad_); + pitch_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); roll_offset_ = std::clamp( - roll_sum_ / static_cast(sample_count_), - -offset_limit_rad_, offset_limit_rad_); + roll_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); calibrated_once_ = true; } @@ -109,45 +101,39 @@ class DeformableChassisActiveSuspension { } Corrections update( - double pitch, double roll, - double pitch_rate, double roll_rate, - bool suspension_active, bool low_prone, - double min_angle_deg, double max_angle_deg, - double base_angle_deg, bool correction_inverted, - const std::array& base_joint_angles, - double dt) { + double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, + bool low_prone, double min_angle_deg, double max_angle_deg, double base_angle_deg, + bool correction_inverted, const std::array& base_joint_angles, double dt) { - Corrections corr; + Corrections corrections; if (!suspension_active) { reset_attitude_(); run_correction_trajectory_( - low_prone, min_angle_deg, max_angle_deg, - base_angle_deg, base_joint_angles, dt, corr); - return corr; + low_prone, min_angle_deg, max_angle_deg, base_angle_deg, base_joint_angles, dt, + corrections); + return corrections; } constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - double cp = std::clamp(pitch, -max_attitude, max_attitude); - double cr = std::clamp(roll, -max_attitude, max_attitude); + double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); + double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); - double pitch_outer = pitch_outer_pid_.update(-cp); - double roll_outer = roll_outer_pid_.update(cr); - double pitch_diff = pitch_inner_pid_.update( - pitch_outer - pitch_rate); - double roll_diff = roll_inner_pid_.update( - roll_outer + roll_rate); + double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + double roll_outer = roll_outer_pid_.update(clamped_roll); + double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); + double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { reset_attitude_(); - return corr; + return corrections; } compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); run_correction_trajectory_( - low_prone, min_angle_deg, max_angle_deg, base_angle_deg, - base_joint_angles, dt, corr); - return corr; + low_prone, min_angle_deg, max_angle_deg, base_angle_deg, base_joint_angles, dt, + corrections); + return corrections; } void reset() { @@ -163,28 +149,22 @@ class DeformableChassisActiveSuspension { } private: - static double deg_to_rad_(double deg) { - return deg * std::numbers::pi / 180.0; - } + static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } void load_pid_( - rclcpp::Node& node, const std::string& prefix, - pid::PidCalculator& pid, - double kp_default, double ki_default, double kd_default, - double integral_min_default, double integral_max_default, - double output_min_default, double output_max_default) { - - pid.kp = node.get_parameter_or(prefix + "kp", kp_default); - pid.ki = node.get_parameter_or(prefix + "ki", ki_default); - pid.kd = node.get_parameter_or(prefix + "kd", kd_default); - pid.integral_min = - node.get_parameter_or(prefix + "integral_min", integral_min_default); - pid.integral_max = - node.get_parameter_or(prefix + "integral_max", integral_max_default); - pid.output_min = - node.get_parameter_or(prefix + "output_min", output_min_default); - pid.output_max = - node.get_parameter_or(prefix + "output_max", output_max_default); + rclcpp::Node& node, const std::string& prefix, pid::PidCalculator& pid, double kp_default, + double ki_default, double kd_default, double integral_min_default, + double integral_max_default, double output_min_default, double output_max_default) { + + pid.kp = node.get_parameter_or(prefix + "kp", kp_default); + pid.ki = node.get_parameter_or(prefix + "ki", ki_default); + pid.kd = node.get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = node.get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = node.get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = node.get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = node.get_parameter_or(prefix + "output_max", output_max_default); } void reset_attitude_() { @@ -196,129 +176,124 @@ class DeformableChassisActiveSuspension { } void reset_calibration_window_() { - calibration_hold_elapsed_ = 0.0; - sample_count_ = 0; - pitch_sum_ = 0.0; - roll_sum_ = 0.0; - calibration_completed_for_window_ = false; + calibration_hold_elapsed_ = 0.0; + sample_count_ = 0; + pitch_sum_ = 0.0; + roll_sum_ = 0.0; + calibration_completed_for_window_ = false; } - void compute_correction_targets_( - double pitch_diff, double roll_diff, bool inverted) { + void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { if (inverted) { - double fp = std::max(pitch_diff, 0.0); - double bp = std::max(-pitch_diff, 0.0); - double lr = std::max(-roll_diff, 0.0); - double rr = std::max(roll_diff, 0.0); - correction_target_rad_[0] = -(fp + lr); - correction_target_rad_[1] = -(bp + lr); - correction_target_rad_[2] = -(bp + rr); - correction_target_rad_[3] = -(fp + rr); + double front_pitch_contribution = std::max(pitch_diff, 0.0); + double back_pitch_contribution = std::max(-pitch_diff, 0.0); + double left_roll_contribution = std::max(-roll_diff, 0.0); + double right_roll_contribution = std::max(roll_diff, 0.0); + correction_target_rad_[kLeftFront] = + -(front_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kRightBack] = + -(back_pitch_contribution + right_roll_contribution); + correction_target_rad_[kRightFront] = + -(front_pitch_contribution + right_roll_contribution); } else { - double fp = std::max(-pitch_diff, 0.0); - double bp = std::max(pitch_diff, 0.0); - double lr = std::max(roll_diff, 0.0); - double rr = std::max(-roll_diff, 0.0); - correction_target_rad_[0] = fp + lr; - correction_target_rad_[1] = bp + lr; - correction_target_rad_[2] = bp + rr; - correction_target_rad_[3] = fp + rr; + double front_pitch_contribution = std::max(-pitch_diff, 0.0); + double back_pitch_contribution = std::max(pitch_diff, 0.0); + double left_roll_contribution = std::max(roll_diff, 0.0); + double right_roll_contribution = std::max(-roll_diff, 0.0); + correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; + correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; + correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = + front_pitch_contribution + right_roll_contribution; } } void run_correction_trajectory_( - bool low_prone, - double min_angle_deg, double max_angle_deg, - double base_angle_deg, - const std::array& base_joint_angles, - double dt, - Corrections& corr) { + bool low_prone, double min_angle_deg, double max_angle_deg, double base_angle_deg, + const std::array& base_joint_angles, double dt, Corrections& corrections) { double max_target_rad = deg_to_rad_(max_angle_deg); double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); for (size_t i = 0; i < kJointCount; ++i) { double base_angle = std::isfinite(base_joint_angles[i]) - ? base_joint_angles[i] - : (low_prone - ? min_susp_rad - : deg_to_rad_(base_angle_deg)); + ? base_joint_angles[i] + : (low_prone ? min_susp_rad : deg_to_rad_(base_angle_deg)); double correction_min = min_susp_rad - base_angle; double correction_max = max_target_rad - base_angle; - double target = std::clamp( - correction_target_rad_[i], correction_min, correction_max); + double target = std::clamp(correction_target_rad_[i], correction_min, correction_max); - double& angle_st = correction_state_rad_[i]; - double& vel_st = correction_velocity_state_rad_[i]; - double& acc_st = correction_acceleration_state_rad_[i]; + double& angle_state = correction_state_rad_[i]; + double& velocity_state = correction_velocity_state_rad_[i]; + double& acceleration_state = correction_acceleration_state_rad_[i]; - double position_error = target - angle_st; + double position_error = target - angle_state; double stopping_distance = - vel_st * vel_st / (2.0 * correction_acc_limit_); + velocity_state * velocity_state / (2.0 * correction_acc_limit_); double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 - && std::abs(position_error) > stopping_distance) { - desired_velocity = - std::copysign(correction_vel_limit_, position_error); + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { + desired_velocity = std::copysign(correction_vel_limit_, position_error); } - double velocity_error = desired_velocity - vel_st; - acc_st = std::clamp( - velocity_error / dt, -correction_acc_limit_, - correction_acc_limit_); + double velocity_error = desired_velocity - velocity_state; + acceleration_state = + std::clamp(velocity_error / dt, -correction_acc_limit_, correction_acc_limit_); - vel_st += acc_st * dt; - vel_st = std::clamp( - vel_st, -correction_vel_limit_, correction_vel_limit_); - angle_st += vel_st * dt; + velocity_state += acceleration_state * dt; + velocity_state = + std::clamp(velocity_state, -correction_vel_limit_, correction_vel_limit_); + angle_state += velocity_state * dt; - double next_error = target - angle_st; + double next_error = target - angle_state; if ((position_error > 0.0 && next_error < 0.0) || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(vel_st) < 1e-3)) { - angle_st = target; - vel_st = 0.0; - acc_st = 0.0; + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; } - corr.active[i] = true; + corrections.joint_correction_active[i] = true; } - corr.angle = correction_state_rad_; - corr.velocity = correction_velocity_state_rad_; - corr.acceleration = correction_acceleration_state_rad_; + corrections.joint_angle_correction = correction_state_rad_; + corrections.joint_velocity_correction = correction_velocity_state_rad_; + corrections.joint_acceleration_correction = correction_acceleration_state_rad_; } + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr size_t kJointCount = 4; + pid::PidCalculator pitch_outer_pid_{}; pid::PidCalculator pitch_inner_pid_{}; pid::PidCalculator roll_outer_pid_{}; pid::PidCalculator roll_inner_pid_{}; - double correction_vel_limit_ = 40.0; - double correction_acc_limit_ = 200.0; - - double calibration_wait_time_ = 2.0; - double calibration_sample_time_ = 3.0; - double calibration_hold_elapsed_ = 0.0; - size_t sample_count_ = 0; - double pitch_sum_ = 0.0; - double roll_sum_ = 0.0; - bool calibration_completed_for_window_ = false; - bool calibrated_once_ = false; - double pitch_offset_ = 0.0; - double roll_offset_ = 0.0; - - std::array correction_target_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array correction_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array correction_velocity_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; - std::array correction_acceleration_state_rad_ = { - 0.0, 0.0, 0.0, 0.0}; + double correction_vel_limit_ = 40.0; + double correction_acc_limit_ = 200.0; + + double calibration_wait_time_ = 2.0; + double calibration_sample_time_ = 3.0; + double calibration_hold_elapsed_ = 0.0; + size_t sample_count_ = 0; + double pitch_sum_ = 0.0; + double roll_sum_ = 0.0; + bool calibration_completed_for_window_ = false; + bool calibrated_once_ = false; + double pitch_offset_ = 0.0; + double roll_offset_ = 0.0; + + std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp index cbeea088..74bc3d6b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_omni_wheel_controller.cpp @@ -26,43 +26,32 @@ class DeformableOmniWheelController DeformableOmniWheelController() : Node( get_component_name(), - rclcpp::NodeOptions{} - .automatically_declare_parameters_from_overrides(true)) + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , mass_(get_parameter("mass").as_double()) , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) , wheel_radius_(get_parameter("wheel_radius").as_double()) - , friction_coefficient_( - get_parameter("friction_coefficient").as_double()) + , friction_coefficient_(get_parameter("friction_coefficient").as_double()) , k1_(get_parameter("k1").as_double()) , k2_(get_parameter("k2").as_double()) , no_load_power_(get_parameter("no_load_power").as_double()) , com_height_(get_parameter_or("com_height", 0.0)) - , chassis_radius_x_(get_parameter_or( - "chassis_radius_x", 0.2341741 / std::numbers::sqrt2)) - , chassis_radius_y_(get_parameter_or( - "chassis_radius_y", 0.2341741 / std::numbers::sqrt2)) + , chassis_radius_x_(get_parameter_or("chassis_radius_x", 0.2341741 / std::numbers::sqrt2)) + , chassis_radius_y_(get_parameter_or("chassis_radius_y", 0.2341741 / std::numbers::sqrt2)) , translational_velocity_pid_calculator_(5.0, 0.0, 0.0) , angular_velocity_pid_calculator_(5.0, 0.0, 0.0) , wheel_velocity_pid_(0.6, 0.0, 0.0) { - register_input( - "/chassis/left_front_wheel/max_torque", - wheel_motor_max_control_torque_); + register_input("/chassis/left_front_wheel/max_torque", wheel_motor_max_control_torque_); for (size_t i = 0; i < kWheelCount; ++i) { register_input( - fmt::format( - "/chassis/{}_wheel/velocity", kWheelName[i]), - wheel_velocity_[i]); + fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), wheel_velocity_[i]); register_output( - fmt::format( - "/chassis/{}_wheel/control_torque", - kWheelName[i]), + fmt::format("/chassis/{}_wheel/control_torque", kWheelName[i]), wheel_control_torque_[i], nan_); } - register_input( - "/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/radius", chassis_radius_); } @@ -79,8 +68,7 @@ class DeformableOmniWheelController return; } - if (!std::isfinite(*chassis_radius_) - || *chassis_radius_ <= 1e-6) { + if (!std::isfinite(*chassis_radius_) || *chassis_radius_ <= 1e-6) { reset_all_controls(); return; } @@ -89,29 +77,29 @@ class DeformableOmniWheelController for (size_t i = 0; i < kWheelCount; ++i) wheel_velocities[i] = *wheel_velocity_[i]; - const auto chassis_velocity = - calculate_chassis_velocity(wheel_velocities); - auto chassis_control_torque = - calculate_chassis_control_torque(chassis_velocity); - const auto wheel_pid_torques = calculate_wheel_pid_torques( - wheel_velocities, chassis_velocity); + const auto chassis_velocity = calculate_chassis_velocity(wheel_velocities); + auto chassis_control_torque = calculate_chassis_control_torque(chassis_velocity); + const auto wheel_pid_torques = + calculate_wheel_pid_torques(wheel_velocities, chassis_velocity); chassis_control_torque.torque = constrain_chassis_control_torque( wheel_velocities, chassis_control_torque, wheel_pid_torques); - const auto wheel_control_torques = calculate_wheel_control_torques( - chassis_control_torque, wheel_pid_torques); + const auto wheel_control_torques = + calculate_wheel_control_torques(chassis_control_torque, wheel_pid_torques); for (size_t i = 0; i < kWheelCount; ++i) *wheel_control_torque_[i] = wheel_control_torques[i]; } private: - static constexpr size_t kWheelCount = 4; + static constexpr size_t kWheelCount = 4; static constexpr const char* kWheelName[] = { - "left_front", "left_back", "right_back", "right_front", + "left_front", + "left_back", + "right_back", + "right_front", }; - static constexpr double nan_ = - std::numeric_limits::quiet_NaN(); - static constexpr double g_ = 9.81; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double g_ = 9.81; struct ChassisControlTorque { Eigen::Vector2d torque; @@ -123,121 +111,92 @@ class DeformableOmniWheelController *wheel_control_torque_[i] = 0.0; } - Eigen::Vector3d calculate_chassis_velocity( - const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d calculate_chassis_velocity(const Eigen::Vector4d& wheel_velocities) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const double a_plus_b = - std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector3d velocity; velocity.x() = -w1 - w2 + w3 + w4; velocity.y() = w1 - w2 - w3 + w4; velocity.z() = (w1 + w2 + w3 + w4) / a_plus_b; - velocity *= - (-std::numbers::sqrt2 / 4 * wheel_radius_); + velocity *= (-std::numbers::sqrt2 / 4 * wheel_radius_); return velocity; } - ChassisControlTorque calculate_chassis_control_torque( - const Eigen::Vector3d& chassis_velocity) { + ChassisControlTorque calculate_chassis_control_torque(const Eigen::Vector3d& chassis_velocity) { ChassisControlTorque result; - Eigen::Vector3d err = + Eigen::Vector3d chassis_velocity_error = chassis_control_velocity_->vector - chassis_velocity; Eigen::Vector2d translational_torque = (-std::numbers::sqrt2 / 4 * wheel_radius_) * mass_ - * translational_velocity_pid_calculator_.update( - err.head<2>()); + * translational_velocity_pid_calculator_.update(chassis_velocity_error.head<2>()); result.torque.x() = translational_torque.norm(); - const double a_plus_b = - std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); - result.torque.y() = - (-std::numbers::sqrt2 / 4 * wheel_radius_) - * (moment_of_inertia_ / a_plus_b) - * angular_velocity_pid_calculator_.update(err[2]); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + result.torque.y() = (-std::numbers::sqrt2 / 4 * wheel_radius_) + * (moment_of_inertia_ / a_plus_b) + * angular_velocity_pid_calculator_.update(chassis_velocity_error[2]); Eigen::Vector2d translational_torque_direction; if (result.torque.x() > 0) - translational_torque_direction = - translational_torque / result.torque.x(); + translational_torque_direction = translational_torque / result.torque.x(); else - translational_torque_direction = - Eigen::Vector2d::UnitX(); - auto& [x, y] = translational_torque_direction; + translational_torque_direction = Eigen::Vector2d::UnitX(); + auto& [x, y] = translational_torque_direction; result.lambda = {-x + y, -x - y}; return result; } Eigen::Vector4d calculate_wheel_pid_torques( - const Eigen::Vector4d& wheel_velocities, - const Eigen::Vector3d& chassis_velocity) { + const Eigen::Vector4d& wheel_velocities, const Eigen::Vector3d& chassis_velocity) { const auto& [x, y, z] = chassis_velocity; - const double a_plus_b = - std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); + const double a_plus_b = std::numbers::sqrt2 * std::max(*chassis_radius_, 1e-6); Eigen::Vector4d wheel_control_velocity = { -x + y + a_plus_b * z, -x - y + a_plus_b * z, +x - y + a_plus_b * z, +x + y + a_plus_b * z, }; - wheel_control_velocity *= - -1 / (std::numbers::sqrt2 * wheel_radius_); - return wheel_velocity_pid_.update( - wheel_control_velocity - wheel_velocities); + wheel_control_velocity *= -1 / (std::numbers::sqrt2 * wheel_radius_); + return wheel_velocity_pid_.update(wheel_control_velocity - wheel_velocities); } Eigen::Vector2d constrain_chassis_control_torque( - const Eigen::Vector4d& wheel_velocities, - const ChassisControlTorque& chassis_control_torque, + const Eigen::Vector4d& wheel_velocities, const ChassisControlTorque& chassis_control_torque, const Eigen::Vector4d& wheel_pid_torques) const { const auto& [w1, w2, w3, w4] = wheel_velocities; - const auto& [x_max, y_max] = chassis_control_torque.torque; - const double y_sign = y_max > 0 ? 1.0 : -1.0; - const auto& [lambda_1, lambda_2] = - chassis_control_torque.lambda; + const auto& [x_max, y_max] = chassis_control_torque.torque; + const double y_sign = y_max > 0 ? 1.0 : -1.0; + const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; const auto& [t1, t2, t3, t4] = wheel_pid_torques; - const double rhombus_top = - (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; - const double rhombus_right = - rhombus_top - / std::max(std::abs(lambda_1), std::abs(lambda_2)); + const double rhombus_top = (friction_coefficient_ * mass_ * g_ * wheel_radius_) / 4; + const double rhombus_right = rhombus_top / std::max(std::abs(lambda_1), std::abs(lambda_2)); const double a = 4 * k1_; const double b = 0; const double c = 4 * k1_; - const double d = lambda_1 * (w1 - w3 + 2 * k1_ * (t1 - t3)) - + lambda_2 * (w2 - w4 + 2 * k1_ * (t2 - t4)); - const double e = y_sign * (2 * k1_ * wheel_pid_torques.sum() - + wheel_velocities.sum()); - const double f = - k1_ * wheel_pid_torques.array().pow(2).sum() - + (wheel_pid_torques.array() * wheel_velocities.array()) - .sum() - + k2_ * wheel_velocities.array().pow(2).sum() - - no_load_power_ - *power_limit_; + const double d = + lambda_1 * (w1 - w3 + 2 * k1_ * (t1 - t3)) + lambda_2 * (w2 - w4 + 2 * k1_ * (t2 - t4)); + const double e = y_sign * (2 * k1_ * wheel_pid_torques.sum() + wheel_velocities.sum()); + const double f = k1_ * wheel_pid_torques.array().pow(2).sum() + + (wheel_pid_torques.array() * wheel_velocities.array()).sum() + + k2_ * wheel_velocities.array().pow(2).sum() - no_load_power_ + - *power_limit_; Eigen::Vector2d result = Eigen::Vector2d::Constant(nan_); if (com_height_ > 1e-6) { - const double dir_x = -(lambda_1 + lambda_2) / 2.0; - const double dir_y = (lambda_1 - lambda_2) / 2.0; - const double coeff = - -com_height_ - / (std::numbers::sqrt2 * wheel_radius_); - const double gamma_1 = - coeff - * (+dir_x / chassis_radius_x_ - + dir_y / chassis_radius_y_); - const double gamma_2 = - coeff - * (-dir_x / chassis_radius_x_ - + dir_y / chassis_radius_y_); + const double dir_x = -(lambda_1 + lambda_2) / 2.0; + const double dir_y = (lambda_1 - lambda_2) / 2.0; + const double coeff = -com_height_ / (std::numbers::sqrt2 * wheel_radius_); + const double gamma_1 = coeff * (+dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); + const double gamma_2 = coeff * (-dir_x / chassis_radius_x_ + dir_y / chassis_radius_y_); const double force_to_torque = friction_coefficient_ * wheel_radius_; - const double rhs = force_to_torque * mass_ * g_ / 4.0; + const double rhs = force_to_torque * mass_ * g_ / 4.0; const std::vector half_planes = { {lambda_1 - force_to_torque * gamma_1, y_sign, rhs}, {-lambda_1 - force_to_torque * gamma_1, -y_sign, rhs}, @@ -249,14 +208,12 @@ class DeformableOmniWheelController {lambda_2 + force_to_torque * gamma_2, -y_sign, rhs}, }; result = qcp_solver_.solve( - {1.0, 1.0}, {x_max, std::abs(y_max)}, - half_planes, {a, b, c, d, e, f}); + {1.0, 1.0}, {x_max, std::abs(y_max)}, half_planes, {a, b, c, d, e, f}); } if (!result.array().isFinite().all()) { result = qcp_solver_.solve( - {1.0, 1.0}, {x_max, std::abs(y_max)}, - {rhombus_right, rhombus_top}, + {1.0, 1.0}, {x_max, std::abs(y_max)}, {rhombus_right, rhombus_top}, {a, b, c, d, e, f}); } result.y() *= y_sign; @@ -264,18 +221,15 @@ class DeformableOmniWheelController } static Eigen::Vector4d calculate_wheel_control_torques( - ChassisControlTorque chassis_control_torque, - Eigen::Vector4d wheel_pid_torques) { - const auto& [lambda_1, lambda_2] = - chassis_control_torque.lambda; - Eigen::Vector4d wheel_torques = { + ChassisControlTorque chassis_control_torque, Eigen::Vector4d wheel_pid_torques) { + const auto& [lambda_1, lambda_2] = chassis_control_torque.lambda; + Eigen::Vector4d wheel_torques = { +lambda_1 * chassis_control_torque.torque.x(), +lambda_2 * chassis_control_torque.torque.x(), -lambda_1 * chassis_control_torque.torque.x(), -lambda_2 * chassis_control_torque.torque.x(), }; - wheel_torques.array() += - chassis_control_torque.torque.y(); + wheel_torques.array() += chassis_control_torque.torque.y(); wheel_torques.array() += wheel_pid_torques.array(); return wheel_torques; } @@ -291,18 +245,14 @@ class DeformableOmniWheelController InputInterface wheel_motor_max_control_torque_; - std::array, kWheelCount> - wheel_velocity_; - std::array, kWheelCount> - wheel_control_torque_; + std::array, kWheelCount> wheel_velocity_; + std::array, kWheelCount> wheel_control_torque_; - InputInterface - chassis_control_velocity_; + InputInterface chassis_control_velocity_; InputInterface power_limit_; InputInterface chassis_radius_; - pid::MatrixPidCalculator<2> - translational_velocity_pid_calculator_; + pid::MatrixPidCalculator<2> translational_velocity_pid_calculator_; pid::PidCalculator angular_velocity_pid_calculator_; pid::MatrixPidCalculator<4> wheel_velocity_pid_; @@ -315,5 +265,4 @@ class DeformableOmniWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DeformableOmniWheelController, - rmcs_executor::Component) + rmcs_core::controller::chassis::DeformableOmniWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 875920ed..7ccad8c7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -36,19 +36,17 @@ class DeformableInfantryGimbalController get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); - get_parameter_or( - "ctrl_hold_chassis_yaw_velocity_max", ctrl_hold_chassis_yaw_velocity_max_, 30.0); } auto update() -> void override { const auto switch_right = *input_.switch_right; - const auto switch_left = *input_.switch_left; + const auto switch_left = *input_.switch_left; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { suspension_on_by_switch_ = false; - last_switch_right_ = switch_right; + last_switch_right_ = switch_right; reset_all_controls(); return; } @@ -79,18 +77,19 @@ class DeformableInfantryGimbalController *output_.yaw_control_torque = kNaN; } - const auto feedforward_enabled = auto_aim_active && input_.auto_aim_feedforward_valid.ready() + const auto feedforward_enabled = auto_aim_active + && input_.auto_aim_feedforward_valid.ready() && *input_.auto_aim_feedforward_valid; if (std::isfinite(angle_error.yaw_angle_error)) { const auto yaw_velocity_ff = feedforward_enabled && input_.auto_aim_yaw_rate.ready() && std::isfinite(*input_.auto_aim_yaw_rate) - ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate - : 0.0; - const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() - && std::isfinite(*input_.auto_aim_yaw_acc) - ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc - : 0.0; + ? yaw_vel_ff_gain_ * *input_.auto_aim_yaw_rate + : 0.0; + const auto yaw_acc_ff = feedforward_enabled && input_.auto_aim_yaw_acc.ready() + && std::isfinite(*input_.auto_aim_yaw_acc) + ? yaw_acc_ff_gain_ * *input_.auto_aim_yaw_acc + : 0.0; const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error) + yaw_velocity_ff; @@ -103,33 +102,31 @@ class DeformableInfantryGimbalController pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } else { - const auto pitch_acc_ff = feedforward_enabled - && input_.auto_aim_pitch_acc.ready() - && std::isfinite(*input_.auto_aim_pitch_acc) - ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc - : 0.0; + const auto pitch_acc_ff = feedforward_enabled && input_.auto_aim_pitch_acc.ready() + && std::isfinite(*input_.auto_aim_pitch_acc) + ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc + : 0.0; const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = - pitch_velocity_pid_.update( - pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_acc_ff; + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_acc_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } } private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); static constexpr auto kDefaultDt = 1e-3; auto configure_pid(const std::string& prefix, pid::PidCalculator& calculator) -> void { @@ -151,7 +148,6 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/tf", tf); component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); component.register_input("/gimbal/pitch/angle", pitch_angle); @@ -177,7 +173,6 @@ class DeformableInfantryGimbalController InputInterface mouse; InputInterface update_rate; - InputInterface tf; InputInterface yaw_angle; InputInterface yaw_velocity; InputInterface pitch_angle; @@ -203,9 +198,6 @@ class DeformableInfantryGimbalController component.register_output("/gimbal/pitch/control_angle", pitch_control_angle, kNaN); component.register_output("/gimbal/yaw/control_angle_error", yaw_angle_error, kNaN); component.register_output("/gimbal/pitch/control_angle_error", pitch_angle_error, kNaN); - component.register_output( - "/chassis/manual_yaw_velocity_override", chassis_manual_yaw_velocity_override, - kNaN); } OutputInterface yaw_control_torque; @@ -215,7 +207,6 @@ class DeformableInfantryGimbalController OutputInterface pitch_control_angle; OutputInterface yaw_angle_error; OutputInterface pitch_angle_error; - OutputInterface chassis_manual_yaw_velocity_override; } output_{*this}; auto ctrl_hold_requested() const -> bool { @@ -232,7 +223,8 @@ class DeformableInfantryGimbalController } auto update_dt() const -> double { - if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) && *input_.update_rate > 1e-6) + if (input_.update_rate.ready() && std::isfinite(*input_.update_rate) + && *input_.update_rate > 1e-6) return 1.0 / *input_.update_rate; return kDefaultDt; } @@ -283,10 +275,10 @@ class DeformableInfantryGimbalController if (!ctrl_hold_active_) activate_ctrl_hold(); - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; if (input_.pitch_angle.ready() && std::isfinite(*input_.pitch_angle)) { auto pitch_target_error = ctrl_hold_pitch_target_angle_ - *input_.pitch_angle; @@ -295,7 +287,7 @@ class DeformableInfantryGimbalController else if (pitch_target_error < -std::numbers::pi) pitch_target_error += 2 * std::numbers::pi; - *output_.pitch_angle_error = pitch_target_error; + *output_.pitch_angle_error = pitch_target_error; const auto pitch_velocity_ref = pitch_angle_pid_.update(pitch_target_error); if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; @@ -304,7 +296,7 @@ class DeformableInfantryGimbalController } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; - *output_.pitch_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; } } } @@ -314,18 +306,17 @@ class DeformableInfantryGimbalController yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); - *output_.yaw_control_torque = kNaN; - *output_.yaw_control_angle = kNaN; + *output_.yaw_control_torque = kNaN; + *output_.yaw_control_angle = kNaN; *output_.pitch_control_velocity = kNaN; - *output_.pitch_control_torque = kNaN; - *output_.pitch_control_angle = kNaN; - *output_.chassis_manual_yaw_velocity_override = kNaN; + *output_.pitch_control_torque = kNaN; + *output_.pitch_control_angle = kNaN; } auto reset_all_controls() -> void { deactivate_ctrl_hold(); gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); - *output_.yaw_angle_error = kNaN; + *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; reset_control_outputs(); } @@ -354,16 +345,15 @@ class DeformableInfantryGimbalController get_parameter("pitch_velocity_kd").as_double(), }; - double joystick_sensitivity_ = 0.003; - double mouse_sensitivity_ = 0.5; - bool pitch_torque_control_enabled_ = false; - double yaw_vel_ff_gain_ = 0.0; - double yaw_acc_ff_gain_ = 0.0; - double pitch_acc_ff_gain_ = 0.0; + double joystick_sensitivity_ = 0.003; + double mouse_sensitivity_ = 0.5; + bool pitch_torque_control_enabled_ = false; + double yaw_vel_ff_gain_ = 0.0; + double yaw_acc_ff_gain_ = 0.0; + double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; - double ctrl_hold_chassis_yaw_velocity_max_ = 30.0; - bool suspension_on_by_switch_ = false; - bool ctrl_hold_active_ = false; + bool suspension_on_by_switch_ = false; + bool ctrl_hold_active_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index f93aaca2..a654bf63 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -58,7 +58,7 @@ class DeformableInfantryOmniB tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); // For command: remote-status - using Srv = std_srvs::srv::Trigger; + using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { @@ -71,7 +71,8 @@ class DeformableInfantryOmniB top_board_ = std::make_unique( *this, *deformable_infantry_command_, vt13_, get_parameter("serial_filter_top_board").as_string()); - remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); + remote_control_ = + std::make_unique(*this, bottom_board_->dr16_, vt13_); } ~DeformableInfantryOmniB() override = default; @@ -90,8 +91,15 @@ class DeformableInfantryOmniB } private: + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; static constexpr const char* kJointName[] = { - "left_front", "left_back", "right_back", "right_front", + "left_front", + "left_back", + "right_back", + "right_front", }; class DeformableInfantryOmniBCommand; @@ -173,10 +181,13 @@ class DeformableInfantryOmniB "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); for (size_t i = 0; i < 4; ++i) { deformableInfantry.register_output( - fmt::format("/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), + fmt::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmniB::kJointName[i]), joint_physical_angle_[i], nan_); deformableInfantry.register_output( - fmt::format("/chassis/{}_joint/physical_velocity", DeformableInfantryOmniB::kJointName[i]), + fmt::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmniB::kJointName[i]), joint_physical_velocity_[i], nan_); } deformableInfantry.register_output("/chassis/encoder/alpha", encoder_alpha_, nan_); @@ -203,7 +214,7 @@ class DeformableInfantryOmniB const double q3 = imu_.q3(); double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); const double standard_pitch = std::asin(sin_pitch); const double standard_roll = @@ -211,10 +222,10 @@ class DeformableInfantryOmniB // Export chassis attitude using the requested convention: // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); + *chassis_imu_roll_rate_ = imu_.gx(); } for (auto& motor : chassis_wheel_motors_) @@ -224,8 +235,7 @@ class DeformableInfantryOmniB for (size_t i = 0; i < 4; ++i) update_joint_physical_feedback_( - i, joint_physical_angle_[i], - joint_physical_velocity_[i]); + i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -250,7 +260,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[kLeftFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -261,7 +271,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[kLeftBack].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -272,7 +282,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[kRightBack].generate_command(), device::CanPacket8::PaddingQuarter{}, gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -283,7 +293,7 @@ class DeformableInfantryOmniB .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), + chassis_wheel_motors_[kRightFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -291,7 +301,7 @@ class DeformableInfantryOmniB .as_bytes(), }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ @@ -308,36 +318,28 @@ class DeformableInfantryOmniB } else { for (size_t i = 0; i < 4; ++i) { switch (i) { - case 0: + case kLeftFront: builder.can0_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 1: + case kLeftBack: builder.can1_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 2: + case kRightBack: builder.can2_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 3: + case kRightFront: builder.can3_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; } @@ -350,8 +352,8 @@ class DeformableInfantryOmniB rmcs_executor::Component& command_; static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; static constexpr double default_radius_ = 0.5 * rod_length_ + chassis_radius_base_; static double to_physical_angle_(double motor_angle) { @@ -364,27 +366,27 @@ class DeformableInfantryOmniB size_t index, OutputInterface& angle_output, OutputInterface& velocity_output) { if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = nan_; + *angle_output = nan_; *velocity_output = nan_; return; } - *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); + *angle_output = to_physical_angle_(chassis_joint_motors_[index].angle()); *velocity_output = to_physical_velocity_(chassis_joint_motors_[index].velocity()); } void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *joint_physical_angle_[0], *joint_physical_angle_[1], - *joint_physical_angle_[2], *joint_physical_angle_[3]}; + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; const Eigen::Vector4d alpha_dot_rad{ - *joint_physical_velocity_[0], *joint_physical_velocity_[1], - *joint_physical_velocity_[2], *joint_physical_velocity_[3]}; + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = nan_; + *encoder_alpha_ = nan_; *encoder_alpha_dot_ = nan_; - *radius_ = default_radius_; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", @@ -392,7 +394,7 @@ class DeformableInfantryOmniB return; } - *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } @@ -411,37 +413,42 @@ class DeformableInfantryOmniB if (debug_log_wheel_motor_) { std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) - fmt::format_to( - std::back_inserter(wheel_rx_str), "{}%c", - i > 0 ? " " : "", wheel_rx(i)); + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } RCLCPP_INFO( deformable_infantry_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " "rx=[%s]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - wheel_rx_str.c_str()); + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) - fmt::format_to( - std::back_inserter(joint_rx_str), "{}%c", - i > 0 ? " " : "", joint_rx(i)); + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } RCLCPP_INFO( deformable_infantry_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "rx=[%s]", - *joint_physical_angle_[0], *joint_physical_angle_[1], - *joint_physical_angle_[2], *joint_physical_angle_[3], - *joint_physical_velocity_[0], *joint_physical_velocity_[1], - *joint_physical_velocity_[2], *joint_physical_velocity_[3], + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], joint_rx_str.c_str()); } @@ -453,7 +460,7 @@ class DeformableInfantryOmniB if (now < next_supercap_feedback_log_time_) return; - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); @@ -461,8 +468,7 @@ class DeformableInfantryOmniB deformable_infantry_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : nan_, supercap_rx ? supercap_.chassis_voltage() : nan_, supercap_rx ? supercap_.chassis_power() : nan_, @@ -482,8 +488,7 @@ class DeformableInfantryOmniB dr16_.store_status(data.uart_data); } - void process_chassis_can_receive_( - size_t index, const librmcs::data::CanDataView& data) { + void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { @@ -501,8 +506,7 @@ class DeformableInfantryOmniB void can1_receive_callback(const librmcs::data::CanDataView& data) override { process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { + if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -543,43 +547,31 @@ class DeformableInfantryOmniB OutputInterface& tf_; device::Bmi088 imu_{1000, 0.2, 0.0}; - device::LkMotor gimbal_yaw_motor_{ - deformable_infantry_, command_, "/gimbal/yaw"}; + device::LkMotor gimbal_yaw_motor_{deformable_infantry_, command_, "/gimbal/yaw"}; device::Dr16 dr16_; device::DjiMotor chassis_wheel_motors_[4]{ - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/left_front_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/left_back_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/right_back_wheel"}, - device::DjiMotor{ - deformable_infantry_, command_, "/chassis/right_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_front_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/left_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_back_wheel"}, + device::DjiMotor{deformable_infantry_, command_, "/chassis/right_front_wheel"}, }; device::LkMotor chassis_joint_motors_[4]{ - device::LkMotor{ - deformable_infantry_, command_, "/chassis/left_front_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/left_back_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/right_back_joint"}, - device::LkMotor{ - deformable_infantry_, command_, "/chassis/right_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_front_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/left_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_back_joint"}, + device::LkMotor{deformable_infantry_, command_, "/chassis/right_front_joint"}, }; std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; - Clock::time_point next_chassis_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; - Clock::time_point next_supercap_feedback_log_time_{ - Clock::now() + std::chrono::seconds(1)}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; + Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; device::Supercap supercap_{deformable_infantry_, command_}; - std::atomic latest_supercap_status_{ - device::CanPacket8{uint64_t{0}}}; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::DjiMotor gimbal_bullet_feeder_{ deformable_infantry_, command_, "/gimbal/bullet_feeder"}; @@ -605,8 +597,8 @@ class DeformableInfantryOmniB explicit TopBoard( DeformableInfantryOmniB& deformableInfantry, - DeformableInfantryOmniBCommand& deformableInfantry_command, - device::Vt13& vt13, const std::string& serial_filter = {}) + DeformableInfantryOmniBCommand& deformableInfantry_command, device::Vt13& vt13, + const std::string& serial_filter = {}) : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) @@ -657,7 +649,7 @@ class DeformableInfantryOmniB gimbal_imu_pose.conjugate()); *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); - *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( @@ -667,7 +659,7 @@ class DeformableInfantryOmniB void command_update() { auto builder = start_transmit(); builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index e2058665..ff100c1d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -56,10 +56,11 @@ class DeformableInfantryOmni *this, *command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( *this, *command_, vt13_, get_parameter("serial_filter_top_board").as_string()); - remote_control_ = std::make_unique(*this, bottom_board_->dr16_, vt13_); + remote_control_ = + std::make_unique(*this, bottom_board_->dr16_, vt13_); // For command: remote-status - using Srv = std_srvs::srv::Trigger; + using Srv = std_srvs::srv::Trigger; status_service_ = create_service( "/rmcs/service/robot_status", [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { @@ -83,9 +84,16 @@ class DeformableInfantryOmni } private: - static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; static constexpr const char* kJointName[] = { - "left_front", "left_back", "right_back", "right_front", + "left_front", + "left_back", + "right_back", + "right_front", }; class Command : public rmcs_executor::Component { @@ -154,10 +162,13 @@ class DeformableInfantryOmni status.register_output("/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); for (size_t i = 0; i < 4; ++i) { status.register_output( - fmt::format("/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), + fmt::format( + "/chassis/{}_joint/physical_angle", DeformableInfantryOmni::kJointName[i]), joint_physical_angle_[i], kNaN); status.register_output( - fmt::format("/chassis/{}_joint/physical_velocity", DeformableInfantryOmni::kJointName[i]), + fmt::format( + "/chassis/{}_joint/physical_velocity", + DeformableInfantryOmni::kJointName[i]), joint_physical_velocity_[i], kNaN); } status.register_output("/chassis/encoder/alpha", encoder_alpha_, kNaN); @@ -182,7 +193,7 @@ class DeformableInfantryOmni const double q3 = imu_.q3(); double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); - sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); const double standard_pitch = std::asin(sin_pitch); const double standard_roll = @@ -190,10 +201,10 @@ class DeformableInfantryOmni // Export chassis attitude using the requested convention: // pitch < 0 when the front is higher, roll > 0 when the left side is higher. - *chassis_imu_pitch_ = -standard_pitch; - *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; *chassis_imu_pitch_rate_ = -imu_.gy(); - *chassis_imu_roll_rate_ = imu_.gx(); + *chassis_imu_roll_rate_ = imu_.gx(); } for (auto& motor : chassis_wheel_motors_) @@ -203,8 +214,7 @@ class DeformableInfantryOmni for (size_t i = 0; i < 4; ++i) update_joint_physical_feedback_( - i, joint_physical_angle_[i], - joint_physical_velocity_[i]); + i, joint_physical_angle_[i], joint_physical_velocity_[i]); update_geometry_feedback_(); if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) @@ -229,7 +239,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[0].generate_command(), + chassis_wheel_motors_[kLeftFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -240,7 +250,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[kLeftBack].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -251,7 +261,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[2].generate_command(), + chassis_wheel_motors_[kRightBack].generate_command(), device::CanPacket8::PaddingQuarter{}, gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -262,7 +272,7 @@ class DeformableInfantryOmni .can_id = 0x200, .can_data = device::CanPacket8{ - chassis_wheel_motors_[3].generate_command(), + chassis_wheel_motors_[kRightFront].generate_command(), device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, @@ -270,7 +280,7 @@ class DeformableInfantryOmni .as_bytes(), }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_yaw_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ @@ -287,36 +297,28 @@ class DeformableInfantryOmni } else { for (size_t i = 0; i < 4; ++i) { switch (i) { - case 0: + case kLeftFront: builder.can0_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 1: + case kLeftBack: builder.can1_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 2: + case kRightBack: builder.can2_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; - case 3: + case kRightFront: builder.can3_transmit({ - .can_id = 0x141, - .can_data = - chassis_joint_motors_[i].generate_command() - .as_bytes(), + .can_id = 0x141, + .can_data = chassis_joint_motors_[i].generate_command().as_bytes(), }); break; } @@ -326,9 +328,9 @@ class DeformableInfantryOmni private: static constexpr double joint_zero_physical_angle_rad_ = 62.5 * std::numbers::pi / 180.0; - static constexpr double chassis_radius_base_ = 0.2341741; - static constexpr double rod_length_ = 0.150; - static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; + static constexpr double chassis_radius_base_ = 0.2341741; + static constexpr double rod_length_ = 0.150; + static constexpr double default_radius_ = chassis_radius_base_ + rod_length_; DeformableInfantryOmni& status_; Component& command_; @@ -352,9 +354,9 @@ class DeformableInfantryOmni std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; - bool debug_log_supercap_ = false; - bool debug_log_wheel_motor_ = false; - bool debug_log_deformable_joint_motor_ = false; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; Clock::time_point next_chassis_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; Clock::time_point next_supercap_feedback_log_time_{Clock::now() + std::chrono::seconds(1)}; device::Supercap supercap_{status_, command_}; @@ -383,7 +385,7 @@ class DeformableInfantryOmni OutputInterface& velocity_output) { if (!joint_status_received_[index].load(std::memory_order_relaxed)) { - *angle_output = kNaN; + *angle_output = kNaN; *velocity_output = kNaN; return; } @@ -393,22 +395,22 @@ class DeformableInfantryOmni }; const auto to_physical_velocity = [](double motor_velocity) { return -motor_velocity; }; - *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); + *angle_output = to_physical_angle(chassis_joint_motors_[index].angle()); *velocity_output = to_physical_velocity(chassis_joint_motors_[index].velocity()); } void update_geometry_feedback_() { const Eigen::Vector4d alpha_rad{ - *joint_physical_angle_[0], *joint_physical_angle_[1], - *joint_physical_angle_[2], *joint_physical_angle_[3]}; + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront]}; const Eigen::Vector4d alpha_dot_rad{ - *joint_physical_velocity_[0], *joint_physical_velocity_[1], - *joint_physical_velocity_[2], *joint_physical_velocity_[3]}; + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront]}; if (!alpha_rad.array().isFinite().all() || !alpha_dot_rad.array().isFinite().all()) { - *encoder_alpha_ = kNaN; + *encoder_alpha_ = kNaN; *encoder_alpha_dot_ = kNaN; - *radius_ = default_radius_; + *radius_ = default_radius_; RCLCPP_WARN_THROTTLE( status_.get_logger(), *status_.get_clock(), 1000, "deformable joint feedback invalid, fallback chassis radius to default %.3f m", @@ -416,7 +418,7 @@ class DeformableInfantryOmni return; } - *encoder_alpha_ = alpha_rad.mean(); + *encoder_alpha_ = alpha_rad.mean(); *encoder_alpha_dot_ = alpha_dot_rad.mean(); *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } @@ -435,37 +437,42 @@ class DeformableInfantryOmni if (debug_log_wheel_motor_) { std::string wheel_rx_str; - for (size_t i = 0; i < 4; ++i) - fmt::format_to( - std::back_inserter(wheel_rx_str), "{}%c", - i > 0 ? " " : "", wheel_rx(i)); + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + wheel_rx_str.push_back(' '); + wheel_rx_str.push_back(wheel_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " "rx=[%s]", - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), - chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), - wheel_rx_str.c_str()); + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), + chassis_wheel_motors_[kLeftFront].angle(), + chassis_wheel_motors_[kLeftBack].angle(), + chassis_wheel_motors_[kRightBack].angle(), + chassis_wheel_motors_[kRightFront].angle(), wheel_rx_str.c_str()); } if (debug_log_deformable_joint_motor_) { std::string joint_rx_str; - for (size_t i = 0; i < 4; ++i) - fmt::format_to( - std::back_inserter(joint_rx_str), "{}%c", - i > 0 ? " " : "", joint_rx(i)); + for (size_t i = 0; i < 4; ++i) { + if (i > 0) + joint_rx_str.push_back(' '); + joint_rx_str.push_back(joint_rx(i)); + } RCLCPP_INFO( status_.get_logger(), "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " "rx=[%s]", - *joint_physical_angle_[0], *joint_physical_angle_[1], - *joint_physical_angle_[2], *joint_physical_angle_[3], - *joint_physical_velocity_[0], *joint_physical_velocity_[1], - *joint_physical_velocity_[2], *joint_physical_velocity_[3], + *joint_physical_angle_[kLeftFront], *joint_physical_angle_[kLeftBack], + *joint_physical_angle_[kRightBack], *joint_physical_angle_[kRightFront], + *joint_physical_velocity_[kLeftFront], *joint_physical_velocity_[kLeftBack], + *joint_physical_velocity_[kRightBack], *joint_physical_velocity_[kRightFront], joint_rx_str.c_str()); } @@ -477,7 +484,7 @@ class DeformableInfantryOmni if (now < next_supercap_feedback_log_time_) return; - const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); @@ -485,8 +492,7 @@ class DeformableInfantryOmni status_.get_logger(), "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", - supercap_rx ? 'Y' : 'N', - supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, supercap_rx ? supercap_.supercap_voltage() : kNaN, supercap_rx ? supercap_.chassis_voltage() : kNaN, supercap_rx ? supercap_.chassis_power() : kNaN, @@ -506,8 +512,7 @@ class DeformableInfantryOmni dr16_.store_status(data.uart_data); } - void process_chassis_can_receive_( - size_t index, const librmcs::data::CanDataView& data) { + void process_chassis_can_receive_(size_t index, const librmcs::data::CanDataView& data) { if (data.is_extended_can_id || data.is_remote_transmission) return; if (data.can_id == 0x201) { @@ -525,8 +530,7 @@ class DeformableInfantryOmni void can1_receive_callback(const librmcs::data::CanDataView& data) override { process_chassis_can_receive_(1, data); - if (!data.is_extended_can_id && !data.is_remote_transmission - && data.can_id == 0x300) { + if (!data.is_extended_can_id && !data.is_remote_transmission && data.can_id == 0x300) { if (data.can_data.size() == 8) latest_supercap_status_.store( device::CanPacket8{data.can_data}, std::memory_order_relaxed); @@ -617,7 +621,7 @@ class DeformableInfantryOmni gimbal_imu_pose.conjugate()); *gimbal_pitch_velocity_imu_ = gimbal_imu_.gy(); - *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); + *gimbal_yaw_velocity_imu_ = gimbal_imu_.gz(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); tf_->set_state( @@ -627,7 +631,7 @@ class DeformableInfantryOmni void command_update() { auto builder = start_transmit(); builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); builder.can1_transmit({ diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index c35bb8df..4ac2b681 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -123,7 +123,7 @@ class Flight } .as_bytes()}) .can2_transmit( - {.can_id = 0x141, + {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) .can3_transmit( {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); @@ -152,7 +152,7 @@ class Flight Eigen::Quaterniond{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(); } @@ -220,7 +220,7 @@ class Flight } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { @@ -253,7 +253,7 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; - device::Dr16 dr16_{*this}; + device::Dr16 dr16_; device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 69ddef42..21729d37 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -54,8 +54,7 @@ class OmniInfantry , 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} { + , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") { for (auto& motor : chassis_wheel_motors_) motor.configure( @@ -93,12 +92,12 @@ class OmniInfantry start_transmit().gpio_digital_read( librmcs::spec::rmcs_board_lite::kGpioDescriptors.kUart0Tx, { - .period_ms = 0, - .asap = false, - .rising_edge = false, - .falling_edge = true, + .period_ms = 0, + .asap = false, + .rising_edge = false, + .falling_edge = true, .capture_timestamp = true, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) @@ -134,10 +133,10 @@ class OmniInfantry }; } - OmniInfantry(const OmniInfantry&) = delete; + OmniInfantry(const OmniInfantry&) = delete; OmniInfantry& operator=(const OmniInfantry&) = delete; - OmniInfantry(OmniInfantry&&) = delete; - OmniInfantry& operator=(OmniInfantry&&) = delete; + OmniInfantry(OmniInfantry&&) = delete; + OmniInfantry& operator=(OmniInfantry&&) = delete; ~OmniInfantry() override = default; @@ -164,7 +163,7 @@ class OmniInfantry }); builder.can1_transmit({ - .can_id = 0x145, + .can_id = 0x145, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes(), }); @@ -181,7 +180,7 @@ class OmniInfantry }); builder.can2_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), }); @@ -226,7 +225,7 @@ class OmniInfantry tf_->set_transform( snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp index 40150e60..8639ff9f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero-little.cpp @@ -135,10 +135,10 @@ class SteeringHeroLittle Eigen::Translation3d{0.06603, 0.0, 0.082}); } - SteeringHeroLittle(const SteeringHeroLittle&) = delete; + SteeringHeroLittle(const SteeringHeroLittle&) = delete; SteeringHeroLittle& operator=(const SteeringHeroLittle&) = delete; - SteeringHeroLittle(SteeringHeroLittle&&) = delete; - SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; + SteeringHeroLittle(SteeringHeroLittle&&) = delete; + SteeringHeroLittle& operator=(SteeringHeroLittle&&) = delete; ~SteeringHeroLittle() override = default; @@ -292,10 +292,10 @@ class SteeringHeroLittle }); } - TopBoard(const TopBoard&) = delete; + TopBoard(const TopBoard&) = delete; TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; ~TopBoard() final = default; @@ -311,7 +311,7 @@ class SteeringHeroLittle tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_yaw_velocity_imu_ = imu_.gz(); *gimbal_pitch_velocity_imu_ = imu_.gy(); gimbal_top_yaw_motor_.update_status(); @@ -336,7 +336,7 @@ class SteeringHeroLittle last_camera_capturer_trigger_timestamp_ = *camera_capturer_trigger_timestamp_; *photoelectric_sensor_status_ = photoelectric_sensor_status_atomic.load(); - *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); + *grayscale_sensor_status_ = grayscale_sensor_status_atomic.load(); } void command_update() { @@ -344,19 +344,19 @@ class SteeringHeroLittle if (std::isfinite(gimbal_pitch_motor_.control_angle())) builder.can0_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_ .generate_angle_command(gimbal_pitch_motor_.control_angle()) .as_bytes(), }); else builder.can0_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), }); // Used to distinguish pitch encoder control from IMU control. builder.can0_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_top_yaw_motor_.generate_command().as_bytes(), }); @@ -393,7 +393,7 @@ class SteeringHeroLittle }); builder.can3_transmit({ - .can_id = 0x142, + .can_id = 0x142, .can_data = gimbal_bullet_feeder_.generate_torque_command().as_bytes(), }); @@ -401,14 +401,14 @@ class SteeringHeroLittle librmcs::spec::rmcs_board_lite::kGpioDescriptors[2], { .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); builder.gpio_digital_read( librmcs::spec::rmcs_board_lite::kGpioDescriptors[3], { .period_ms = 20, - .pull = librmcs::data::GpioPull::kUp, + .pull = librmcs::data::GpioPull::kUp, }); } @@ -526,7 +526,6 @@ class SteeringHeroLittle // , can2_receive_rate_counter_(logger_, "bottom/can2") // , can3_receive_rate_counter_(logger_, "bottom/can3") , imu_(1000, 0.2, 0.0) - , dr16_(steering_hero) , supercap_(steering_hero, steering_hero_command) , chassis_steering_motors_( {steering_hero, steering_hero_command, "/chassis/left_front_steering"}, @@ -631,10 +630,10 @@ class SteeringHeroLittle steering_hero.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); } - BottomBoard(const BottomBoard&) = delete; + BottomBoard(const BottomBoard&) = delete; BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; ~BottomBoard() final = default; @@ -728,7 +727,7 @@ class SteeringHeroLittle }); builder.can3_transmit({ - .can_id = 0x141, + .can_id = 0x141, .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), }); @@ -813,7 +812,7 @@ class SteeringHeroLittle } void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + dr16_.store_status(data.uart_data); } void accelerometer_receive_callback( diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp index e46c0ec4..6f1bc947 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/deformable_infantry_ui.cpp @@ -65,15 +65,9 @@ class DeformableInfantry register_input("/chassis/voltage", chassis_voltage_); - for (size_t i = 0; i < 4; ++i) { + for (size_t i = 0; i < kJointCount; ++i) { register_input( - fmt::format("/chassis/{}_wheel/velocity", kWheelName[i]), - wheel_velocity_[i]); - } - - for (size_t i = 0; i < 4; ++i) { - register_input( - fmt::format("/chassis/{}_joint/physical_angle", kWheelName[i]), + fmt::format("/chassis/{}_joint/physical_angle", kJointName[i]), joint_physical_angle_[i], false); } @@ -124,12 +118,14 @@ class DeformableInfantry private: void update_ctrl_ui() { const bool ctrl_active = keyboard_.ready() && keyboard_->ctrl; - const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); - - crosshair_circle_.set_x(static_cast(std::lround( - static_cast(crosshair_base_x_) + 45.0 * reveal))); - crosshair_circle_.set_y(static_cast(std::lround( - static_cast(crosshair_base_y_) + 20.0 * reveal))); + const double reveal = ctrl_transition_.update(*timestamp_, ctrl_active); + + crosshair_circle_.set_x( + static_cast( + std::lround(static_cast(crosshair_base_x_) + 45.0 * reveal))); + crosshair_circle_.set_y( + static_cast( + std::lround(static_cast(crosshair_base_y_) + 20.0 * reveal))); } void update_time_reminder() { @@ -161,18 +157,19 @@ class DeformableInfantry return; } - std::array leg_angles; - for (size_t i = 0; i < 4; ++i) + std::array leg_angles; + for (size_t i = 0; i < kJointCount; ++i) leg_angles[i] = *joint_physical_angle_[i]; deformable_chassis_leg_arcs_.update( - *chassis_angle_, leg_angles, active_suspension_active_.ready() && *active_suspension_active_); + *chassis_angle_, leg_angles, + active_suspension_active_.ready() && *active_suspension_active_); } static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; - static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; + static constexpr double friction_wheel_speed_indicator_radius_ = 430.0; static constexpr uint16_t friction_wheel_speed_indicator_font_size_ = 20; - static constexpr double supercap_cutoff_voltage = 8.0; + static constexpr double supercap_cutoff_voltage = 8.0; static uint16_t friction_wheel_speed_indicator_center_x() { return static_cast(std::lround( @@ -197,12 +194,15 @@ class DeformableInfantry InputInterface chassis_voltage_; - static constexpr const char* kWheelName[] = { - "left_front", "left_back", "right_back", "right_front", + static constexpr size_t kJointCount = 4; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", }; - std::array, 4> wheel_velocity_; - std::array, 4> joint_physical_angle_; + std::array, kJointCount> joint_physical_angle_; InputInterface robot_bullet_allowance_; From e5a746937738e3d6a274f641b74c0e2ee246d324 Mon Sep 17 00:00:00 2001 From: Palejoker <2797572751@qq.com> Date: Tue, 7 Jul 2026 21:02:23 +0800 Subject: [PATCH 81/90] feat(gimbal): add pitch gravity feedforward --- .../config/deformable-infantry-omni-b.yaml | 2 ++ .../config/deformable-infantry-omni.yaml | 2 ++ .../deformable_infantry_gimbal_controller.cpp | 22 +++++++++++++++---- 3 files changed, 22 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 76768d83..5b11c1c9 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -132,6 +132,8 @@ gimbal_controller: pitch_velocity_kd: 0.0 pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_phase: 0.66 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 8f02e7e8..9708118b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -130,6 +130,8 @@ gimbal_controller: pitch_velocity_kd: 0.0 pitch_acc_ff_gain: 0.10 + pitch_gravity_ff_gain: 5.95 + pitch_gravity_ff_phase: 0.66 pitch_torque_control: true diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 7ccad8c7..9a2bc55d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -35,6 +35,8 @@ class DeformableInfantryGimbalController get_parameter("yaw_vel_ff_gain", yaw_vel_ff_gain_); get_parameter("yaw_acc_ff_gain", yaw_acc_ff_gain_); get_parameter("pitch_acc_ff_gain", pitch_acc_ff_gain_); + get_parameter_or("pitch_gravity_ff_gain", pitch_gravity_ff_gain_, 0.0); + get_parameter_or("pitch_gravity_ff_phase", pitch_gravity_ff_phase_, 0.0); get_parameter_or("ctrl_hold_pitch_target_angle", ctrl_hold_pitch_target_angle_, 0.0); } @@ -108,6 +110,7 @@ class DeformableInfantryGimbalController && std::isfinite(*input_.auto_aim_pitch_acc) ? pitch_acc_ff_gain_ * *input_.auto_aim_pitch_acc : 0.0; + const auto pitch_gravity_ff = pitch_gravity_feedforward(); const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); @@ -115,7 +118,8 @@ class DeformableInfantryGimbalController *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) - + pitch_acc_ff; + + pitch_acc_ff + + pitch_gravity_ff; } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -255,6 +259,13 @@ class DeformableInfantryGimbalController return gimbal_solver_.update(TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); } + auto pitch_gravity_feedforward() const -> double { + if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) + return 0.0; + else + return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); + } + auto activate_ctrl_hold() -> void { ctrl_hold_active_ = true; pitch_angle_pid_.reset(); @@ -292,7 +303,8 @@ class DeformableInfantryGimbalController if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = - pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu); + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity_imu) + + pitch_gravity_feedforward(); } else { pitch_velocity_pid_.reset(); *output_.pitch_control_velocity = pitch_velocity_ref; @@ -352,8 +364,10 @@ class DeformableInfantryGimbalController double yaw_acc_ff_gain_ = 0.0; double pitch_acc_ff_gain_ = 0.0; double ctrl_hold_pitch_target_angle_ = 0.0; - bool suspension_on_by_switch_ = false; - bool ctrl_hold_active_ = false; + double pitch_gravity_ff_gain_ = 0.0; + double pitch_gravity_ff_phase_ = 0.0; + bool suspension_on_by_switch_ = false; + bool ctrl_hold_active_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; }; From 4f1a7dc3511a35b1647e8ecfb68f9ac84a9d4f28 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Wed, 8 Jul 2026 15:46:56 +0800 Subject: [PATCH 82/90] refactor(chassis): rename mode manager and update state Co-authored-by: Copilot --- .../controller/chassis/deformable_chassis.cpp | 67 +++--- .../deformable_chassis_mode_manager.hpp | 225 ++++++++++-------- .../chassis/deformable_chassis_suspension.hpp | 19 +- .../deformable_infantry_gimbal_controller.cpp | 19 +- 4 files changed, 173 insertions(+), 157 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 395fd4d8..bf1a3c35 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include #include #include @@ -37,7 +36,7 @@ class DeformableChassis rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(10.0, 0.0, 0.0) , spin_ratio_(std::clamp(get_parameter_or("spin_ratio", 0.6), 0.0, 1.0)) - , mode_mgr_(*this) { + , joint_mode_mgr_(*this) { following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; @@ -84,11 +83,11 @@ class DeformableChassis register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); - register_output("/chassis/ctrl_hold_active", ctrl_hold_active_, false); + register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); register_output("/chassis/active_suspension/active", active_suspension_active_, false); *mode_ = rmcs_msgs::ChassisMode::AUTO; - *ctrl_hold_active_ = false; + *pitch_lock_active_ = false; chassis_control_velocity_->vector << nan_, nan_, nan_; } @@ -130,12 +129,12 @@ class DeformableChassis double rotary_knob = rotary_knob_.ready() ? *rotary_knob_ : 0.0; - mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); + joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); - const auto& mode_result = mode_mgr_.result(); - *mode_ = mode_result.mode; - *ctrl_hold_active_ = mode_result.ctrl_hold_active; - *active_suspension_active_ = mode_result.suspension_active; + const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); + *mode_ = joint_posture_state.mode; + *pitch_lock_active_ = joint_posture_state.pitch_lock_active; + *active_suspension_active_ = joint_posture_state.suspension_active; update_velocity_control(); run_joint_intent_pipeline_(); @@ -160,9 +159,9 @@ class DeformableChassis void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::AUTO; - *ctrl_hold_active_ = false; + *pitch_lock_active_ = false; - mode_mgr_.reset(); + joint_mode_mgr_.reset(); suspension_.reset(); joint_mgr_.reset(); @@ -209,7 +208,8 @@ class DeformableChassis case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { - bool forward = mode_mgr_.result().spinning_forward; + const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); + bool forward = joint_posture_state.spinning_forward; angular_velocity = spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); angular_velocity = @@ -266,26 +266,19 @@ class DeformableChassis } std::array compute_target_angles_rad_() const { - const auto& mode_result = mode_mgr_.result(); - std::array targets_deg = mode_result.joint_target_deg; + const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); + std::array targets_deg = joint_posture_state.joint_posture_target_deg; std::array targets_rad; for (size_t i = 0; i < kJointCount; ++i) targets_rad[i] = deg_to_rad(targets_deg[i]); - if (mode_result.suspension_active) { - double base_target_angle_rad = mode_result.low_prone - ? mode_mgr_.active_suspension_min_angle_rad() - : deg_to_rad(mode_result.base_angle_deg); - targets_rad.fill(base_target_angle_rad); - } - return targets_rad; } void run_joint_intent_pipeline_() { const auto current_physical_angles = read_feedback_(); - const auto& mode_result = mode_mgr_.result(); + const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); if (!joint_mgr_.init_from_feedback(current_physical_angles)) { publish_nan_joint_targets(); @@ -295,19 +288,23 @@ class DeformableChassis std::array target_angles_rad = compute_target_angles_rad_(); suspension_.calibrate( - *chassis_imu_pitch_, *chassis_imu_roll_, mode_result.symmetric_target, update_dt()); + *chassis_imu_pitch_, *chassis_imu_roll_, joint_posture_state.symmetric_posture_target, + update_dt()); *scope_motor_control_torque = suspension_.scope_torque( - mode_result.suspension_active, mode_result.mode == rmcs_msgs::ChassisMode::SPIN); + joint_posture_state.suspension_active, + joint_posture_state.mode == rmcs_msgs::ChassisMode::SPIN); auto corrections = suspension_.update( *chassis_imu_pitch_ - suspension_.pitch_offset(), *chassis_imu_roll_ - suspension_.roll_offset(), *chassis_imu_pitch_rate_, - *chassis_imu_roll_rate_, mode_result.suspension_active, mode_result.low_prone, - mode_mgr_.min_angle(), mode_mgr_.max_angle(), mode_result.base_angle_deg, - mode_mgr_.correction_inverted(), joint_mgr_.angle_states(), update_dt()); + *chassis_imu_roll_rate_, joint_posture_state.suspension_active, + joint_posture_state.ctrl_low_prone_active, joint_mode_mgr_.min_angle(), + joint_mode_mgr_.max_angle(), joint_posture_state.suspension_reference_angle_deg, + joint_mode_mgr_.correction_inverted(), joint_mgr_.angle_states(), update_dt()); - joint_mgr_.run_trajectory(target_angles_rad, mode_result.suspension_active, update_dt()); + joint_mgr_.run_trajectory( + target_angles_rad, joint_posture_state.suspension_active, update_dt()); publish_joint_targets_(current_physical_angles, corrections); } @@ -316,8 +313,8 @@ class DeformableChassis const std::array& feedback_angles, const DeformableChassisActiveSuspension::Corrections& corrections) { - double min_angle_rad = mode_mgr_.active_suspension_min_angle_rad(); - double max_angle_rad = mode_mgr_.max_angle_rad(); + double min_angle_rad = joint_mode_mgr_.active_suspension_min_angle_rad(); + double max_angle_rad = joint_mode_mgr_.max_angle_rad(); if (!joint_mgr_.any_active()) { publish_nan_joint_targets(); @@ -326,10 +323,10 @@ class DeformableChassis for (size_t i = 0; i < kJointCount; ++i) { if (!joint_mgr_.joint_active(i)) { - *joint_target_angle_[i] = nan_; - *joint_target_velocity_[i] = nan_; + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; *joint_target_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; + *joint_angle_error_[i] = nan_; continue; } @@ -376,7 +373,7 @@ class DeformableChassis OutputInterface mode_; OutputInterface chassis_control_velocity_; - OutputInterface ctrl_hold_active_; + OutputInterface pitch_lock_active_; OutputInterface active_suspension_active_; InputInterface chassis_imu_pitch_; @@ -395,7 +392,7 @@ class DeformableChassis pid::PidCalculator following_velocity_controller_; const double spin_ratio_; - DeformableChassisModeManager mode_mgr_; + DeformableChassisModeManager joint_mode_mgr_; DeformableChassisActiveSuspension suspension_; DeformableChassisJointManager joint_mgr_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp index 82d3a07a..2dba6501 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp @@ -14,53 +14,50 @@ namespace rmcs_core::controller::chassis { class DeformableChassisModeManager { public: - struct Result { - rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; - bool ctrl_hold_active = false; - bool suspension_active = false; - bool low_prone = false; - bool symmetric_target = true; - bool spinning_forward = true; - std::array joint_target_deg = {58.0, 58.0, 58.0, 58.0}; - double base_angle_deg = 58.0; + struct JointPostureState { + rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; + bool ctrl_low_prone_active = false; + bool pitch_lock_active = false; + bool suspension_active = false; + bool symmetric_posture_target = true; + bool spinning_forward = true; + std::array joint_posture_target_deg = {58.0, 58.0, 58.0, 58.0}; + double suspension_reference_angle_deg = 58.0; }; explicit DeformableChassisModeManager(rclcpp::Node& node) : min_angle_(node.get_parameter_or("min_angle", 7.0)) , max_angle_(node.get_parameter_or("max_angle", 58.0)) - , base_angle_deg_( - std::clamp( - node.get_parameter_or("active_suspension_base_angle", max_angle_), - min_angle_ - 5.0, max_angle_)) , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { - result_.base_angle_deg = base_angle_deg_; - result_.joint_target_deg.fill(max_angle_); current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); + update_joint_posture_state_(false); } void reset() { - result_.mode = rmcs_msgs::ChassisMode::AUTO; - result_.ctrl_hold_active = false; - result_.suspension_active = false; - result_.low_prone = false; - result_.symmetric_target = true; - result_.spinning_forward = true; - result_.joint_target_deg.fill(max_angle_); - result_.base_angle_deg = max_angle_; + joint_posture_state_.mode = rmcs_msgs::ChassisMode::AUTO; + joint_posture_state_.ctrl_low_prone_active = false; + joint_posture_state_.pitch_lock_active = false; + joint_posture_state_.suspension_active = false; + joint_posture_state_.symmetric_posture_target = true; + joint_posture_state_.spinning_forward = true; + joint_posture_state_.joint_posture_target_deg.fill(max_angle_); + joint_posture_state_.suspension_reference_angle_deg = max_angle_; current_target_angle_ = max_angle_; - base_angle_deg_ = max_angle_; joint_current_target_angle_.fill(max_angle_); - apply_symmetric_target_ = true; - complex_spin_active_ = false; - complex_spin_elapsed_ = 0.0; - suspension_on_by_dr16_switch_ = false; + apply_symmetric_target_ = true; + complex_spin_active_ = false; + complex_spin_elapsed_ = 0.0; + suspension_enabled_by_toggle_ = false; + suspension_toggle_left_down_pending_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - last_keyboard_ = rmcs_msgs::Keyboard::zero(); - last_rotary_knob_ = 0.0; + last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + last_keyboard_ = rmcs_msgs::Keyboard::zero(); + last_rotary_knob_ = 0.0; + + update_joint_posture_state_(false); } void update( @@ -69,24 +66,20 @@ class DeformableChassisModeManager { update_mode_from_inputs_(switch_left, switch_right, keyboard); update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); - - result_.low_prone = keyboard.ctrl; - result_.ctrl_hold_active = keyboard.ctrl || suspension_on_by_dr16_switch_; - result_.suspension_active = suspension_enable_ && result_.ctrl_hold_active; - - result_.symmetric_target = symmetric_joint_target_requested_(); - update_lift_target_toggle_(keyboard, rotary_knob, dt); - result_.joint_target_deg = joint_current_target_angle_; - result_.base_angle_deg = base_angle_deg_; + joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; + joint_posture_state_.pitch_lock_active = keyboard.ctrl; + joint_posture_state_.suspension_active = + suspension_enable_ && (keyboard.ctrl || suspension_enabled_by_toggle_); + update_joint_posture_state_(keyboard.ctrl); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } - const Result& result() const { return result_; } + const JointPostureState& joint_posture_state() const { return joint_posture_state_; } double min_angle() const { return min_angle_; } double max_angle() const { return max_angle_; } @@ -96,7 +89,7 @@ class DeformableChassisModeManager { bool correction_inverted() const { double midpoint = (min_angle_ - 5.0 + max_angle_) / 2.0; - return base_angle_deg_ > midpoint; + return joint_posture_state_.suspension_reference_angle_deg > midpoint; } private: @@ -106,10 +99,10 @@ class DeformableChassisModeManager { rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { - auto mode = result_.mode; + auto mode = joint_posture_state_.mode; if (switch_left == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); - result_.mode = mode; + joint_posture_state_.mode = mode; return; } @@ -119,16 +112,16 @@ class DeformableChassisModeManager { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; - result_.spinning_forward = !result_.spinning_forward; + mode = rmcs_msgs::ChassisMode::SPIN; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.c && keyboard.c) { deactivate_complex_spin_(); if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; - result_.spinning_forward = !result_.spinning_forward; + mode = rmcs_msgs::ChassisMode::SPIN; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.z && keyboard.z) { deactivate_complex_spin_(); @@ -139,38 +132,38 @@ class DeformableChassisModeManager { if (complex_spin_active_) mode = rmcs_msgs::ChassisMode::SPIN; - result_.mode = mode; + joint_posture_state_.mode = mode; } void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { - complex_spin_active_ = true; - complex_spin_elapsed_ = 0.0; + complex_spin_active_ = true; + complex_spin_elapsed_ = 0.0; apply_symmetric_target_ = true; if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - result_.spinning_forward = !result_.spinning_forward; + mode = rmcs_msgs::ChassisMode::SPIN; + joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } void deactivate_complex_spin_() { - complex_spin_active_ = false; + complex_spin_active_ = false; complex_spin_elapsed_ = 0.0; } void apply_front_high_rear_low_target_() { - joint_current_target_angle_[kLeftFront] = max_angle_; + joint_current_target_angle_[kLeftFront] = max_angle_; joint_current_target_angle_[kRightFront] = max_angle_; - joint_current_target_angle_[kLeftBack] = min_angle_; - joint_current_target_angle_[kRightBack] = min_angle_; - apply_symmetric_target_ = false; + joint_current_target_angle_[kLeftBack] = min_angle_; + joint_current_target_angle_[kRightBack] = min_angle_; + apply_symmetric_target_ = false; } void apply_front_low_rear_high_target_() { - joint_current_target_angle_[kLeftFront] = min_angle_; + joint_current_target_angle_[kLeftFront] = min_angle_; joint_current_target_angle_[kRightFront] = min_angle_; - joint_current_target_angle_[kLeftBack] = max_angle_; - joint_current_target_angle_[kRightBack] = max_angle_; - apply_symmetric_target_ = false; + joint_current_target_angle_[kLeftBack] = max_angle_; + joint_current_target_angle_[kRightBack] = max_angle_; + apply_symmetric_target_ = false; } void toggle_bg_target_() { @@ -181,30 +174,39 @@ class DeformableChassisModeManager { } void update_suspension_toggle_from_inputs_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard&) { - if (suspension_toggle_requested_by_switch_(switch_left, switch_right)) - suspension_on_by_dr16_switch_ = !suspension_on_by_dr16_switch_; + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) { + const bool keyboard_toggle_requested = !last_keyboard_.e && keyboard.e; + if (keyboard_toggle_requested + || suspension_toggle_requested_by_switch_(switch_left, switch_right)) + suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; } bool suspension_toggle_requested_by_switch_( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) const { - return switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP - && last_switch_right_ == rmcs_msgs::Switch::MIDDLE; - } + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { + if (last_switch_left_ != rmcs_msgs::Switch::DOWN && switch_left == rmcs_msgs::Switch::DOWN) + suspension_toggle_left_down_pending_ = true; + + const bool right_entered_up = + last_switch_right_ != rmcs_msgs::Switch::UP && switch_right == rmcs_msgs::Switch::UP; + if (suspension_toggle_left_down_pending_ && switch_left == rmcs_msgs::Switch::DOWN + && right_entered_up) { + suspension_toggle_left_down_pending_ = false; + return true; + } - bool symmetric_joint_target_requested_() const { - constexpr double epsilon = 1e-6; - return std::all_of( - joint_current_target_angle_.begin() + 1, joint_current_target_angle_.end(), - [&](double v) { return std::abs(v - joint_current_target_angle_.front()) <= epsilon; }); + if (switch_left != rmcs_msgs::Switch::DOWN) + suspension_toggle_left_down_pending_ = false; + + return false; } void update_lift_target_toggle_( const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { constexpr double rotary_knob_symmetric_edge_threshold = 0.7; - constexpr double rotary_knob_bg_edge_threshold = -0.9; - constexpr double complex_spin_toggle_period = 0.5; + constexpr double rotary_knob_bg_edge_threshold = -0.9; + constexpr double complex_spin_toggle_period = 0.5; const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; @@ -235,14 +237,10 @@ class DeformableChassisModeManager { if (rotary_knob_toggle_condition || keyboard_toggle_condition || complex_spin_toggle_condition) { - if (result_.suspension_active && keyboard_toggle_condition) { - base_angle_deg_ = - (std::abs(base_angle_deg_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; - } else { - current_target_angle_ = - (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; - apply_symmetric_target_ = true; - } + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); } else if (rotary_knob_bg_toggle_condition) { toggle_bg_target_(); } else if (front_high_rear_low) { @@ -254,30 +252,61 @@ class DeformableChassisModeManager { last_rotary_knob_ = rotary_knob; } - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; static constexpr size_t kRightFront = 3; static constexpr size_t kJointCount = 4; - Result result_; + void update_joint_posture_state_(bool ctrl_low_prone_active) { + std::array effective_joint_posture_target_deg = + joint_current_target_angle_; + if (ctrl_low_prone_active) + effective_joint_posture_target_deg.fill(min_angle_ - 5.0); + + joint_posture_state_.joint_posture_target_deg = effective_joint_posture_target_deg; + joint_posture_state_.symmetric_posture_target = + symmetric_joint_target_requested_(effective_joint_posture_target_deg); + + if (joint_posture_state_.symmetric_posture_target) { + joint_posture_state_.suspension_reference_angle_deg = + effective_joint_posture_target_deg.front(); + return; + } + + double posture_angle_sum = 0.0; + for (double angle_deg : effective_joint_posture_target_deg) + posture_angle_sum += angle_deg; + joint_posture_state_.suspension_reference_angle_deg = + posture_angle_sum / static_cast(kJointCount); + } + + static bool + symmetric_joint_target_requested_(const std::array& joint_target_deg) { + constexpr double epsilon = 1e-6; + return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { + return std::abs(v - joint_target_deg.front()) <= epsilon; + }); + } + + JointPostureState joint_posture_state_; double min_angle_; double max_angle_; - double base_angle_deg_; bool suspension_enable_; double current_target_angle_; std::array joint_current_target_angle_; - bool apply_symmetric_target_ = true; - bool complex_spin_active_ = false; - double complex_spin_elapsed_ = 0.0; - bool suspension_on_by_dr16_switch_ = false; + bool apply_symmetric_target_ = true; + bool complex_spin_active_ = false; + double complex_spin_elapsed_ = 0.0; + bool suspension_enabled_by_toggle_ = false; + bool suspension_toggle_left_down_pending_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - double last_rotary_knob_ = 0.0; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + double last_rotary_knob_ = 0.0; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp index c14fd014..7e6af68c 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp @@ -102,7 +102,8 @@ class DeformableChassisActiveSuspension { Corrections update( double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, - bool low_prone, double min_angle_deg, double max_angle_deg, double base_angle_deg, + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, bool correction_inverted, const std::array& base_joint_angles, double dt) { Corrections corrections; @@ -110,8 +111,8 @@ class DeformableChassisActiveSuspension { if (!suspension_active) { reset_attitude_(); run_correction_trajectory_( - low_prone, min_angle_deg, max_angle_deg, base_angle_deg, base_joint_angles, dt, - corrections); + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, dt, corrections); return corrections; } @@ -131,8 +132,8 @@ class DeformableChassisActiveSuspension { compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); run_correction_trajectory_( - low_prone, min_angle_deg, max_angle_deg, base_angle_deg, base_joint_angles, dt, - corrections); + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, dt, corrections); return corrections; } @@ -211,8 +212,9 @@ class DeformableChassisActiveSuspension { } void run_correction_trajectory_( - bool low_prone, double min_angle_deg, double max_angle_deg, double base_angle_deg, - const std::array& base_joint_angles, double dt, Corrections& corrections) { + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, const std::array& base_joint_angles, double dt, + Corrections& corrections) { double max_target_rad = deg_to_rad_(max_angle_deg); double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); @@ -220,7 +222,8 @@ class DeformableChassisActiveSuspension { for (size_t i = 0; i < kJointCount; ++i) { double base_angle = std::isfinite(base_joint_angles[i]) ? base_joint_angles[i] - : (low_prone ? min_susp_rad : deg_to_rad_(base_angle_deg)); + : (low_prone_override_active ? min_susp_rad + : deg_to_rad_(base_angle_deg)); double correction_min = min_susp_rad - base_angle; double correction_max = max_target_rad - base_angle; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 9a2bc55d..ef554b02 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -47,14 +47,10 @@ class DeformableInfantryGimbalController using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - suspension_on_by_switch_ = false; - last_switch_right_ = switch_right; reset_all_controls(); return; } - update_ctrl_hold_request_state(switch_left, switch_right); - if (ctrl_hold_requested()) { update_ctrl_hold_control(); } else { @@ -151,6 +147,7 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse/velocity", mouse_velocity); component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); + component.register_input("/chassis/pitch_lock_active", pitch_lock_active, false); component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); @@ -176,6 +173,7 @@ class DeformableInfantryGimbalController InputInterface mouse_velocity; InputInterface mouse; InputInterface update_rate; + InputInterface pitch_lock_active; InputInterface yaw_angle; InputInterface yaw_velocity; @@ -214,16 +212,7 @@ class DeformableInfantryGimbalController } output_{*this}; auto ctrl_hold_requested() const -> bool { - return (input_.keyboard.ready() && input_.keyboard->ctrl) || suspension_on_by_switch_; - } - - auto update_ctrl_hold_request_state( - rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) -> void { - if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP - && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { - suspension_on_by_switch_ = !suspension_on_by_switch_; - } - last_switch_right_ = switch_right; + return input_.pitch_lock_active.ready() && *input_.pitch_lock_active; } auto update_dt() const -> double { @@ -366,9 +355,7 @@ class DeformableInfantryGimbalController double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; - bool suspension_on_by_switch_ = false; bool ctrl_hold_active_ = false; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; }; } // namespace rmcs_core::controller::gimbal From 610dea34cac0df4c34a9807d8ba5218c318eed03 Mon Sep 17 00:00:00 2001 From: eye-on Date: Wed, 8 Jul 2026 16:17:07 +0800 Subject: [PATCH 83/90] feat(hardware): connect auto-aim interfaces to deformable infantry - Output /auto_aim/camera_transform, /auto_aim/barrel_direction, /auto_aim/yaw_velocity from hardware layer - Expose gimbal_yaw_velocity() accessor on TopBoard - Remove standalone auto-aim launch (now runs as Component in RMCS) - Bump rmcs_auto_aim_v2 submodule (single-process architecture, fire controller & tracker refactor, dynamic yaw limits) --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py | 16 +--------------- .../src/hardware/deformable-infantry-omni-b.cpp | 17 +++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 17 +++++++++++++++++ 4 files changed, 36 insertions(+), 16 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..06d7e380 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit 06d7e380ebc039ad3a708cce293febc25e5ded76 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index d810a08d..8246f06a 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -6,8 +6,7 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import IncludeLaunchDescription,LogInfo -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -51,19 +50,6 @@ def visit( ) ) - if is_automatic: - pass - - entities.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' - ]) - ) - ) - - - return entities diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index 25d8fe1e..fe906fe9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -52,6 +52,10 @@ class DeformableInfantryOmniB register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -91,6 +95,12 @@ class DeformableInfantryOmniB imu_board_->update(); vt13_.update_status(); remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -723,6 +733,10 @@ class DeformableInfantryOmniB ~TopBoard() override = default; + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + void request_hard_sync_read() { // RMCS-lite top board variant currently has no GPIO hard-sync request // path. @@ -848,6 +862,9 @@ class DeformableInfantryOmniB } OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; InputInterface timestamp_; device::Vt13 vt13_; std::atomic hard_sync_pending_{false}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 67044424..78805893 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -48,6 +48,10 @@ class DeformableInfantryOmni register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -78,6 +82,12 @@ class DeformableInfantryOmni imu_board_->update(); vt13_.update_status(); remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -675,6 +685,10 @@ class DeformableInfantryOmni ~TopBoard() override = default; + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + void request_hard_sync_read() { // RMCS-lite top board variant currently has no GPIO hard-sync request // path. @@ -837,6 +851,9 @@ class DeformableInfantryOmni } OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; InputInterface timestamp_; device::Vt13 vt13_; From ae839bce164ecf1d4b54362fd65d1e9c31468c97 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 9 Jul 2026 04:13:48 +0800 Subject: [PATCH 84/90] Squashed commit of the following: commit e95abf726b239c5a76bdd4ce887e81083d1407ff Author: ZGZ713912 Date: Thu Jul 9 04:10:12 2026 +0800 refactor(chassis): simplify suspension toggle logic commit bef37297935e300081592296725ca021266d8b05 Author: ZGZ713912 Date: Thu Jul 9 02:57:18 2026 +0800 feat(controller): add deformable suspension component commit 5101024d991e1a1706b0cf7d37f50f391c8a0512 Author: ZGZ713912 Date: Wed Jul 8 23:12:09 2026 +0800 feat(chassis): enhance passive suspension parameters commit 5c483b6f5ef5cb3efe334d004f054db37655897e Author: ZGZ713912 Date: Wed Jul 8 22:38:45 2026 +0800 feat(chassis): add passive suspension parameters --- .../config/deformable-infantry-omni-b.yaml | 22 + .../config/deformable-infantry-omni.yaml | 22 + rmcs_ws/src/rmcs_core/plugins.xml | 2 +- .../controller/chassis/deformable_chassis.cpp | 262 ++---- .../deformable_chassis_joint_manager.hpp | 137 ---- .../chassis/deformable_chassis_suspension.hpp | 302 ------- .../chassis/deformable_joint_controller.cpp | 52 +- ...s_mode_manager.hpp => deformable_mode.hpp} | 239 ++++-- .../chassis/deformable_suspension.cpp | 746 ++++++++++++++++++ .../deformable_infantry_gimbal_controller.cpp | 28 +- 10 files changed, 1066 insertions(+), 746 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp rename rmcs_ws/src/rmcs_core/src/controller/chassis/{deformable_chassis_mode_manager.hpp => deformable_mode.hpp} (56%) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 5b11c1c9..e2b5b5bd 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -60,8 +61,29 @@ chassis_controller: min_angle: 8.0 max_angle: 58.0 active_suspension_enable: true + passive_suspension_enable: true spin_ratio: 1.0 +deformable_suspension: + ros__parameters: + passive_suspension_enable: true + passive_suspension_kp: 0.035 + passive_suspension_ki: 0.005 + passive_suspension_deadband: 0.08 + passive_suspension_load_lpf_alpha: 0.08 + passive_suspension_integral_limit: 3.0 + passive_suspension_max_angle_correction_deg: 8.0 + passive_suspension_correction_velocity_limit_deg: 180.0 + passive_suspension_correction_acceleration_limit_deg: 720.0 + passive_suspension_load_sign_left_front: -1.0 + passive_suspension_load_sign_left_back: -1.0 + passive_suspension_load_sign_right_back: -1.0 + passive_suspension_load_sign_right_front: -1.0 + passive_suspension_load_bias_left_front: 0.0 + passive_suspension_load_bias_left_back: 0.0 + passive_suspension_load_bias_right_back: 0.0 + passive_suspension_load_bias_right_front: 0.0 + # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 8.0 active_suspension_pitch_outer_ki: 0.02 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 9708118b..8a90a717 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller + - rmcs_core::controller::chassis::DeformableSuspension -> deformable_suspension - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::DeformableOmniWheelController -> deformable_chassis_controller @@ -58,8 +59,29 @@ chassis_controller: min_angle: 12.0 max_angle: 50.0 active_suspension_enable: true + passive_suspension_enable: true spin_ratio: 1.0 +deformable_suspension: + ros__parameters: + passive_suspension_enable: true + passive_suspension_kp: 0.035 + passive_suspension_ki: 0.005 + passive_suspension_deadband: 0.08 + passive_suspension_load_lpf_alpha: 0.08 + passive_suspension_integral_limit: 3.0 + passive_suspension_max_angle_correction_deg: 8.0 + passive_suspension_correction_velocity_limit_deg: 180.0 + passive_suspension_correction_acceleration_limit_deg: 720.0 + passive_suspension_load_sign_left_front: -1.0 + passive_suspension_load_sign_left_back: -1.0 + passive_suspension_load_sign_right_back: -1.0 + passive_suspension_load_sign_right_front: -1.0 + passive_suspension_load_bias_left_front: 0.0 + passive_suspension_load_bias_left_back: 0.0 + passive_suspension_load_bias_right_back: 0.0 + passive_suspension_load_bias_right_front: 0.0 + # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 08264072..8a2c5858 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -18,8 +18,8 @@ + - diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index bf1a3c35..fc6f25fc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -1,10 +1,8 @@ #include #include #include -#include #include #include -#include #include #include @@ -18,19 +16,16 @@ #include #include +#include "controller/chassis/deformable_mode.hpp" #include "controller/pid/pid_calculator.hpp" -#include "deformable_chassis_joint_manager.hpp" -#include "deformable_chassis_mode_manager.hpp" -#include "deformable_chassis_suspension.hpp" - namespace rmcs_core::controller::chassis { class DeformableChassis : public rmcs_executor::Component , public rclcpp::Node { public: - DeformableChassis() + explicit DeformableChassis() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) @@ -41,9 +36,6 @@ class DeformableChassis following_velocity_controller_.output_max = angular_velocity_max_; following_velocity_controller_.output_min = -angular_velocity_max_; - suspension_.configure(*this); - joint_mgr_.configure(*this); - register_input("/remote/joystick/right", joystick_right_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); @@ -54,40 +46,35 @@ class DeformableChassis register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); - register_input("/chassis/imu/roll", chassis_imu_roll_, false); - register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); - register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); - - for (size_t i = 0; i < kJointCount; ++i) { - register_input( - fmt::format("/chassis/{}_joint/physical_angle", kJointName[i]), - joint_physical_angle_[i], false); - register_output( - fmt::format("/chassis/{}_joint/target_physical_angle", kJointName[i]), - joint_target_angle_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/target_physical_velocity", kJointName[i]), - joint_target_velocity_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/target_physical_acceleration", kJointName[i]), - joint_target_acceleration_[i], nan_); - register_output( - fmt::format("/chassis/{}_joint/control_angle_error", kJointName[i]), - joint_angle_error_[i], nan_); - } - - register_output("/gimbal/scope/control_torque", scope_motor_control_torque, nan_); - register_output("/chassis/angle", chassis_angle_, nan_); register_output("/chassis/control_angle", chassis_control_angle_, nan_); register_output("/chassis/control_mode", mode_); register_output("/chassis/control_velocity", chassis_control_velocity_); register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); register_output("/chassis/active_suspension/active", active_suspension_active_, false); + register_output("/chassis/deformable/passive_suspension_active", passive_suspension_active_, false); + register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); + register_output("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); + register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); + register_output("/chassis/deformable/min_angle_deg", min_angle_deg_, joint_mode_mgr_.min_angle()); + register_output("/chassis/deformable/max_angle_deg", max_angle_deg_, joint_mode_mgr_.max_angle()); + register_output( + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_, joint_mode_mgr_.suspension_reference_angle_deg()); + register_output("/chassis/deformable/reset_count", deformable_reset_count_, static_cast(0)); + for (size_t i = 0; i < kJointCount; ++i) { + register_output( + fmt::format("/chassis/deformable/{}_joint/posture_target_angle", kJointName[i]), + joint_posture_target_angle_rad_[i], deg_to_rad(joint_mode_mgr_.max_angle())); + } - *mode_ = rmcs_msgs::ChassisMode::AUTO; + *mode_ = rmcs_msgs::ChassisMode::AUTO; *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *passive_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; chassis_control_velocity_->vector << nan_, nan_, nan_; } @@ -102,23 +89,14 @@ class DeformableChassis get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". " "Set to 0.0."); } - if (!chassis_imu_pitch_.ready()) - chassis_imu_pitch_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_.ready()) - chassis_imu_roll_.make_and_bind_directly(0.0); - if (!chassis_imu_pitch_rate_.ready()) - chassis_imu_pitch_rate_.make_and_bind_directly(0.0); - if (!chassis_imu_roll_rate_.ready()) - chassis_imu_roll_rate_.make_and_bind_directly(0.0); - validate_joint_feedback_inputs(); } void update() override { using rmcs_msgs::Switch; const auto switch_right = *switch_right_; - const auto switch_left = *switch_left_; - const auto keyboard = *keyboard_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -131,47 +109,48 @@ class DeformableChassis joint_mode_mgr_.update(switch_left, switch_right, keyboard, rotary_knob, update_dt()); - const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); - *mode_ = joint_posture_state.mode; - *pitch_lock_active_ = joint_posture_state.pitch_lock_active; - *active_suspension_active_ = joint_posture_state.suspension_active; + *mode_ = joint_mode_mgr_.mode(); + *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); + *active_suspension_active_ = joint_mode_mgr_.suspension_active(); + *passive_suspension_active_ = joint_mode_mgr_.passive_suspension_active(); + *low_prone_active_ = joint_mode_mgr_.low_prone_active(); + *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); + *correction_inverted_ = joint_mode_mgr_.correction_inverted(); + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); update_velocity_control(); - run_joint_intent_pipeline_(); } while (false); } private: - static constexpr size_t kJointCount = 4; - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double translational_velocity_max_ = 10.0; - static constexpr double angular_velocity_max_ = 30.0; - static constexpr double default_dt_ = 1e-3; - - void validate_joint_feedback_inputs() const { - for (size_t i = 0; i < kJointCount; ++i) - if (!joint_physical_angle_[i].ready()) - throw std::runtime_error( - "missing deformable chassis feedback " - "interfaces: expected " - "/chassis/*_joint/physical_angle"); - } + static constexpr double angular_velocity_max_ = 30.0; + static constexpr double default_dt_ = 1e-3; void reset_all_controls() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; - *pitch_lock_active_ = false; - joint_mode_mgr_.reset(); - suspension_.reset(); - joint_mgr_.reset(); + *deformable_reset_count_ += 1; + + *mode_ = rmcs_msgs::ChassisMode::AUTO; + *pitch_lock_active_ = false; + *active_suspension_active_ = false; + *passive_suspension_active_ = false; + *low_prone_active_ = false; + *symmetric_posture_target_ = true; + *correction_inverted_ = false; + *min_angle_deg_ = joint_mode_mgr_.min_angle(); + *max_angle_deg_ = joint_mode_mgr_.max_angle(); + *suspension_reference_angle_deg_ = joint_mode_mgr_.suspension_reference_angle_deg(); + publish_joint_posture_targets_(); chassis_control_velocity_->vector << nan_, nan_, nan_; - *chassis_angle_ = nan_; + *chassis_angle_ = nan_; *chassis_control_angle_ = nan_; - - *scope_motor_control_torque = nan_; - - publish_nan_joint_targets(); } double update_dt() const { @@ -182,7 +161,7 @@ class DeformableChassis void update_velocity_control() { Eigen::Vector2d translational_velocity = update_translational_velocity_control(); - double angular_velocity = update_angular_velocity_control(); + double angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } @@ -201,15 +180,14 @@ class DeformableChassis } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan_; switch (*mode_) { case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { - const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); - bool forward = joint_posture_state.spinning_forward; + bool forward = joint_mode_mgr_.spinning_forward(); angular_velocity = spin_ratio_ * (forward ? angular_velocity_max_ : -angular_velocity_max_); angular_velocity = @@ -234,7 +212,7 @@ class DeformableChassis default: break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -254,104 +232,12 @@ class DeformableChassis static double deg_to_rad(double deg) { return deg * std::numbers::pi / 180.0; } - std::array read_feedback_() const { - std::array angles; - angles.fill(nan_); - - for (size_t i = 0; i < kJointCount; ++i) - if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) - angles[i] = *joint_physical_angle_[i]; - - return angles; - } - - std::array compute_target_angles_rad_() const { - const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); - std::array targets_deg = joint_posture_state.joint_posture_target_deg; + void publish_joint_posture_targets_() { + std::array targets_deg{}; + joint_mode_mgr_.copy_joint_posture_target_deg(targets_deg); - std::array targets_rad; for (size_t i = 0; i < kJointCount; ++i) - targets_rad[i] = deg_to_rad(targets_deg[i]); - - return targets_rad; - } - - void run_joint_intent_pipeline_() { - const auto current_physical_angles = read_feedback_(); - const auto& joint_posture_state = joint_mode_mgr_.joint_posture_state(); - - if (!joint_mgr_.init_from_feedback(current_physical_angles)) { - publish_nan_joint_targets(); - return; - } - - std::array target_angles_rad = compute_target_angles_rad_(); - - suspension_.calibrate( - *chassis_imu_pitch_, *chassis_imu_roll_, joint_posture_state.symmetric_posture_target, - update_dt()); - - *scope_motor_control_torque = suspension_.scope_torque( - joint_posture_state.suspension_active, - joint_posture_state.mode == rmcs_msgs::ChassisMode::SPIN); - - auto corrections = suspension_.update( - *chassis_imu_pitch_ - suspension_.pitch_offset(), - *chassis_imu_roll_ - suspension_.roll_offset(), *chassis_imu_pitch_rate_, - *chassis_imu_roll_rate_, joint_posture_state.suspension_active, - joint_posture_state.ctrl_low_prone_active, joint_mode_mgr_.min_angle(), - joint_mode_mgr_.max_angle(), joint_posture_state.suspension_reference_angle_deg, - joint_mode_mgr_.correction_inverted(), joint_mgr_.angle_states(), update_dt()); - - joint_mgr_.run_trajectory( - target_angles_rad, joint_posture_state.suspension_active, update_dt()); - - publish_joint_targets_(current_physical_angles, corrections); - } - - void publish_joint_targets_( - const std::array& feedback_angles, - const DeformableChassisActiveSuspension::Corrections& corrections) { - - double min_angle_rad = joint_mode_mgr_.active_suspension_min_angle_rad(); - double max_angle_rad = joint_mode_mgr_.max_angle_rad(); - - if (!joint_mgr_.any_active()) { - publish_nan_joint_targets(); - return; - } - - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_mgr_.joint_active(i)) { - *joint_target_angle_[i] = nan_; - *joint_target_velocity_[i] = nan_; - *joint_target_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; - continue; - } - - double target = joint_mgr_.angle_state(i) + corrections.joint_angle_correction[i]; - *joint_target_angle_[i] = std::clamp(target, min_angle_rad, max_angle_rad); - *joint_target_velocity_[i] = - joint_mgr_.velocity_state(i) + corrections.joint_velocity_correction[i]; - *joint_target_acceleration_[i] = - joint_mgr_.acceleration_state(i) + corrections.joint_acceleration_correction[i]; - *joint_angle_error_[i] = std::isfinite(feedback_angles[i]) - ? feedback_angles[i] - *joint_target_angle_[i] - : nan_; - } - } - - void publish_nan_joint_targets() { - suspension_.reset(); - joint_mgr_.reset(); - - for (size_t i = 0; i < kJointCount; ++i) { - *joint_target_angle_[i] = nan_; - *joint_target_velocity_[i] = nan_; - *joint_target_acceleration_[i] = nan_; - *joint_angle_error_[i] = nan_; - } + *joint_posture_target_angle_rad_[i] = deg_to_rad(targets_deg[i]); } static constexpr const char* kJointName[] = { @@ -375,26 +261,20 @@ class DeformableChassis OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; OutputInterface active_suspension_active_; - - InputInterface chassis_imu_pitch_; - InputInterface chassis_imu_roll_; - InputInterface chassis_imu_pitch_rate_; - InputInterface chassis_imu_roll_rate_; - - OutputInterface scope_motor_control_torque; - - std::array, kJointCount> joint_physical_angle_; - std::array, kJointCount> joint_target_angle_; - std::array, kJointCount> joint_target_velocity_; - std::array, kJointCount> joint_target_acceleration_; - std::array, kJointCount> joint_angle_error_; + OutputInterface passive_suspension_active_; + OutputInterface low_prone_active_; + OutputInterface symmetric_posture_target_; + OutputInterface correction_inverted_; + OutputInterface min_angle_deg_; + OutputInterface max_angle_deg_; + OutputInterface suspension_reference_angle_deg_; + OutputInterface deformable_reset_count_; + std::array, kJointCount> joint_posture_target_angle_rad_; pid::PidCalculator following_velocity_controller_; const double spin_ratio_; DeformableChassisModeManager joint_mode_mgr_; - DeformableChassisActiveSuspension suspension_; - DeformableChassisJointManager joint_mgr_; }; } // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp deleted file mode 100644 index 953fa60d..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_joint_manager.hpp +++ /dev/null @@ -1,137 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -namespace rmcs_core::controller::chassis { - -class DeformableChassisJointManager { -public: - static constexpr size_t kJointCount = 4; - - DeformableChassisJointManager() = default; - - void configure(rclcpp::Node& node) { - target_vel_limit_ = std::max( - deg_to_rad_(std::abs(node.get_parameter_or("target_physical_velocity_limit", 180.0))), - 1e-6); - target_acc_limit_ = std::max( - deg_to_rad_( - std::abs(node.get_parameter_or("target_physical_acceleration_limit", 720.0))), - 1e-6); - suspension_vel_limit_ = std::max( - deg_to_rad_( - std::abs(node.get_parameter_or( - "active_suspension_target_velocity_limit_deg", - node.get_parameter_or("target_physical_velocity_limit", 180.0)))), - 1e-6); - suspension_acc_limit_ = std::max( - deg_to_rad_( - std::abs(node.get_parameter_or( - "active_suspension_target_acceleration_limit_deg", - node.get_parameter_or("target_physical_acceleration_limit", 720.0)))), - 1e-6); - } - - void reset() { - joint_target_active_.fill(false); - target_angle_state_rad_.fill(nan_); - target_velocity_state_rad_.fill(0.0); - target_acceleration_state_rad_.fill(0.0); - } - - bool init_from_feedback(const std::array& physical_angles) { - - bool any_active = false; - for (size_t i = 0; i < kJointCount; ++i) { - if (std::isfinite(physical_angles[i]) && !joint_target_active_[i]) { - target_angle_state_rad_[i] = physical_angles[i]; - target_velocity_state_rad_[i] = 0.0; - target_acceleration_state_rad_[i] = 0.0; - joint_target_active_[i] = true; - } - any_active = any_active || joint_target_active_[i]; - } - return any_active; - } - - bool any_active() const { - for (size_t i = 0; i < kJointCount; ++i) - if (joint_target_active_[i]) - return true; - return false; - } - - void run_trajectory( - const std::array& target_angles_rad, bool suspension_active, - double dt) { - - for (size_t i = 0; i < kJointCount; ++i) { - if (!joint_target_active_[i]) - continue; - - double& angle_state = target_angle_state_rad_[i]; - double& velocity_state = target_velocity_state_rad_[i]; - double& acceleration_state = target_acceleration_state_rad_[i]; - double target = target_angles_rad[i]; - - double vel_limit = suspension_active ? suspension_vel_limit_ : target_vel_limit_; - double acc_limit = suspension_active ? suspension_acc_limit_ : target_acc_limit_; - - if (!std::isfinite(target) || !std::isfinite(angle_state)) - continue; - - double position_error = target - angle_state; - double stopping_distance = velocity_state * velocity_state / (2.0 * acc_limit); - - double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(vel_limit, position_error); - } - - double velocity_error = desired_velocity - velocity_state; - acceleration_state = std::clamp(velocity_error / dt, -acc_limit, acc_limit); - - velocity_state += acceleration_state * dt; - velocity_state = std::clamp(velocity_state, -vel_limit, vel_limit); - angle_state += velocity_state * dt; - - double next_error = target - angle_state; - if ((position_error > 0.0 && next_error < 0.0) - || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { - angle_state = target; - velocity_state = 0.0; - acceleration_state = 0.0; - } - } - } - - double angle_state(size_t i) const { return target_angle_state_rad_[i]; } - double velocity_state(size_t i) const { return target_velocity_state_rad_[i]; } - double acceleration_state(size_t i) const { return target_acceleration_state_rad_[i]; } - bool joint_active(size_t i) const { return joint_target_active_[i]; } - - std::array angle_states() const { return target_angle_state_rad_; } - -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } - - std::array joint_target_active_ = {false, false, false, false}; - std::array target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array target_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array target_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - - double target_vel_limit_ = 0.0; - double target_acc_limit_ = 0.0; - double suspension_vel_limit_ = 0.0; - double suspension_acc_limit_ = 0.0; -}; - -} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp deleted file mode 100644 index 7e6af68c..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_suspension.hpp +++ /dev/null @@ -1,302 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -#include "controller/pid/pid_calculator.hpp" - -namespace rmcs_core::controller::chassis { - -class DeformableChassisActiveSuspension { -public: - struct Corrections { - std::array joint_angle_correction = {0.0, 0.0, 0.0, 0.0}; - std::array joint_velocity_correction = {0.0, 0.0, 0.0, 0.0}; - std::array joint_acceleration_correction = {0.0, 0.0, 0.0, 0.0}; - std::array joint_correction_active = {false, false, false, false}; - }; - - void configure(rclcpp::Node& node) { - load_pid_( - node, "active_suspension_pitch_outer_", pitch_outer_pid_, // - 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, 3.0); - load_pid_( - node, "active_suspension_pitch_inner_", pitch_inner_pid_, // - 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); - load_pid_( - node, "active_suspension_roll_outer_", roll_outer_pid_, // - 8.0, 0.35, 0.28, -2.0, 2.0, -3.0, 3.0); - load_pid_( - node, "active_suspension_roll_inner_", roll_inner_pid_, // - 2.0, 0.0, 0.0, -1.0, 1.0, -0.785, 0.785); - - correction_vel_limit_ = std::max( - deg_to_rad_( - std::abs(node.get_parameter_or( - "active_suspension_correction_velocity_limit_deg", 720.0))), - 1e-6); - correction_acc_limit_ = std::max( - deg_to_rad_( - std::abs(node.get_parameter_or( - "active_suspension_correction_acceleration_limit_deg", 3600.0))), - 1e-6); - - calibration_wait_time_ = - std::max(node.get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); - calibration_sample_time_ = - std::max(node.get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); - } - - void calibrate(double pitch, double roll, bool symmetric_target, double dt) { - - if (calibrated_once_) - return; - - if (!symmetric_target) { - reset_calibration_window_(); - return; - } - - if (!std::isfinite(pitch) || !std::isfinite(roll)) - return; - - calibration_hold_elapsed_ += dt; - if (calibration_hold_elapsed_ < calibration_wait_time_) - return; - - double calibration_end = calibration_wait_time_ + calibration_sample_time_; - if (calibration_hold_elapsed_ < calibration_end) { - pitch_sum_ += pitch; - roll_sum_ += roll; - ++sample_count_; - return; - } - - if (calibration_completed_for_window_) - return; - - calibration_completed_for_window_ = true; - if (sample_count_ == 0) - return; - - pitch_offset_ = std::clamp( - pitch_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); - roll_offset_ = std::clamp( - roll_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); - calibrated_once_ = true; - } - - bool calibrated() const { return calibrated_once_; } - double pitch_offset() const { return pitch_offset_; } - double roll_offset() const { return roll_offset_; } - - double scope_torque(bool suspension_active, bool is_spin) const { - if (suspension_active && !is_spin) - return -0.3; - return 0.3; - } - - Corrections update( - double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, - bool low_prone_override_active, double min_angle_deg, double max_angle_deg, - double base_angle_deg, - bool correction_inverted, const std::array& base_joint_angles, double dt) { - - Corrections corrections; - - if (!suspension_active) { - reset_attitude_(); - run_correction_trajectory_( - low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, - base_joint_angles, dt, corrections); - return corrections; - } - - constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; - double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); - double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); - - double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); - double roll_outer = roll_outer_pid_.update(clamped_roll); - double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); - double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); - - if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { - reset_attitude_(); - return corrections; - } - - compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); - run_correction_trajectory_( - low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, - base_joint_angles, dt, corrections); - return corrections; - } - - void reset() { - pitch_outer_pid_.reset(); - pitch_inner_pid_.reset(); - roll_outer_pid_.reset(); - roll_inner_pid_.reset(); - correction_target_rad_.fill(0.0); - correction_state_rad_.fill(0.0); - correction_velocity_state_rad_.fill(0.0); - correction_acceleration_state_rad_.fill(0.0); - reset_calibration_window_(); - } - -private: - static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; - - static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } - - void load_pid_( - rclcpp::Node& node, const std::string& prefix, pid::PidCalculator& pid, double kp_default, - double ki_default, double kd_default, double integral_min_default, - double integral_max_default, double output_min_default, double output_max_default) { - - pid.kp = node.get_parameter_or(prefix + "kp", kp_default); - pid.ki = node.get_parameter_or(prefix + "ki", ki_default); - pid.kd = node.get_parameter_or(prefix + "kd", kd_default); - pid.integral_min = node.get_parameter_or(prefix + "integral_min", integral_min_default); - pid.integral_max = node.get_parameter_or(prefix + "integral_max", integral_max_default); - pid.output_min = node.get_parameter_or(prefix + "output_min", output_min_default); - pid.output_max = node.get_parameter_or(prefix + "output_max", output_max_default); - } - - void reset_attitude_() { - pitch_outer_pid_.reset(); - pitch_inner_pid_.reset(); - roll_outer_pid_.reset(); - roll_inner_pid_.reset(); - correction_target_rad_.fill(0.0); - } - - void reset_calibration_window_() { - calibration_hold_elapsed_ = 0.0; - sample_count_ = 0; - pitch_sum_ = 0.0; - roll_sum_ = 0.0; - calibration_completed_for_window_ = false; - } - - void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { - - if (inverted) { - double front_pitch_contribution = std::max(pitch_diff, 0.0); - double back_pitch_contribution = std::max(-pitch_diff, 0.0); - double left_roll_contribution = std::max(-roll_diff, 0.0); - double right_roll_contribution = std::max(roll_diff, 0.0); - correction_target_rad_[kLeftFront] = - -(front_pitch_contribution + left_roll_contribution); - correction_target_rad_[kLeftBack] = -(back_pitch_contribution + left_roll_contribution); - correction_target_rad_[kRightBack] = - -(back_pitch_contribution + right_roll_contribution); - correction_target_rad_[kRightFront] = - -(front_pitch_contribution + right_roll_contribution); - } else { - double front_pitch_contribution = std::max(-pitch_diff, 0.0); - double back_pitch_contribution = std::max(pitch_diff, 0.0); - double left_roll_contribution = std::max(roll_diff, 0.0); - double right_roll_contribution = std::max(-roll_diff, 0.0); - correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; - correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; - correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; - correction_target_rad_[kRightFront] = - front_pitch_contribution + right_roll_contribution; - } - } - - void run_correction_trajectory_( - bool low_prone_override_active, double min_angle_deg, double max_angle_deg, - double base_angle_deg, const std::array& base_joint_angles, double dt, - Corrections& corrections) { - - double max_target_rad = deg_to_rad_(max_angle_deg); - double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); - - for (size_t i = 0; i < kJointCount; ++i) { - double base_angle = std::isfinite(base_joint_angles[i]) - ? base_joint_angles[i] - : (low_prone_override_active ? min_susp_rad - : deg_to_rad_(base_angle_deg)); - - double correction_min = min_susp_rad - base_angle; - double correction_max = max_target_rad - base_angle; - double target = std::clamp(correction_target_rad_[i], correction_min, correction_max); - - double& angle_state = correction_state_rad_[i]; - double& velocity_state = correction_velocity_state_rad_[i]; - double& acceleration_state = correction_acceleration_state_rad_[i]; - - double position_error = target - angle_state; - double stopping_distance = - velocity_state * velocity_state / (2.0 * correction_acc_limit_); - - double desired_velocity = 0.0; - if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) { - desired_velocity = std::copysign(correction_vel_limit_, position_error); - } - - double velocity_error = desired_velocity - velocity_state; - acceleration_state = - std::clamp(velocity_error / dt, -correction_acc_limit_, correction_acc_limit_); - - velocity_state += acceleration_state * dt; - velocity_state = - std::clamp(velocity_state, -correction_vel_limit_, correction_vel_limit_); - angle_state += velocity_state * dt; - - double next_error = target - angle_state; - if ((position_error > 0.0 && next_error < 0.0) - || (position_error < 0.0 && next_error > 0.0) - || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { - angle_state = target; - velocity_state = 0.0; - acceleration_state = 0.0; - } - - corrections.joint_correction_active[i] = true; - } - - corrections.joint_angle_correction = correction_state_rad_; - corrections.joint_velocity_correction = correction_velocity_state_rad_; - corrections.joint_acceleration_correction = correction_acceleration_state_rad_; - } - - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; - static constexpr size_t kRightFront = 3; - static constexpr size_t kJointCount = 4; - - pid::PidCalculator pitch_outer_pid_{}; - pid::PidCalculator pitch_inner_pid_{}; - pid::PidCalculator roll_outer_pid_{}; - pid::PidCalculator roll_inner_pid_{}; - - double correction_vel_limit_ = 40.0; - double correction_acc_limit_ = 200.0; - - double calibration_wait_time_ = 2.0; - double calibration_sample_time_ = 3.0; - double calibration_hold_elapsed_ = 0.0; - size_t sample_count_ = 0; - double pitch_sum_ = 0.0; - double roll_sum_ = 0.0; - bool calibration_completed_for_window_ = false; - bool calibrated_once_ = false; - double pitch_offset_ = 0.0; - double roll_offset_ = 0.0; - - std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; - std::array correction_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; -}; - -} // namespace rmcs_core::controller::chassis diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp index 830f5eb1..38bb7ec1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_joint_controller.cpp @@ -32,27 +32,19 @@ class DeformableJointController : public rmcs_executor::Component , public rclcpp::Node { public: - // Joint controller owns only the local angle-servo execution. Chassis publishes the - // higher-level target angle trajectory; this controller turns that target into motor torque. - struct ControllerConfig { - rmcs_core::controller::adrc::TD::Config td; - rmcs_core::controller::adrc::ESO::Config eso; - rmcs_core::controller::adrc::NLESF::Config nlesf; - double output_min = -std::numeric_limits::infinity(); - double output_max = std::numeric_limits::infinity(); - }; - - struct InputSnapshot { - double measurement_angle = std::numeric_limits::quiet_NaN(); - double setpoint_angle = std::numeric_limits::quiet_NaN(); - double setpoint_velocity = std::numeric_limits::quiet_NaN(); - }; - - DeformableJointController() + explicit DeformableJointController() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - register_interfaces_(); + register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); + register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); + if (has_parameter("setpoint_velocity")) { + register_input( + get_parameter("setpoint_velocity").as_string(), setpoint_velocity_, false); + use_setpoint_velocity_ = true; + } + register_output(get_parameter("control").as_string(), control_torque_, nan_); + load_config_(); apply_config_(); } @@ -75,15 +67,21 @@ class DeformableJointController } private: - void register_interfaces_() { - register_input(get_parameter("measurement_angle").as_string(), measurement_angle_); - register_input(get_parameter("setpoint_angle").as_string(), setpoint_angle_); - if (has_parameter("setpoint_velocity")) { - register_input(get_parameter("setpoint_velocity").as_string(), setpoint_velocity_, false); - use_setpoint_velocity_ = true; - } - register_output(get_parameter("control").as_string(), control_torque_, nan_); - } + // Joint controller owns only the local angle-servo execution. Chassis publishes the + // higher-level target angle trajectory; this controller turns that target into motor torque. + struct ControllerConfig { + rmcs_core::controller::adrc::TD::Config td; + rmcs_core::controller::adrc::ESO::Config eso; + rmcs_core::controller::adrc::NLESF::Config nlesf; + double output_min = -std::numeric_limits::infinity(); + double output_max = std::numeric_limits::infinity(); + }; + + struct InputSnapshot { + double measurement_angle = std::numeric_limits::quiet_NaN(); + double setpoint_angle = std::numeric_limits::quiet_NaN(); + double setpoint_velocity = std::numeric_limits::quiet_NaN(); + }; void load_config_() { dt_ = load_parameter_or(*this, "dt", 0.001); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp similarity index 56% rename from rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp rename to rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index 2dba6501..b920d0aa 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis_mode_manager.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -14,11 +15,20 @@ namespace rmcs_core::controller::chassis { class DeformableChassisModeManager { public: + enum class SuspensionMode : uint8_t { + OFF = 0, + ACTIVE = 1, + PASSIVE = 2, + }; + struct JointPostureState { rmcs_msgs::ChassisMode mode = rmcs_msgs::ChassisMode::AUTO; bool ctrl_low_prone_active = false; + bool low_prone_active = false; bool pitch_lock_active = false; bool suspension_active = false; + bool passive_suspension_active = false; + SuspensionMode suspension_mode = SuspensionMode::OFF; bool symmetric_posture_target = true; bool spinning_forward = true; std::array joint_posture_target_deg = {58.0, 58.0, 58.0, 58.0}; @@ -28,7 +38,12 @@ class DeformableChassisModeManager { explicit DeformableChassisModeManager(rclcpp::Node& node) : min_angle_(node.get_parameter_or("min_angle", 7.0)) , max_angle_(node.get_parameter_or("max_angle", 58.0)) - , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { + , active_suspension_base_angle_( + std::clamp( + node.get_parameter_or("active_suspension_base_angle", max_angle_), + min_angle_ - 5.0, max_angle_)) + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) + , passive_suspension_enable_(node.get_parameter_or("passive_suspension_enable", true)) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -37,23 +52,26 @@ class DeformableChassisModeManager { void reset() { joint_posture_state_.mode = rmcs_msgs::ChassisMode::AUTO; joint_posture_state_.ctrl_low_prone_active = false; + joint_posture_state_.low_prone_active = false; joint_posture_state_.pitch_lock_active = false; joint_posture_state_.suspension_active = false; + joint_posture_state_.passive_suspension_active = false; + joint_posture_state_.suspension_mode = SuspensionMode::OFF; joint_posture_state_.symmetric_posture_target = true; joint_posture_state_.spinning_forward = true; joint_posture_state_.joint_posture_target_deg.fill(max_angle_); joint_posture_state_.suspension_reference_angle_deg = max_angle_; current_target_angle_ = max_angle_; + active_suspension_base_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); apply_symmetric_target_ = true; complex_spin_active_ = false; complex_spin_elapsed_ = 0.0; suspension_enabled_by_toggle_ = false; - suspension_toggle_left_down_pending_ = false; + passive_suspension_enabled_by_toggle_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); last_rotary_knob_ = 0.0; @@ -65,20 +83,37 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right, keyboard); - update_lift_target_toggle_(keyboard, rotary_knob, dt); + update_suspension_toggle_from_inputs_(switch_left, switch_right); joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; - joint_posture_state_.pitch_lock_active = keyboard.ctrl; - joint_posture_state_.suspension_active = - suspension_enable_ && (keyboard.ctrl || suspension_enabled_by_toggle_); - update_joint_posture_state_(keyboard.ctrl); + joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active; + joint_posture_state_.pitch_lock_active = + joint_posture_state_.ctrl_low_prone_active || suspension_enabled_by_toggle_; + + update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); + update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); + update_joint_posture_state_(joint_posture_state_.low_prone_active); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; last_keyboard_ = keyboard; } + rmcs_msgs::ChassisMode mode() const { return joint_posture_state_.mode; } + bool pitch_lock_active() const { return joint_posture_state_.pitch_lock_active; } + bool suspension_active() const { return joint_posture_state_.suspension_active; } + bool passive_suspension_active() const { + return joint_posture_state_.passive_suspension_active; + } + bool low_prone_active() const { return joint_posture_state_.low_prone_active; } + bool symmetric_posture_target() const { return joint_posture_state_.symmetric_posture_target; } + bool spinning_forward() const { return joint_posture_state_.spinning_forward; } + double suspension_reference_angle_deg() const { + return joint_posture_state_.suspension_reference_angle_deg; + } + void copy_joint_posture_target_deg(std::array& out) const { + out = joint_posture_state_.joint_posture_target_deg; + } + const JointPostureState& joint_posture_state() const { return joint_posture_state_; } double min_angle() const { return min_angle_; } @@ -93,56 +128,61 @@ class DeformableChassisModeManager { } private: + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr size_t kJointCount = 4; + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + static bool + symmetric_joint_target_requested_(const std::array& joint_target_deg) { + constexpr double epsilon = 1e-6; + return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { + return std::abs(v - joint_target_deg.front()) <= epsilon; + }); + } + void update_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard) { - auto mode = joint_posture_state_.mode; + auto next_mode = joint_posture_state_.mode; if (switch_left == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); - joint_posture_state_.mode = mode; + joint_posture_state_.mode = next_mode; return; } if (last_switch_right_ == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::DOWN) { deactivate_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::STEP_DOWN; + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + next_mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + next_mode = rmcs_msgs::ChassisMode::SPIN; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.c && keyboard.c) { deactivate_complex_spin_(); - if (mode == rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::AUTO; + if (next_mode == rmcs_msgs::ChassisMode::SPIN) { + next_mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + next_mode = rmcs_msgs::ChassisMode::SPIN; joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; } } else if (!last_keyboard_.z && keyboard.z) { deactivate_complex_spin_(); - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; + next_mode = next_mode == rmcs_msgs::ChassisMode::STEP_DOWN + ? rmcs_msgs::ChassisMode::AUTO + : rmcs_msgs::ChassisMode::STEP_DOWN; } if (complex_spin_active_) - mode = rmcs_msgs::ChassisMode::SPIN; - - joint_posture_state_.mode = mode; - } + next_mode = rmcs_msgs::ChassisMode::SPIN; - void activate_complex_spin_(rmcs_msgs::ChassisMode& mode) { - complex_spin_active_ = true; - complex_spin_elapsed_ = 0.0; - apply_symmetric_target_ = true; - if (mode != rmcs_msgs::ChassisMode::SPIN) { - mode = rmcs_msgs::ChassisMode::SPIN; - joint_posture_state_.spinning_forward = !joint_posture_state_.spinning_forward; - } + joint_posture_state_.mode = next_mode; } void deactivate_complex_spin_() { @@ -166,58 +206,78 @@ class DeformableChassisModeManager { apply_symmetric_target_ = false; } - void toggle_bg_target_() { + void toggle_front_back_posture_target_() { if (joint_current_target_angle_[kLeftFront] > joint_current_target_angle_[kLeftBack]) apply_front_low_rear_high_target_(); else apply_front_high_rear_low_target_(); } - void update_suspension_toggle_from_inputs_( + void update_suspension_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, - const rmcs_msgs::Keyboard& keyboard) { + const rmcs_msgs::Keyboard& keyboard, double rotary_knob) { + const bool passive_toggle_requested = !last_keyboard_.x && keyboard.x; + const bool remote_suspension_rotary_mode = + switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::MIDDLE; + const bool remote_active_toggle_requested = + remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); + const bool remote_passive_toggle_requested = + remote_suspension_rotary_mode && rotary_knob_up_edge_(rotary_knob); + + if ((passive_toggle_requested || remote_passive_toggle_requested) + && passive_suspension_enable_) { + passive_suspension_enabled_by_toggle_ = !passive_suspension_enabled_by_toggle_; + if (passive_suspension_enabled_by_toggle_) + suspension_enabled_by_toggle_ = false; + } + const bool keyboard_toggle_requested = !last_keyboard_.e && keyboard.e; - if (keyboard_toggle_requested - || suspension_toggle_requested_by_switch_(switch_left, switch_right)) + if (keyboard_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; + + const bool active_requested = + suspension_enable_ + && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); + if (active_requested) + passive_suspension_enabled_by_toggle_ = false; + + const bool passive_requested = + passive_suspension_enable_ + && passive_suspension_enabled_by_toggle_; + + joint_posture_state_.suspension_mode = SuspensionMode::OFF; + if (active_requested) + joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; + else if (passive_requested) + joint_posture_state_.suspension_mode = SuspensionMode::PASSIVE; + + joint_posture_state_.suspension_active = + joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; + joint_posture_state_.passive_suspension_active = + joint_posture_state_.suspension_mode == SuspensionMode::PASSIVE; } - bool suspension_toggle_requested_by_switch_( + void update_suspension_toggle_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { - if (last_switch_left_ != rmcs_msgs::Switch::DOWN && switch_left == rmcs_msgs::Switch::DOWN) - suspension_toggle_left_down_pending_ = true; - - const bool right_entered_up = - last_switch_right_ != rmcs_msgs::Switch::UP && switch_right == rmcs_msgs::Switch::UP; - if (suspension_toggle_left_down_pending_ && switch_left == rmcs_msgs::Switch::DOWN - && right_entered_up) { - suspension_toggle_left_down_pending_ = false; - return true; + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; } - - if (switch_left != rmcs_msgs::Switch::DOWN) - suspension_toggle_left_down_pending_ = false; - - return false; } - void update_lift_target_toggle_( + void update_posture_target_from_inputs_( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { - constexpr double rotary_knob_symmetric_edge_threshold = 0.7; - constexpr double rotary_knob_bg_edge_threshold = -0.9; constexpr double complex_spin_toggle_period = 0.5; + const bool remote_joint_posture_rotary_mode = + switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; - const bool rotary_knob_toggle_condition = - last_rotary_knob_ < rotary_knob_symmetric_edge_threshold - && rotary_knob >= rotary_knob_symmetric_edge_threshold; - + remote_joint_posture_rotary_mode && rotary_knob_down_edge_(rotary_knob); const bool rotary_knob_bg_toggle_condition = - last_rotary_knob_ > rotary_knob_bg_edge_threshold - && rotary_knob <= rotary_knob_bg_edge_threshold; - + remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; @@ -237,12 +297,19 @@ class DeformableChassisModeManager { if (rotary_knob_toggle_condition || keyboard_toggle_condition || complex_spin_toggle_condition) { - current_target_angle_ = - (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; - apply_symmetric_target_ = true; - joint_current_target_angle_.fill(current_target_angle_); + if (joint_posture_state_.suspension_active && keyboard_toggle_condition) { + active_suspension_base_angle_ = + (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) + ? min_angle_ + : max_angle_; + } else { + current_target_angle_ = + (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); + } } else if (rotary_knob_bg_toggle_condition) { - toggle_bg_target_(); + toggle_front_back_posture_target_(); } else if (front_high_rear_low) { apply_front_high_rear_low_target_(); } else if (front_low_rear_high) { @@ -252,22 +319,33 @@ class DeformableChassisModeManager { last_rotary_knob_ = rotary_knob; } - static constexpr size_t kLeftFront = 0; - static constexpr size_t kLeftBack = 1; - static constexpr size_t kRightBack = 2; - static constexpr size_t kRightFront = 3; - static constexpr size_t kJointCount = 4; + bool rotary_knob_down_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ < rotary_knob_edge_threshold + && rotary_knob >= rotary_knob_edge_threshold; + } + + bool rotary_knob_up_edge_(double rotary_knob) const { + constexpr double rotary_knob_edge_threshold = 0.7; + return last_rotary_knob_ > -rotary_knob_edge_threshold + && rotary_knob <= -rotary_knob_edge_threshold; + } - void update_joint_posture_state_(bool ctrl_low_prone_active) { + void update_joint_posture_state_(bool low_prone_active) { std::array effective_joint_posture_target_deg = joint_current_target_angle_; - if (ctrl_low_prone_active) + if (low_prone_active) effective_joint_posture_target_deg.fill(min_angle_ - 5.0); joint_posture_state_.joint_posture_target_deg = effective_joint_posture_target_deg; joint_posture_state_.symmetric_posture_target = symmetric_joint_target_requested_(effective_joint_posture_target_deg); + if (joint_posture_state_.suspension_active) { + joint_posture_state_.suspension_reference_angle_deg = active_suspension_base_angle_; + return; + } + if (joint_posture_state_.symmetric_posture_target) { joint_posture_state_.suspension_reference_angle_deg = effective_joint_posture_target_deg.front(); @@ -281,19 +359,13 @@ class DeformableChassisModeManager { posture_angle_sum / static_cast(kJointCount); } - static bool - symmetric_joint_target_requested_(const std::array& joint_target_deg) { - constexpr double epsilon = 1e-6; - return std::all_of(joint_target_deg.begin() + 1, joint_target_deg.end(), [&](double v) { - return std::abs(v - joint_target_deg.front()) <= epsilon; - }); - } - JointPostureState joint_posture_state_; double min_angle_; double max_angle_; + double active_suspension_base_angle_; bool suspension_enable_; + bool passive_suspension_enable_; double current_target_angle_; std::array joint_current_target_angle_; @@ -301,10 +373,9 @@ class DeformableChassisModeManager { bool complex_spin_active_ = false; double complex_spin_elapsed_ = 0.0; bool suspension_enabled_by_toggle_ = false; - bool suspension_toggle_left_down_pending_ = false; + bool passive_suspension_enabled_by_toggle_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); double last_rotary_knob_ = 0.0; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp new file mode 100644 index 00000000..0668e6e6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -0,0 +1,746 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class DeformableSuspension + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DeformableSuspension() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + load_config_(); + + register_input("/predefined/update_rate", update_rate_, false); + + register_input("/chassis/active_suspension/active", active_suspension_active_); + register_input("/chassis/deformable/reset_count", reset_count_, false); + register_input( + "/chassis/deformable/passive_suspension_active", passive_suspension_active_); + register_input("/chassis/deformable/low_prone_active", low_prone_active_); + register_input( + "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); + register_input("/chassis/deformable/correction_inverted", correction_inverted_); + register_input("/chassis/deformable/min_angle_deg", min_angle_deg_); + register_input("/chassis/deformable/max_angle_deg", max_angle_deg_); + register_input( + "/chassis/deformable/suspension_reference_angle_deg", + suspension_reference_angle_deg_); + + register_input("/chassis/imu/pitch", chassis_imu_pitch_, false); + register_input("/chassis/imu/roll", chassis_imu_roll_, false); + register_input("/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, false); + register_input("/chassis/imu/roll_rate", chassis_imu_roll_rate_, false); + + for (size_t i = 0; i < kJointCount; ++i) { + register_input( + std::string{"/chassis/deformable/"} + kJointName[i] + "_joint/posture_target_angle", + joint_posture_target_angle_rad_[i]); + register_input( + std::string{"/chassis/"} + kJointName[i] + "_joint/physical_angle", + joint_physical_angle_[i], false); + register_input( + std::string{"/chassis/"} + kJointName[i] + "_joint/torque", joint_torque_[i], + false); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", + joint_target_angle_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_velocity", + joint_target_velocity_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + + "_joint/target_physical_acceleration", + joint_target_acceleration_[i], nan_); + register_output( + std::string{"/chassis/"} + kJointName[i] + "_joint/control_angle_error", + joint_angle_error_[i], nan_); + } + } + + void before_updating() override { + if (!update_rate_.ready()) + update_rate_.make_and_bind_directly(1000.0); + if (!reset_count_.ready()) + reset_count_.make_and_bind_directly(static_cast(0)); + if (!chassis_imu_pitch_.ready()) + chassis_imu_pitch_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_.ready()) + chassis_imu_roll_.make_and_bind_directly(0.0); + if (!chassis_imu_pitch_rate_.ready()) + chassis_imu_pitch_rate_.make_and_bind_directly(0.0); + if (!chassis_imu_roll_rate_.ready()) + chassis_imu_roll_rate_.make_and_bind_directly(0.0); + + validate_joint_feedback_inputs_(); + reset_all_controls_(); + last_reset_count_ = *reset_count_; + } + + void update() override { + if (*reset_count_ != last_reset_count_) { + reset_all_controls_(); + last_reset_count_ = *reset_count_; + return; + } + + const auto current_physical_angles = read_feedback_(); + const auto current_joint_torques = read_joint_torques_(); + + if (!init_joint_targets_from_feedback_(current_physical_angles)) { + publish_nan_joint_targets_(); + return; + } + + const auto posture_target_angles_rad = read_posture_target_angles_rad_(); + const auto dt = update_dt_(); + + if (*active_suspension_active_) + calibrate_(*chassis_imu_pitch_, *chassis_imu_roll_, *symmetric_posture_target_, dt); + + std::array joint_angle_states{}; + copy_joint_angle_states_(joint_angle_states); + update_suspension_state_( + *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, + *chassis_imu_pitch_rate_, *chassis_imu_roll_rate_, *active_suspension_active_, + *passive_suspension_active_, *low_prone_active_, *min_angle_deg_, *max_angle_deg_, + *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, + current_joint_torques, dt); + + const auto target_angles_rad = compute_joint_trajectory_targets_( + posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, + *min_angle_deg_, *suspension_reference_angle_deg_); + + run_joint_trajectory_(target_angles_rad, *active_suspension_active_, dt); + publish_joint_targets_(current_physical_angles); + } + +private: + static constexpr size_t kJointCount = 4; + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double offset_limit_rad_ = 1.0 * std::numbers::pi / 180.0; + static constexpr size_t kLeftFront = 0; + static constexpr size_t kLeftBack = 1; + static constexpr size_t kRightBack = 2; + static constexpr size_t kRightFront = 3; + static constexpr const char* kJointName[] = { + "left_front", + "left_back", + "right_back", + "right_front", + }; + + static double deg_to_rad_(double deg) { return deg * std::numbers::pi / 180.0; } + + void validate_joint_feedback_inputs_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (!joint_physical_angle_[i].ready()) + throw std::runtime_error( + "missing deformable chassis feedback interfaces: expected " + "/chassis/*_joint/physical_angle"); + } + + double update_dt_() const { + if (update_rate_.ready() && std::isfinite(*update_rate_) && *update_rate_ > 1e-6) + return 1.0 / *update_rate_; + return 1e-3; + } + + void load_pid_( + const std::string& prefix, pid::PidCalculator& pid, double kp_default, + double ki_default, double kd_default, double integral_min_default, + double integral_max_default, double output_min_default, double output_max_default) { + pid.kp = get_parameter_or(prefix + "kp", kp_default); + pid.ki = get_parameter_or(prefix + "ki", ki_default); + pid.kd = get_parameter_or(prefix + "kd", kd_default); + pid.integral_min = get_parameter_or(prefix + "integral_min", integral_min_default); + pid.integral_max = get_parameter_or(prefix + "integral_max", integral_max_default); + pid.output_min = get_parameter_or(prefix + "output_min", output_min_default); + pid.output_max = get_parameter_or(prefix + "output_max", output_max_default); + } + + void load_config_() { + joint_target_vel_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_velocity_limit", 180.0))), + 1e-6); + joint_target_acc_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or("target_physical_acceleration_limit", 720.0))), + 1e-6); + suspension_target_vel_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_velocity_limit_deg", + get_parameter_or("target_physical_velocity_limit", 180.0)))), + 1e-6); + suspension_target_acc_limit_ = std::max( + deg_to_rad_(std::abs(get_parameter_or( + "active_suspension_target_acceleration_limit_deg", + get_parameter_or("target_physical_acceleration_limit", 720.0)))), + 1e-6); + + load_pid_( + "active_suspension_pitch_outer_", pitch_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); + load_pid_( + "active_suspension_pitch_inner_", pitch_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); + load_pid_( + "active_suspension_roll_outer_", roll_outer_pid_, 8.0, 0.35, 0.28, -2.0, 2.0, + -3.0, 3.0); + load_pid_( + "active_suspension_roll_inner_", roll_inner_pid_, 2.0, 0.0, 0.0, -1.0, 1.0, + -0.785, 0.785); + + active_correction_vel_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_velocity_limit_deg", 720.0))), + 1e-6); + active_correction_acc_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("active_suspension_correction_acceleration_limit_deg", 3600.0))), + 1e-6); + + passive_enabled_ = get_parameter_or("passive_suspension_enable", true); + passive_kp_ = get_parameter_or("passive_suspension_kp", 0.035); + passive_ki_ = get_parameter_or("passive_suspension_ki", 0.005); + passive_deadband_ = std::max(get_parameter_or("passive_suspension_deadband", 0.08), 0.0); + passive_integral_limit_ = + std::max(get_parameter_or("passive_suspension_integral_limit", 3.0), 0.0); + passive_load_lpf_alpha_ = + std::clamp(get_parameter_or("passive_suspension_load_lpf_alpha", 0.08), 0.0, 1.0); + passive_max_correction_rad_ = std::max( + deg_to_rad_( + std::abs(get_parameter_or("passive_suspension_max_angle_correction_deg", 8.0))), + 1e-6); + passive_correction_vel_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("passive_suspension_correction_velocity_limit_deg", 180.0))), + 1e-6); + passive_correction_acc_limit_ = std::max( + deg_to_rad_(std::abs( + get_parameter_or("passive_suspension_correction_acceleration_limit_deg", 720.0))), + 1e-6); + + passive_load_sign_[kLeftFront] = + get_parameter_or("passive_suspension_load_sign_left_front", -1.0); + passive_load_sign_[kLeftBack] = + get_parameter_or("passive_suspension_load_sign_left_back", -1.0); + passive_load_sign_[kRightBack] = + get_parameter_or("passive_suspension_load_sign_right_back", -1.0); + passive_load_sign_[kRightFront] = + get_parameter_or("passive_suspension_load_sign_right_front", -1.0); + + passive_load_bias_[kLeftFront] = + get_parameter_or("passive_suspension_load_bias_left_front", 0.0); + passive_load_bias_[kLeftBack] = + get_parameter_or("passive_suspension_load_bias_left_back", 0.0); + passive_load_bias_[kRightBack] = + get_parameter_or("passive_suspension_load_bias_right_back", 0.0); + passive_load_bias_[kRightFront] = + get_parameter_or("passive_suspension_load_bias_right_front", 0.0); + + calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); + calibration_sample_time_ = + std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); + } + + std::array read_feedback_() const { + std::array angles; + angles.fill(nan_); + + for (size_t i = 0; i < kJointCount; ++i) + if (joint_physical_angle_[i].ready() && std::isfinite(*joint_physical_angle_[i])) + angles[i] = *joint_physical_angle_[i]; + + return angles; + } + + std::array read_joint_torques_() const { + std::array torques; + torques.fill(nan_); + + for (size_t i = 0; i < kJointCount; ++i) + if (joint_torque_[i].ready() && std::isfinite(*joint_torque_[i])) + torques[i] = *joint_torque_[i]; + + return torques; + } + + std::array read_posture_target_angles_rad_() const { + std::array targets{}; + for (size_t i = 0; i < kJointCount; ++i) + targets[i] = *joint_posture_target_angle_rad_[i]; + return targets; + } + + std::array compute_joint_trajectory_targets_( + const std::array& posture_target_angles_rad, bool suspension_active, + bool low_prone_active, double min_angle_deg, double suspension_reference_angle_deg) const { + if (!suspension_active) + return posture_target_angles_rad; + + std::array target_angles_rad{}; + double target_angle_rad = low_prone_active ? deg_to_rad_(min_angle_deg - 5.0) + : deg_to_rad_(suspension_reference_angle_deg); + target_angles_rad.fill(target_angle_rad); + return target_angles_rad; + } + + void reset_attitude_() { + pitch_outer_pid_.reset(); + pitch_inner_pid_.reset(); + roll_outer_pid_.reset(); + roll_inner_pid_.reset(); + correction_target_rad_.fill(0.0); + } + + void reset_passive_state_() { + passive_load_initialized_ = false; + passive_filtered_load_proxy_.fill(0.0); + passive_load_integral_.fill(0.0); + } + + void reset_calibration_window_() { + calibration_hold_elapsed_ = 0.0; + sample_count_ = 0; + pitch_sum_ = 0.0; + roll_sum_ = 0.0; + calibration_completed_for_window_ = false; + } + + void reset_all_controls_() { + reset_attitude_(); + correction_state_rad_.fill(0.0); + correction_velocity_state_rad_.fill(0.0); + correction_acceleration_state_rad_.fill(0.0); + joint_target_active_.fill(false); + joint_target_angle_state_rad_.fill(nan_); + joint_target_velocity_state_rad_.fill(0.0); + joint_target_acceleration_state_rad_.fill(0.0); + reset_passive_state_(); + reset_calibration_window_(); + calibrated_once_ = false; + pitch_offset_value_ = 0.0; + roll_offset_value_ = 0.0; + + for (size_t i = 0; i < kJointCount; ++i) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + } + } + + void calibrate_(double pitch, double roll, bool symmetric_target, double dt) { + if (calibrated_once_) + return; + + if (!symmetric_target) { + reset_calibration_window_(); + return; + } + + if (!std::isfinite(pitch) || !std::isfinite(roll)) + return; + + calibration_hold_elapsed_ += dt; + if (calibration_hold_elapsed_ < calibration_wait_time_) + return; + + const double calibration_end = calibration_wait_time_ + calibration_sample_time_; + if (calibration_hold_elapsed_ < calibration_end) { + pitch_sum_ += pitch; + roll_sum_ += roll; + ++sample_count_; + return; + } + + if (calibration_completed_for_window_) + return; + + calibration_completed_for_window_ = true; + if (sample_count_ == 0) + return; + + pitch_offset_value_ = std::clamp( + pitch_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + roll_offset_value_ = std::clamp( + roll_sum_ / static_cast(sample_count_), -offset_limit_rad_, offset_limit_rad_); + calibrated_once_ = true; + } + + bool init_joint_targets_from_feedback_(const std::array& physical_angles) { + bool any_active_value = false; + for (size_t i = 0; i < kJointCount; ++i) { + if (std::isfinite(physical_angles[i]) && !joint_target_active_[i]) { + joint_target_angle_state_rad_[i] = physical_angles[i]; + joint_target_velocity_state_rad_[i] = 0.0; + joint_target_acceleration_state_rad_[i] = 0.0; + joint_target_active_[i] = true; + } + any_active_value = any_active_value || joint_target_active_[i]; + } + return any_active_value; + } + + void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { + if (inverted) { + const double front_pitch_contribution = std::max(pitch_diff, 0.0); + const double back_pitch_contribution = std::max(-pitch_diff, 0.0); + const double left_roll_contribution = std::max(-roll_diff, 0.0); + const double right_roll_contribution = std::max(roll_diff, 0.0); + correction_target_rad_[kLeftFront] = + -(front_pitch_contribution + left_roll_contribution); + correction_target_rad_[kLeftBack] = + -(back_pitch_contribution + left_roll_contribution); + correction_target_rad_[kRightBack] = + -(back_pitch_contribution + right_roll_contribution); + correction_target_rad_[kRightFront] = + -(front_pitch_contribution + right_roll_contribution); + } else { + const double front_pitch_contribution = std::max(-pitch_diff, 0.0); + const double back_pitch_contribution = std::max(pitch_diff, 0.0); + const double left_roll_contribution = std::max(roll_diff, 0.0); + const double right_roll_contribution = std::max(-roll_diff, 0.0); + correction_target_rad_[kLeftFront] = front_pitch_contribution + left_roll_contribution; + correction_target_rad_[kLeftBack] = back_pitch_contribution + left_roll_contribution; + correction_target_rad_[kRightBack] = back_pitch_contribution + right_roll_contribution; + correction_target_rad_[kRightFront] = front_pitch_contribution + right_roll_contribution; + } + } + + void update_passive_targets_(const std::array& joint_torques, double dt) { + if (dt <= 0.0 || !std::isfinite(dt)) { + correction_target_rad_.fill(0.0); + return; + } + + std::array load_proxy{}; + for (size_t i = 0; i < kJointCount; ++i) { + if (!std::isfinite(joint_torques[i])) { + correction_target_rad_.fill(0.0); + reset_passive_state_(); + return; + } + + load_proxy[i] = passive_load_sign_[i] * (joint_torques[i] - passive_load_bias_[i]); + if (!passive_load_initialized_) { + passive_filtered_load_proxy_[i] = load_proxy[i]; + } else { + passive_filtered_load_proxy_[i] += + passive_load_lpf_alpha_ * (load_proxy[i] - passive_filtered_load_proxy_[i]); + } + } + passive_load_initialized_ = true; + + double load_average = 0.0; + for (double load : passive_filtered_load_proxy_) + load_average += load; + load_average /= static_cast(kJointCount); + + std::array raw_targets{}; + for (size_t i = 0; i < kJointCount; ++i) { + double load_error = load_average - passive_filtered_load_proxy_[i]; + if (std::abs(load_error) <= passive_deadband_) + load_error = 0.0; + + passive_load_integral_[i] = std::clamp( + passive_load_integral_[i] + load_error * dt, -passive_integral_limit_, + passive_integral_limit_); + + raw_targets[i] = passive_kp_ * load_error + passive_ki_ * passive_load_integral_[i]; + } + + double raw_target_mean = 0.0; + for (double raw_target : raw_targets) + raw_target_mean += raw_target; + raw_target_mean /= static_cast(kJointCount); + + for (size_t i = 0; i < kJointCount; ++i) { + correction_target_rad_[i] = std::clamp( + raw_targets[i] - raw_target_mean, -passive_max_correction_rad_, + passive_max_correction_rad_); + } + } + + void run_correction_trajectory_( + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, const std::array& base_joint_angles, + double correction_vel_limit, double correction_acc_limit, double dt) { + const double max_target_rad = deg_to_rad_(max_angle_deg); + const double min_susp_rad = deg_to_rad_(min_angle_deg - 5.0); + + for (size_t i = 0; i < kJointCount; ++i) { + const double base_angle = std::isfinite(base_joint_angles[i]) + ? base_joint_angles[i] + : (low_prone_override_active ? min_susp_rad + : deg_to_rad_(base_angle_deg)); + + const double correction_min = min_susp_rad - base_angle; + const double correction_max = max_target_rad - base_angle; + const double target = + std::clamp(correction_target_rad_[i], correction_min, correction_max); + + double& angle_state = correction_state_rad_[i]; + double& velocity_state = correction_velocity_state_rad_[i]; + double& acceleration_state = correction_acceleration_state_rad_[i]; + + const double position_error = target - angle_state; + const double stopping_distance = + velocity_state * velocity_state / (2.0 * correction_acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(correction_vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = + std::clamp(velocity_error / dt, -correction_acc_limit, correction_acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -correction_vel_limit, correction_vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + void update_suspension_state_( + double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, + bool passive_suspension_active, bool low_prone_override_active, double min_angle_deg, + double max_angle_deg, double base_angle_deg, bool correction_inverted, + const std::array& base_joint_angles, + const std::array& joint_torques, double dt) { + if (passive_suspension_active && passive_enabled_) { + reset_attitude_(); + update_passive_targets_(joint_torques, dt); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, passive_correction_vel_limit_, passive_correction_acc_limit_, + dt); + return; + } + + reset_passive_state_(); + + if (!suspension_active) { + reset_attitude_(); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + return; + } + + constexpr double max_attitude = 30.0 * std::numbers::pi / 180.0; + const double clamped_pitch = std::clamp(pitch, -max_attitude, max_attitude); + const double clamped_roll = std::clamp(roll, -max_attitude, max_attitude); + + const double pitch_outer = pitch_outer_pid_.update(-clamped_pitch); + const double roll_outer = roll_outer_pid_.update(clamped_roll); + const double pitch_diff = pitch_inner_pid_.update(pitch_outer - pitch_rate); + const double roll_diff = roll_inner_pid_.update(roll_outer + roll_rate); + + if (!std::isfinite(pitch_diff) || !std::isfinite(roll_diff)) { + reset_attitude_(); + return; + } + + compute_correction_targets_(pitch_diff, roll_diff, correction_inverted); + run_correction_trajectory_( + low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, + base_joint_angles, active_correction_vel_limit_, active_correction_acc_limit_, dt); + } + + void run_joint_trajectory_( + const std::array& target_angles_rad, bool suspension_active, + double dt) { + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) + continue; + + double& angle_state = joint_target_angle_state_rad_[i]; + double& velocity_state = joint_target_velocity_state_rad_[i]; + double& acceleration_state = joint_target_acceleration_state_rad_[i]; + const double target = target_angles_rad[i]; + + const double vel_limit = + suspension_active ? suspension_target_vel_limit_ : joint_target_vel_limit_; + const double acc_limit = + suspension_active ? suspension_target_acc_limit_ : joint_target_acc_limit_; + + if (!std::isfinite(target) || !std::isfinite(angle_state)) + continue; + + const double position_error = target - angle_state; + const double stopping_distance = velocity_state * velocity_state / (2.0 * acc_limit); + + double desired_velocity = 0.0; + if (std::abs(position_error) > 1e-6 && std::abs(position_error) > stopping_distance) + desired_velocity = std::copysign(vel_limit, position_error); + + const double velocity_error = desired_velocity - velocity_state; + acceleration_state = std::clamp(velocity_error / dt, -acc_limit, acc_limit); + + velocity_state += acceleration_state * dt; + velocity_state = std::clamp(velocity_state, -vel_limit, vel_limit); + angle_state += velocity_state * dt; + + const double next_error = target - angle_state; + if ((position_error > 0.0 && next_error < 0.0) + || (position_error < 0.0 && next_error > 0.0) + || (std::abs(next_error) < 1e-5 && std::abs(velocity_state) < 1e-3)) { + angle_state = target; + velocity_state = 0.0; + acceleration_state = 0.0; + } + } + } + + bool any_joint_target_active_() const { + for (size_t i = 0; i < kJointCount; ++i) + if (joint_target_active_[i]) + return true; + return false; + } + + void copy_joint_angle_states_(std::array& out) const { + out = joint_target_angle_state_rad_; + } + + void publish_joint_targets_(const std::array& feedback_angles) { + const double min_angle_rad = deg_to_rad_(*min_angle_deg_ - 5.0); + const double max_angle_rad = deg_to_rad_(*max_angle_deg_); + + if (!any_joint_target_active_()) { + publish_nan_joint_targets_(); + return; + } + + for (size_t i = 0; i < kJointCount; ++i) { + if (!joint_target_active_[i]) { + *joint_target_angle_[i] = nan_; + *joint_target_velocity_[i] = nan_; + *joint_target_acceleration_[i] = nan_; + *joint_angle_error_[i] = nan_; + continue; + } + + const double target = joint_target_angle_state_rad_[i] + correction_state_rad_[i]; + *joint_target_angle_[i] = std::clamp(target, min_angle_rad, max_angle_rad); + *joint_target_velocity_[i] = + joint_target_velocity_state_rad_[i] + correction_velocity_state_rad_[i]; + *joint_target_acceleration_[i] = + joint_target_acceleration_state_rad_[i] + correction_acceleration_state_rad_[i]; + *joint_angle_error_[i] = std::isfinite(feedback_angles[i]) + ? feedback_angles[i] - *joint_target_angle_[i] + : nan_; + } + } + + void publish_nan_joint_targets_() { + reset_all_controls_(); + } + + InputInterface update_rate_; + + InputInterface active_suspension_active_; + InputInterface reset_count_; + InputInterface passive_suspension_active_; + InputInterface low_prone_active_; + InputInterface symmetric_posture_target_; + InputInterface correction_inverted_; + InputInterface min_angle_deg_; + InputInterface max_angle_deg_; + InputInterface suspension_reference_angle_deg_; + + InputInterface chassis_imu_pitch_; + InputInterface chassis_imu_roll_; + InputInterface chassis_imu_pitch_rate_; + InputInterface chassis_imu_roll_rate_; + + std::array, kJointCount> joint_posture_target_angle_rad_; + std::array, kJointCount> joint_physical_angle_; + std::array, kJointCount> joint_torque_; + + std::array, kJointCount> joint_target_angle_; + std::array, kJointCount> joint_target_velocity_; + std::array, kJointCount> joint_target_acceleration_; + std::array, kJointCount> joint_angle_error_; + + pid::PidCalculator pitch_outer_pid_{}; + pid::PidCalculator pitch_inner_pid_{}; + pid::PidCalculator roll_outer_pid_{}; + pid::PidCalculator roll_inner_pid_{}; + + double active_correction_vel_limit_ = 40.0; + double active_correction_acc_limit_ = 200.0; + + bool passive_enabled_ = true; + double passive_kp_ = 0.035; + double passive_ki_ = 0.005; + double passive_deadband_ = 0.08; + double passive_integral_limit_ = 3.0; + double passive_load_lpf_alpha_ = 0.08; + double passive_max_correction_rad_ = deg_to_rad_(8.0); + double passive_correction_vel_limit_ = deg_to_rad_(180.0); + double passive_correction_acc_limit_ = deg_to_rad_(720.0); + std::array passive_load_sign_ = {-1.0, -1.0, -1.0, -1.0}; + std::array passive_load_bias_ = {0.0, 0.0, 0.0, 0.0}; + + double calibration_wait_time_ = 2.0; + double calibration_sample_time_ = 3.0; + double calibration_hold_elapsed_ = 0.0; + size_t sample_count_ = 0; + double pitch_sum_ = 0.0; + double roll_sum_ = 0.0; + bool calibration_completed_for_window_ = false; + bool calibrated_once_ = false; + double pitch_offset_value_ = 0.0; + double roll_offset_value_ = 0.0; + + bool passive_load_initialized_ = false; + std::array passive_filtered_load_proxy_ = {0.0, 0.0, 0.0, 0.0}; + std::array passive_load_integral_ = {0.0, 0.0, 0.0, 0.0}; + + std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array correction_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + std::array joint_target_active_ = {false, false, false, false}; + std::array joint_target_angle_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + std::array joint_target_acceleration_state_rad_ = {0.0, 0.0, 0.0, 0.0}; + + double joint_target_vel_limit_ = 0.0; + double joint_target_acc_limit_ = 0.0; + double suspension_target_vel_limit_ = 0.0; + double suspension_target_acc_limit_ = 0.0; + size_t last_reset_count_ = 0; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DeformableSuspension, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index ef554b02..bf4588b1 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -43,6 +43,7 @@ class DeformableInfantryGimbalController auto update() -> void override { const auto switch_right = *input_.switch_right; const auto switch_left = *input_.switch_left; + const auto keyboard = *input_.keyboard; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -51,6 +52,8 @@ class DeformableInfantryGimbalController return; } + update_pitch_lock_state(switch_left, switch_right, keyboard); + if (ctrl_hold_requested()) { update_ctrl_hold_control(); } else { @@ -147,7 +150,6 @@ class DeformableInfantryGimbalController component.register_input("/remote/mouse/velocity", mouse_velocity); component.register_input("/remote/mouse", mouse); component.register_input("/predefined/update_rate", update_rate, false); - component.register_input("/chassis/pitch_lock_active", pitch_lock_active, false); component.register_input("/gimbal/yaw/angle", yaw_angle); component.register_input("/gimbal/yaw/velocity", yaw_velocity); @@ -173,7 +175,6 @@ class DeformableInfantryGimbalController InputInterface mouse_velocity; InputInterface mouse; InputInterface update_rate; - InputInterface pitch_lock_active; InputInterface yaw_angle; InputInterface yaw_velocity; @@ -212,7 +213,7 @@ class DeformableInfantryGimbalController } output_{*this}; auto ctrl_hold_requested() const -> bool { - return input_.pitch_lock_active.ready() && *input_.pitch_lock_active; + return pitch_lock_active_; } auto update_dt() const -> double { @@ -249,12 +250,25 @@ class DeformableInfantryGimbalController } auto pitch_gravity_feedforward() const -> double { + if (ctrl_hold_active_) + return 0.0; if (!input_.pitch_angle.ready() || !std::isfinite(*input_.pitch_angle)) return 0.0; - else return pitch_gravity_ff_gain_ * std::sin(*input_.pitch_angle - pitch_gravity_ff_phase_); } + auto update_pitch_lock_state( + rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, + const rmcs_msgs::Keyboard& keyboard) -> void { + if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP + && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { + suspension_on_by_switch_ = !suspension_on_by_switch_; + } + + pitch_lock_active_ = keyboard.ctrl || suspension_on_by_switch_; + last_switch_right_ = switch_right; + } + auto activate_ctrl_hold() -> void { ctrl_hold_active_ = true; pitch_angle_pid_.reset(); @@ -316,6 +330,9 @@ class DeformableInfantryGimbalController auto reset_all_controls() -> void { deactivate_ctrl_hold(); + pitch_lock_active_ = false; + suspension_on_by_switch_ = false; + last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; gimbal_solver_.update(TwoAxisGimbalSolver::SetDisabled{}); *output_.yaw_angle_error = kNaN; *output_.pitch_angle_error = kNaN; @@ -355,6 +372,9 @@ class DeformableInfantryGimbalController double ctrl_hold_pitch_target_angle_ = 0.0; double pitch_gravity_ff_gain_ = 0.0; double pitch_gravity_ff_phase_ = 0.0; + bool pitch_lock_active_ = false; + bool suspension_on_by_switch_ = false; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; bool ctrl_hold_active_ = false; }; From 7e03c4d1f9c13cea46d84bdc17f69de2b6aeccd1 Mon Sep 17 00:00:00 2001 From: eye-on Date: Wed, 8 Jul 2026 16:17:07 +0800 Subject: [PATCH 85/90] feat(hardware): connect auto-aim interfaces to deformable infantry - Output /auto_aim/camera_transform, /auto_aim/barrel_direction, /auto_aim/yaw_velocity from hardware layer - Expose gimbal_yaw_velocity() accessor on TopBoard - Remove standalone auto-aim launch (now runs as Component in RMCS) - Bump rmcs_auto_aim_v2 submodule (single-process architecture, fire controller & tracker refactor, dynamic yaw limits) --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 16 +------------ .../hardware/deformable-infantry-omni-b.cpp | 23 +++++++++++++++++++ .../src/hardware/deformable-infantry-omni.cpp | 23 +++++++++++++++++++ 4 files changed, 48 insertions(+), 16 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 7fbe1bd4..06d7e380 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 7fbe1bd41659bae94d469f43715721313227f95b +Subproject commit 06d7e380ebc039ad3a708cce293febc25e5ded76 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index d810a08d..8246f06a 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -6,8 +6,7 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import IncludeLaunchDescription,LogInfo -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -51,19 +50,6 @@ def visit( ) ) - if is_automatic: - pass - - entities.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' - ]) - ) - ) - - - return entities diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index a654bf63..d9fb8e43 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -54,6 +54,10 @@ class DeformableInfantryOmniB using namespace rmcs_description; register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -82,6 +86,12 @@ class DeformableInfantryOmniB top_board_->update(); vt13_.update_status(); remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -636,6 +646,15 @@ class DeformableInfantryOmniB ~TopBoard() override = default; + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + void update() { gimbal_imu_.update_status(); @@ -763,6 +782,10 @@ class DeformableInfantryOmniB } OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; device::Vt13 vt13_; std::shared_ptr deformable_infantry_command_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index ff100c1d..fcdaeddf 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -49,6 +49,10 @@ class DeformableInfantryOmni using namespace rmcs_description; register_output("/tf", tf_); + register_output( + "/auto_aim/camera_transform", camera_transform_, Eigen::Isometry3d::Identity()); + register_output("/auto_aim/barrel_direction", barrel_direction_, Eigen::Vector3d::UnitX()); + register_output("/auto_aim/yaw_velocity", auto_aim_yaw_velocity_, 0.0); tf_->set_transform(Eigen::Translation3d{0.058, -0.08, 0.0}); @@ -75,6 +79,12 @@ class DeformableInfantryOmni top_board_->update(); vt13_.update_status(); remote_control_->update(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *auto_aim_yaw_velocity_ = top_board_->gimbal_yaw_velocity(); } void command_update() { @@ -608,6 +618,15 @@ class DeformableInfantryOmni ~TopBoard() override = default; + [[nodiscard]] auto gimbal_yaw_velocity() const -> double { + return *gimbal_yaw_velocity_bmi088_; + } + + void request_hard_sync_read() { + // RMCS-lite top board variant currently has no GPIO hard-sync request + // path. + } + void update() { gimbal_imu_.update_status(); @@ -734,6 +753,10 @@ class DeformableInfantryOmni } OutputInterface tf_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + OutputInterface auto_aim_yaw_velocity_; + InputInterface timestamp_; device::Vt13 vt13_; std::unique_ptr top_board_; From 491dfd75450100559d937e4843a4ec09f74fc67d Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 9 Jul 2026 17:08:00 +0800 Subject: [PATCH 86/90] fix(gimbal): update gimbal yaw velocity source --- .../src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp | 2 +- rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp index d9fb8e43..38dd8a86 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni-b.cpp @@ -647,7 +647,7 @@ class DeformableInfantryOmniB ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; + return *gimbal_yaw_velocity_imu_; } void request_hard_sync_read() { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index fcdaeddf..fd8442fb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -619,7 +619,7 @@ class DeformableInfantryOmni ~TopBoard() override = default; [[nodiscard]] auto gimbal_yaw_velocity() const -> double { - return *gimbal_yaw_velocity_bmi088_; + return *gimbal_yaw_velocity_imu_; } void request_hard_sync_read() { From 57cbf824f24dc5ef03fe59af5da4529a6e4827a5 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 9 Jul 2026 17:54:45 +0800 Subject: [PATCH 87/90] feat(chassis): add low prone toggle functionality --- .../controller/chassis/deformable_mode.hpp | 44 ++++++++++++------- 1 file changed, 27 insertions(+), 17 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index b920d0aa..f509d697 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -70,6 +70,7 @@ class DeformableChassisModeManager { complex_spin_elapsed_ = 0.0; suspension_enabled_by_toggle_ = false; passive_suspension_enabled_by_toggle_ = false; + low_prone_enabled_by_toggle_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; last_keyboard_ = rmcs_msgs::Keyboard::zero(); @@ -83,12 +84,13 @@ class DeformableChassisModeManager { const rmcs_msgs::Keyboard& keyboard, double rotary_knob, double dt) { update_mode_from_inputs_(switch_left, switch_right, keyboard); - update_suspension_toggle_from_inputs_(switch_left, switch_right); + update_low_prone_toggle_from_inputs_(switch_left, switch_right); joint_posture_state_.ctrl_low_prone_active = keyboard.ctrl; - joint_posture_state_.low_prone_active = joint_posture_state_.ctrl_low_prone_active; + joint_posture_state_.low_prone_active = + joint_posture_state_.ctrl_low_prone_active || low_prone_enabled_by_toggle_; joint_posture_state_.pitch_lock_active = - joint_posture_state_.ctrl_low_prone_active || suspension_enabled_by_toggle_; + joint_posture_state_.ctrl_low_prone_active; update_suspension_mode_from_inputs_(switch_left, switch_right, keyboard, rotary_knob); update_posture_target_from_inputs_(switch_left, switch_right, keyboard, rotary_knob, dt); @@ -216,7 +218,7 @@ class DeformableChassisModeManager { void update_suspension_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard, double rotary_knob) { - const bool passive_toggle_requested = !last_keyboard_.x && keyboard.x; + const bool keyboard_passive_toggle_requested = !last_keyboard_.x && keyboard.x; const bool remote_suspension_rotary_mode = switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::MIDDLE; const bool remote_active_toggle_requested = @@ -224,15 +226,15 @@ class DeformableChassisModeManager { const bool remote_passive_toggle_requested = remote_suspension_rotary_mode && rotary_knob_up_edge_(rotary_knob); - if ((passive_toggle_requested || remote_passive_toggle_requested) + if ((keyboard_passive_toggle_requested || remote_passive_toggle_requested) && passive_suspension_enable_) { passive_suspension_enabled_by_toggle_ = !passive_suspension_enabled_by_toggle_; if (passive_suspension_enabled_by_toggle_) suspension_enabled_by_toggle_ = false; } - const bool keyboard_toggle_requested = !last_keyboard_.e && keyboard.e; - if (keyboard_toggle_requested || remote_active_toggle_requested) + const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; + if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; const bool active_requested = @@ -257,11 +259,11 @@ class DeformableChassisModeManager { joint_posture_state_.suspension_mode == SuspensionMode::PASSIVE; } - void update_suspension_toggle_from_inputs_( + void update_low_prone_toggle_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right) { if (switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP && last_switch_right_ == rmcs_msgs::Switch::MIDDLE) { - suspension_enabled_by_toggle_ = !suspension_enabled_by_toggle_; + low_prone_enabled_by_toggle_ = !low_prone_enabled_by_toggle_; } } @@ -273,10 +275,10 @@ class DeformableChassisModeManager { const bool remote_joint_posture_rotary_mode = switch_left == rmcs_msgs::Switch::MIDDLE && switch_right == rmcs_msgs::Switch::MIDDLE; - const bool keyboard_toggle_condition = !last_keyboard_.q && keyboard.q; - const bool rotary_knob_toggle_condition = + const bool keyboard_posture_toggle_condition = !last_keyboard_.q && keyboard.q; + const bool remote_posture_toggle_condition = remote_joint_posture_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool rotary_knob_bg_toggle_condition = + const bool remote_front_back_posture_toggle_condition = remote_joint_posture_rotary_mode && rotary_knob_up_edge_(rotary_knob); const bool front_high_rear_low = !last_keyboard_.b && keyboard.b; const bool front_low_rear_high = !last_keyboard_.g && keyboard.g; @@ -295,20 +297,26 @@ class DeformableChassisModeManager { if (apply_symmetric_target_) joint_current_target_angle_.fill(current_target_angle_); - if (rotary_knob_toggle_condition || keyboard_toggle_condition - || complex_spin_toggle_condition) { - if (joint_posture_state_.suspension_active && keyboard_toggle_condition) { + const bool posture_toggle_requested = + remote_posture_toggle_condition || keyboard_posture_toggle_condition + || complex_spin_toggle_condition; + + if (posture_toggle_requested) { + if (joint_posture_state_.suspension_active) { active_suspension_base_angle_ = (std::abs(active_suspension_base_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; + current_target_angle_ = active_suspension_base_angle_; + apply_symmetric_target_ = true; + joint_current_target_angle_.fill(current_target_angle_); } else { current_target_angle_ = (std::abs(current_target_angle_ - max_angle_) < 1e-6) ? min_angle_ : max_angle_; apply_symmetric_target_ = true; joint_current_target_angle_.fill(current_target_angle_); } - } else if (rotary_knob_bg_toggle_condition) { + } else if (remote_front_back_posture_toggle_condition) { toggle_front_back_posture_target_(); } else if (front_high_rear_low) { apply_front_high_rear_low_target_(); @@ -342,7 +350,8 @@ class DeformableChassisModeManager { symmetric_joint_target_requested_(effective_joint_posture_target_deg); if (joint_posture_state_.suspension_active) { - joint_posture_state_.suspension_reference_angle_deg = active_suspension_base_angle_; + joint_posture_state_.suspension_reference_angle_deg = + low_prone_active ? min_angle_ : active_suspension_base_angle_; return; } @@ -374,6 +383,7 @@ class DeformableChassisModeManager { double complex_spin_elapsed_ = 0.0; bool suspension_enabled_by_toggle_ = false; bool passive_suspension_enabled_by_toggle_ = false; + bool low_prone_enabled_by_toggle_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); From e391f780366a77d25390cb487864cec3dc4ba152 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 9 Jul 2026 18:41:48 +0800 Subject: [PATCH 88/90] fix(chassis): adjust min_angle and add rate filter --- .../config/deformable-infantry-omni-b.yaml | 3 +- .../config/deformable-infantry-omni.yaml | 5 +-- .../chassis/deformable_suspension.cpp | 34 ++++++++++++++++++- 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index e2b5b5bd..8320459e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -58,7 +58,7 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 8.0 + min_angle: 7.5 max_angle: 58.0 active_suspension_enable: true passive_suspension_enable: true @@ -122,6 +122,7 @@ deformable_suspension: active_suspension_target_acceleration_limit_deg: 360.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 8a90a717..a8c651f1 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -56,8 +56,8 @@ deformable_infantry: chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent - min_angle: 12.0 - max_angle: 50.0 + min_angle: 7.5 + max_angle: 58.0 active_suspension_enable: true passive_suspension_enable: true spin_ratio: 1.0 @@ -120,6 +120,7 @@ deformable_suspension: active_suspension_target_acceleration_limit_deg: 360.0 active_suspension_correction_velocity_limit_deg: 720.0 active_suspension_correction_acceleration_limit_deg: 3600.0 + active_suspension_rate_lpf_cutoff_hz: 10.0 # Automatic IMU mounting-error calibration. # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 0668e6e6..7ae67648 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -10,6 +10,7 @@ #include #include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" namespace rmcs_core::controller::chassis { @@ -85,6 +86,7 @@ class DeformableSuspension if (!chassis_imu_roll_rate_.ready()) chassis_imu_roll_rate_.make_and_bind_directly(0.0); + configure_active_rate_filters_(1.0 / update_dt_()); validate_joint_feedback_inputs_(); reset_all_controls_(); last_reset_count_ = *reset_count_; @@ -107,6 +109,11 @@ class DeformableSuspension const auto posture_target_angles_rad = read_posture_target_angles_rad_(); const auto dt = update_dt_(); + configure_active_rate_filters_(1.0 / dt); + + double filtered_pitch_rate = *chassis_imu_pitch_rate_; + double filtered_roll_rate = *chassis_imu_roll_rate_; + filter_attitude_rates_(filtered_pitch_rate, filtered_roll_rate); if (*active_suspension_active_) calibrate_(*chassis_imu_pitch_, *chassis_imu_roll_, *symmetric_posture_target_, dt); @@ -115,7 +122,7 @@ class DeformableSuspension copy_joint_angle_states_(joint_angle_states); update_suspension_state_( *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, - *chassis_imu_pitch_rate_, *chassis_imu_roll_rate_, *active_suspension_active_, + filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, *passive_suspension_active_, *low_prone_active_, *min_angle_deg_, *max_angle_deg_, *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, current_joint_torques, dt); @@ -211,6 +218,8 @@ class DeformableSuspension deg_to_rad_(std::abs( get_parameter_or("active_suspension_correction_acceleration_limit_deg", 3600.0))), 1e-6); + active_rate_lpf_cutoff_hz_ = std::max( + get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); passive_enabled_ = get_parameter_or("passive_suspension_enable", true); passive_kp_ = get_parameter_or("passive_suspension_kp", 0.035); @@ -322,6 +331,8 @@ class DeformableSuspension void reset_all_controls_() { reset_attitude_(); + pitch_rate_filter_.reset(); + roll_rate_filter_.reset(); correction_state_rad_.fill(0.0); correction_velocity_state_rad_.fill(0.0); correction_acceleration_state_rad_.fill(0.0); @@ -395,6 +406,23 @@ class DeformableSuspension return any_active_value; } + void configure_active_rate_filters_(double sampling_frequency) { + const double clamped_sampling_frequency = std::max(sampling_frequency, 1e-6); + if (std::abs(active_rate_filter_sampling_hz_ - clamped_sampling_frequency) < 1e-6) + return; + + pitch_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + roll_rate_filter_.set_cutoff(active_rate_lpf_cutoff_hz_, clamped_sampling_frequency); + active_rate_filter_sampling_hz_ = clamped_sampling_frequency; + } + + void filter_attitude_rates_(double& pitch_rate, double& roll_rate) { + if (std::isfinite(pitch_rate)) + pitch_rate = pitch_rate_filter_.update(pitch_rate); + if (std::isfinite(roll_rate)) + roll_rate = roll_rate_filter_.update(roll_rate); + } + void compute_correction_targets_(double pitch_diff, double roll_diff, bool inverted) { if (inverted) { const double front_pitch_contribution = std::max(pitch_diff, 0.0); @@ -690,9 +718,13 @@ class DeformableSuspension pid::PidCalculator pitch_inner_pid_{}; pid::PidCalculator roll_outer_pid_{}; pid::PidCalculator roll_inner_pid_{}; + filter::LowPassFilter<1> pitch_rate_filter_{1.0}; + filter::LowPassFilter<1> roll_rate_filter_{1.0}; double active_correction_vel_limit_ = 40.0; double active_correction_acc_limit_ = 200.0; + double active_rate_lpf_cutoff_hz_ = 10.0; + double active_rate_filter_sampling_hz_ = 0.0; bool passive_enabled_ = true; double passive_kp_ = 0.035; From 3fb1407e965a20f30105932621d9bab638abe765 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Thu, 9 Jul 2026 23:15:47 +0800 Subject: [PATCH 89/90] refactor(chassis): remove passive suspension parameters --- .../config/deformable-infantry-omni-b.yaml | 19 --- .../config/deformable-infantry-omni.yaml | 19 --- .../controller/chassis/deformable_chassis.cpp | 5 - .../controller/chassis/deformable_mode.hpp | 32 +--- .../chassis/deformable_suspension.cpp | 159 +----------------- 5 files changed, 6 insertions(+), 228 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml index 8320459e..d733b5a8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni-b.yaml @@ -61,29 +61,10 @@ chassis_controller: min_angle: 7.5 max_angle: 58.0 active_suspension_enable: true - passive_suspension_enable: true spin_ratio: 1.0 deformable_suspension: ros__parameters: - passive_suspension_enable: true - passive_suspension_kp: 0.035 - passive_suspension_ki: 0.005 - passive_suspension_deadband: 0.08 - passive_suspension_load_lpf_alpha: 0.08 - passive_suspension_integral_limit: 3.0 - passive_suspension_max_angle_correction_deg: 8.0 - passive_suspension_correction_velocity_limit_deg: 180.0 - passive_suspension_correction_acceleration_limit_deg: 720.0 - passive_suspension_load_sign_left_front: -1.0 - passive_suspension_load_sign_left_back: -1.0 - passive_suspension_load_sign_right_back: -1.0 - passive_suspension_load_sign_right_front: -1.0 - passive_suspension_load_bias_left_front: 0.0 - passive_suspension_load_bias_left_back: 0.0 - passive_suspension_load_bias_right_back: 0.0 - passive_suspension_load_bias_right_front: 0.0 - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 8.0 active_suspension_pitch_outer_ki: 0.02 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index a8c651f1..2b35294a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -59,29 +59,10 @@ chassis_controller: min_angle: 7.5 max_angle: 58.0 active_suspension_enable: true - passive_suspension_enable: true spin_ratio: 1.0 deformable_suspension: ros__parameters: - passive_suspension_enable: true - passive_suspension_kp: 0.035 - passive_suspension_ki: 0.005 - passive_suspension_deadband: 0.08 - passive_suspension_load_lpf_alpha: 0.08 - passive_suspension_integral_limit: 3.0 - passive_suspension_max_angle_correction_deg: 8.0 - passive_suspension_correction_velocity_limit_deg: 180.0 - passive_suspension_correction_acceleration_limit_deg: 720.0 - passive_suspension_load_sign_left_front: -1.0 - passive_suspension_load_sign_left_back: -1.0 - passive_suspension_load_sign_right_back: -1.0 - passive_suspension_load_sign_right_front: -1.0 - passive_suspension_load_bias_left_front: 0.0 - passive_suspension_load_bias_left_back: 0.0 - passive_suspension_load_bias_right_back: 0.0 - passive_suspension_load_bias_right_front: 0.0 - # IMU attitude correction at min-angle stance. active_suspension_pitch_outer_kp: 12.0 active_suspension_pitch_outer_ki: 0.02 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index fc6f25fc..9cea6ef5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -52,7 +52,6 @@ class DeformableChassis register_output("/chassis/control_velocity", chassis_control_velocity_); register_output("/chassis/pitch_lock_active", pitch_lock_active_, false); register_output("/chassis/active_suspension/active", active_suspension_active_, false); - register_output("/chassis/deformable/passive_suspension_active", passive_suspension_active_, false); register_output("/chassis/deformable/low_prone_active", low_prone_active_, false); register_output("/chassis/deformable/symmetric_posture_target", symmetric_posture_target_, true); register_output("/chassis/deformable/correction_inverted", correction_inverted_, false); @@ -71,7 +70,6 @@ class DeformableChassis *mode_ = rmcs_msgs::ChassisMode::AUTO; *pitch_lock_active_ = false; *active_suspension_active_ = false; - *passive_suspension_active_ = false; *low_prone_active_ = false; *symmetric_posture_target_ = true; *correction_inverted_ = false; @@ -112,7 +110,6 @@ class DeformableChassis *mode_ = joint_mode_mgr_.mode(); *pitch_lock_active_ = joint_mode_mgr_.pitch_lock_active(); *active_suspension_active_ = joint_mode_mgr_.suspension_active(); - *passive_suspension_active_ = joint_mode_mgr_.passive_suspension_active(); *low_prone_active_ = joint_mode_mgr_.low_prone_active(); *symmetric_posture_target_ = joint_mode_mgr_.symmetric_posture_target(); *correction_inverted_ = joint_mode_mgr_.correction_inverted(); @@ -139,7 +136,6 @@ class DeformableChassis *mode_ = rmcs_msgs::ChassisMode::AUTO; *pitch_lock_active_ = false; *active_suspension_active_ = false; - *passive_suspension_active_ = false; *low_prone_active_ = false; *symmetric_posture_target_ = true; *correction_inverted_ = false; @@ -261,7 +257,6 @@ class DeformableChassis OutputInterface chassis_control_velocity_; OutputInterface pitch_lock_active_; OutputInterface active_suspension_active_; - OutputInterface passive_suspension_active_; OutputInterface low_prone_active_; OutputInterface symmetric_posture_target_; OutputInterface correction_inverted_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp index f509d697..e2667cf5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_mode.hpp @@ -18,7 +18,6 @@ class DeformableChassisModeManager { enum class SuspensionMode : uint8_t { OFF = 0, ACTIVE = 1, - PASSIVE = 2, }; struct JointPostureState { @@ -27,7 +26,6 @@ class DeformableChassisModeManager { bool low_prone_active = false; bool pitch_lock_active = false; bool suspension_active = false; - bool passive_suspension_active = false; SuspensionMode suspension_mode = SuspensionMode::OFF; bool symmetric_posture_target = true; bool spinning_forward = true; @@ -42,8 +40,7 @@ class DeformableChassisModeManager { std::clamp( node.get_parameter_or("active_suspension_base_angle", max_angle_), min_angle_ - 5.0, max_angle_)) - , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) - , passive_suspension_enable_(node.get_parameter_or("passive_suspension_enable", true)) { + , suspension_enable_(node.get_parameter_or("active_suspension_enable", false)) { current_target_angle_ = max_angle_; joint_current_target_angle_.fill(max_angle_); update_joint_posture_state_(false); @@ -55,7 +52,6 @@ class DeformableChassisModeManager { joint_posture_state_.low_prone_active = false; joint_posture_state_.pitch_lock_active = false; joint_posture_state_.suspension_active = false; - joint_posture_state_.passive_suspension_active = false; joint_posture_state_.suspension_mode = SuspensionMode::OFF; joint_posture_state_.symmetric_posture_target = true; joint_posture_state_.spinning_forward = true; @@ -69,7 +65,6 @@ class DeformableChassisModeManager { complex_spin_active_ = false; complex_spin_elapsed_ = 0.0; suspension_enabled_by_toggle_ = false; - passive_suspension_enabled_by_toggle_ = false; low_prone_enabled_by_toggle_ = false; last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; @@ -103,9 +98,6 @@ class DeformableChassisModeManager { rmcs_msgs::ChassisMode mode() const { return joint_posture_state_.mode; } bool pitch_lock_active() const { return joint_posture_state_.pitch_lock_active; } bool suspension_active() const { return joint_posture_state_.suspension_active; } - bool passive_suspension_active() const { - return joint_posture_state_.passive_suspension_active; - } bool low_prone_active() const { return joint_posture_state_.low_prone_active; } bool symmetric_posture_target() const { return joint_posture_state_.symmetric_posture_target; } bool spinning_forward() const { return joint_posture_state_.spinning_forward; } @@ -218,20 +210,10 @@ class DeformableChassisModeManager { void update_suspension_mode_from_inputs_( rmcs_msgs::Switch switch_left, rmcs_msgs::Switch switch_right, const rmcs_msgs::Keyboard& keyboard, double rotary_knob) { - const bool keyboard_passive_toggle_requested = !last_keyboard_.x && keyboard.x; const bool remote_suspension_rotary_mode = switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::MIDDLE; const bool remote_active_toggle_requested = remote_suspension_rotary_mode && rotary_knob_down_edge_(rotary_knob); - const bool remote_passive_toggle_requested = - remote_suspension_rotary_mode && rotary_knob_up_edge_(rotary_knob); - - if ((keyboard_passive_toggle_requested || remote_passive_toggle_requested) - && passive_suspension_enable_) { - passive_suspension_enabled_by_toggle_ = !passive_suspension_enabled_by_toggle_; - if (passive_suspension_enabled_by_toggle_) - suspension_enabled_by_toggle_ = false; - } const bool keyboard_active_suspension_toggle_requested = !last_keyboard_.e && keyboard.e; if (keyboard_active_suspension_toggle_requested || remote_active_toggle_requested) @@ -240,23 +222,13 @@ class DeformableChassisModeManager { const bool active_requested = suspension_enable_ && (joint_posture_state_.low_prone_active || suspension_enabled_by_toggle_); - if (active_requested) - passive_suspension_enabled_by_toggle_ = false; - - const bool passive_requested = - passive_suspension_enable_ - && passive_suspension_enabled_by_toggle_; joint_posture_state_.suspension_mode = SuspensionMode::OFF; if (active_requested) joint_posture_state_.suspension_mode = SuspensionMode::ACTIVE; - else if (passive_requested) - joint_posture_state_.suspension_mode = SuspensionMode::PASSIVE; joint_posture_state_.suspension_active = joint_posture_state_.suspension_mode == SuspensionMode::ACTIVE; - joint_posture_state_.passive_suspension_active = - joint_posture_state_.suspension_mode == SuspensionMode::PASSIVE; } void update_low_prone_toggle_from_inputs_( @@ -374,7 +346,6 @@ class DeformableChassisModeManager { double max_angle_; double active_suspension_base_angle_; bool suspension_enable_; - bool passive_suspension_enable_; double current_target_angle_; std::array joint_current_target_angle_; @@ -382,7 +353,6 @@ class DeformableChassisModeManager { bool complex_spin_active_ = false; double complex_spin_elapsed_ = 0.0; bool suspension_enabled_by_toggle_ = false; - bool passive_suspension_enabled_by_toggle_ = false; bool low_prone_enabled_by_toggle_ = false; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index 7ae67648..e8e4806d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -28,8 +28,6 @@ class DeformableSuspension register_input("/chassis/active_suspension/active", active_suspension_active_); register_input("/chassis/deformable/reset_count", reset_count_, false); - register_input( - "/chassis/deformable/passive_suspension_active", passive_suspension_active_); register_input("/chassis/deformable/low_prone_active", low_prone_active_); register_input( "/chassis/deformable/symmetric_posture_target", symmetric_posture_target_); @@ -52,9 +50,6 @@ class DeformableSuspension register_input( std::string{"/chassis/"} + kJointName[i] + "_joint/physical_angle", joint_physical_angle_[i], false); - register_input( - std::string{"/chassis/"} + kJointName[i] + "_joint/torque", joint_torque_[i], - false); register_output( std::string{"/chassis/"} + kJointName[i] + "_joint/target_physical_angle", joint_target_angle_[i], nan_); @@ -100,7 +95,6 @@ class DeformableSuspension } const auto current_physical_angles = read_feedback_(); - const auto current_joint_torques = read_joint_torques_(); if (!init_joint_targets_from_feedback_(current_physical_angles)) { publish_nan_joint_targets_(); @@ -123,9 +117,8 @@ class DeformableSuspension update_suspension_state_( *chassis_imu_pitch_ - pitch_offset_value_, *chassis_imu_roll_ - roll_offset_value_, filtered_pitch_rate, filtered_roll_rate, *active_suspension_active_, - *passive_suspension_active_, *low_prone_active_, *min_angle_deg_, *max_angle_deg_, - *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, - current_joint_torques, dt); + *low_prone_active_, *min_angle_deg_, *max_angle_deg_, + *suspension_reference_angle_deg_, *correction_inverted_, joint_angle_states, dt); const auto target_angles_rad = compute_joint_trajectory_targets_( posture_target_angles_rad, *active_suspension_active_, *low_prone_active_, @@ -221,45 +214,6 @@ class DeformableSuspension active_rate_lpf_cutoff_hz_ = std::max( get_parameter_or("active_suspension_rate_lpf_cutoff_hz", 10.0), 1e-6); - passive_enabled_ = get_parameter_or("passive_suspension_enable", true); - passive_kp_ = get_parameter_or("passive_suspension_kp", 0.035); - passive_ki_ = get_parameter_or("passive_suspension_ki", 0.005); - passive_deadband_ = std::max(get_parameter_or("passive_suspension_deadband", 0.08), 0.0); - passive_integral_limit_ = - std::max(get_parameter_or("passive_suspension_integral_limit", 3.0), 0.0); - passive_load_lpf_alpha_ = - std::clamp(get_parameter_or("passive_suspension_load_lpf_alpha", 0.08), 0.0, 1.0); - passive_max_correction_rad_ = std::max( - deg_to_rad_( - std::abs(get_parameter_or("passive_suspension_max_angle_correction_deg", 8.0))), - 1e-6); - passive_correction_vel_limit_ = std::max( - deg_to_rad_(std::abs( - get_parameter_or("passive_suspension_correction_velocity_limit_deg", 180.0))), - 1e-6); - passive_correction_acc_limit_ = std::max( - deg_to_rad_(std::abs( - get_parameter_or("passive_suspension_correction_acceleration_limit_deg", 720.0))), - 1e-6); - - passive_load_sign_[kLeftFront] = - get_parameter_or("passive_suspension_load_sign_left_front", -1.0); - passive_load_sign_[kLeftBack] = - get_parameter_or("passive_suspension_load_sign_left_back", -1.0); - passive_load_sign_[kRightBack] = - get_parameter_or("passive_suspension_load_sign_right_back", -1.0); - passive_load_sign_[kRightFront] = - get_parameter_or("passive_suspension_load_sign_right_front", -1.0); - - passive_load_bias_[kLeftFront] = - get_parameter_or("passive_suspension_load_bias_left_front", 0.0); - passive_load_bias_[kLeftBack] = - get_parameter_or("passive_suspension_load_bias_left_back", 0.0); - passive_load_bias_[kRightBack] = - get_parameter_or("passive_suspension_load_bias_right_back", 0.0); - passive_load_bias_[kRightFront] = - get_parameter_or("passive_suspension_load_bias_right_front", 0.0); - calibration_wait_time_ = std::max(get_parameter_or("chassis_imu_calibration_wait_s", 2.0), 0.0); calibration_sample_time_ = std::max(get_parameter_or("chassis_imu_calibration_sample_s", 3.0), 1e-6); @@ -276,17 +230,6 @@ class DeformableSuspension return angles; } - std::array read_joint_torques_() const { - std::array torques; - torques.fill(nan_); - - for (size_t i = 0; i < kJointCount; ++i) - if (joint_torque_[i].ready() && std::isfinite(*joint_torque_[i])) - torques[i] = *joint_torque_[i]; - - return torques; - } - std::array read_posture_target_angles_rad_() const { std::array targets{}; for (size_t i = 0; i < kJointCount; ++i) @@ -315,12 +258,6 @@ class DeformableSuspension correction_target_rad_.fill(0.0); } - void reset_passive_state_() { - passive_load_initialized_ = false; - passive_filtered_load_proxy_.fill(0.0); - passive_load_integral_.fill(0.0); - } - void reset_calibration_window_() { calibration_hold_elapsed_ = 0.0; sample_count_ = 0; @@ -340,7 +277,6 @@ class DeformableSuspension joint_target_angle_state_rad_.fill(nan_); joint_target_velocity_state_rad_.fill(0.0); joint_target_acceleration_state_rad_.fill(0.0); - reset_passive_state_(); reset_calibration_window_(); calibrated_once_ = false; pitch_offset_value_ = 0.0; @@ -449,60 +385,6 @@ class DeformableSuspension } } - void update_passive_targets_(const std::array& joint_torques, double dt) { - if (dt <= 0.0 || !std::isfinite(dt)) { - correction_target_rad_.fill(0.0); - return; - } - - std::array load_proxy{}; - for (size_t i = 0; i < kJointCount; ++i) { - if (!std::isfinite(joint_torques[i])) { - correction_target_rad_.fill(0.0); - reset_passive_state_(); - return; - } - - load_proxy[i] = passive_load_sign_[i] * (joint_torques[i] - passive_load_bias_[i]); - if (!passive_load_initialized_) { - passive_filtered_load_proxy_[i] = load_proxy[i]; - } else { - passive_filtered_load_proxy_[i] += - passive_load_lpf_alpha_ * (load_proxy[i] - passive_filtered_load_proxy_[i]); - } - } - passive_load_initialized_ = true; - - double load_average = 0.0; - for (double load : passive_filtered_load_proxy_) - load_average += load; - load_average /= static_cast(kJointCount); - - std::array raw_targets{}; - for (size_t i = 0; i < kJointCount; ++i) { - double load_error = load_average - passive_filtered_load_proxy_[i]; - if (std::abs(load_error) <= passive_deadband_) - load_error = 0.0; - - passive_load_integral_[i] = std::clamp( - passive_load_integral_[i] + load_error * dt, -passive_integral_limit_, - passive_integral_limit_); - - raw_targets[i] = passive_kp_ * load_error + passive_ki_ * passive_load_integral_[i]; - } - - double raw_target_mean = 0.0; - for (double raw_target : raw_targets) - raw_target_mean += raw_target; - raw_target_mean /= static_cast(kJointCount); - - for (size_t i = 0; i < kJointCount; ++i) { - correction_target_rad_[i] = std::clamp( - raw_targets[i] - raw_target_mean, -passive_max_correction_rad_, - passive_max_correction_rad_); - } - } - void run_correction_trajectory_( bool low_prone_override_active, double min_angle_deg, double max_angle_deg, double base_angle_deg, const std::array& base_joint_angles, @@ -554,22 +436,9 @@ class DeformableSuspension void update_suspension_state_( double pitch, double roll, double pitch_rate, double roll_rate, bool suspension_active, - bool passive_suspension_active, bool low_prone_override_active, double min_angle_deg, - double max_angle_deg, double base_angle_deg, bool correction_inverted, - const std::array& base_joint_angles, - const std::array& joint_torques, double dt) { - if (passive_suspension_active && passive_enabled_) { - reset_attitude_(); - update_passive_targets_(joint_torques, dt); - run_correction_trajectory_( - low_prone_override_active, min_angle_deg, max_angle_deg, base_angle_deg, - base_joint_angles, passive_correction_vel_limit_, passive_correction_acc_limit_, - dt); - return; - } - - reset_passive_state_(); - + bool low_prone_override_active, double min_angle_deg, double max_angle_deg, + double base_angle_deg, bool correction_inverted, + const std::array& base_joint_angles, double dt) { if (!suspension_active) { reset_attitude_(); run_correction_trajectory_( @@ -692,7 +561,6 @@ class DeformableSuspension InputInterface active_suspension_active_; InputInterface reset_count_; - InputInterface passive_suspension_active_; InputInterface low_prone_active_; InputInterface symmetric_posture_target_; InputInterface correction_inverted_; @@ -707,7 +575,6 @@ class DeformableSuspension std::array, kJointCount> joint_posture_target_angle_rad_; std::array, kJointCount> joint_physical_angle_; - std::array, kJointCount> joint_torque_; std::array, kJointCount> joint_target_angle_; std::array, kJointCount> joint_target_velocity_; @@ -726,18 +593,6 @@ class DeformableSuspension double active_rate_lpf_cutoff_hz_ = 10.0; double active_rate_filter_sampling_hz_ = 0.0; - bool passive_enabled_ = true; - double passive_kp_ = 0.035; - double passive_ki_ = 0.005; - double passive_deadband_ = 0.08; - double passive_integral_limit_ = 3.0; - double passive_load_lpf_alpha_ = 0.08; - double passive_max_correction_rad_ = deg_to_rad_(8.0); - double passive_correction_vel_limit_ = deg_to_rad_(180.0); - double passive_correction_acc_limit_ = deg_to_rad_(720.0); - std::array passive_load_sign_ = {-1.0, -1.0, -1.0, -1.0}; - std::array passive_load_bias_ = {0.0, 0.0, 0.0, 0.0}; - double calibration_wait_time_ = 2.0; double calibration_sample_time_ = 3.0; double calibration_hold_elapsed_ = 0.0; @@ -749,10 +604,6 @@ class DeformableSuspension double pitch_offset_value_ = 0.0; double roll_offset_value_ = 0.0; - bool passive_load_initialized_ = false; - std::array passive_filtered_load_proxy_ = {0.0, 0.0, 0.0, 0.0}; - std::array passive_load_integral_ = {0.0, 0.0, 0.0, 0.0}; - std::array correction_target_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array correction_state_rad_ = {0.0, 0.0, 0.0, 0.0}; std::array correction_velocity_state_rad_ = {0.0, 0.0, 0.0, 0.0}; From 264b36aa587aabcc8ec3a1dec2a0632aefa0e490 Mon Sep 17 00:00:00 2001 From: ZGZ713912 Date: Fri, 10 Jul 2026 00:17:58 +0800 Subject: [PATCH 90/90] fix(gimbal): tighten deformable yaw limit handling --- .../src/controller/chassis/deformable_suspension.cpp | 1 - .../src/controller/gimbal/two_axis_gimbal_solver.hpp | 10 ++++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp index e8e4806d..3f562ba0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_suspension.cpp @@ -103,7 +103,6 @@ class DeformableSuspension const auto posture_target_angles_rad = read_posture_target_angles_rad_(); const auto dt = update_dt_(); - configure_active_rate_filters_(1.0 / dt); double filtered_pitch_rate = *chassis_imu_pitch_rate_; double filtered_roll_rate = *chassis_imu_roll_rate_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 17562eb8..ad00c0b9 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -32,6 +32,8 @@ class TwoAxisGimbalSolver { void enable_yaw_limit( rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + if (yaw_upper_limit < yaw_lower_limit) + std::swap(yaw_upper_limit, yaw_lower_limit); yaw_cw_max_ = yaw_upper_limit; yaw_cw_min_ = yaw_lower_limit; component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); @@ -218,8 +220,12 @@ class TwoAxisGimbalSolver { const double err = std::atan2(y, x); const double target_cw = cw - err; - const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); - if (clamped_cw == target_cw) + double normalized_cw = std::fmod(target_cw, two_pi); + if (normalized_cw < 0) + normalized_cw += two_pi; + + const double clamped_cw = std::clamp(normalized_cw, yaw_cw_min_, yaw_cw_max_); + if (clamped_cw == normalized_cw) return; // delta = err_new - err = (cw - clamped_cw) - err