From 2fdab682b4160c39eb33808f84e0b269333f63ea Mon Sep 17 00:00:00 2001 From: Yukikaze2233 Date: Sat, 9 May 2026 00:51:32 +0800 Subject: [PATCH 1/3] 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 2/3] 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 91449f5c6993ac788a554c3bf450199db1ec5177 Mon Sep 17 00:00:00 2001 From: RMCS Date: Sun, 10 May 2026 22:03:53 +0800 Subject: [PATCH 3/3] 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; };