From d9aa1018c5658f1de828862188dd7e201157ad81 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 22 Jul 2026 18:23:02 +0800 Subject: [PATCH 01/15] feat: Add spin stuck detection for chassis and develop nav gimbal control --- .../controller/chassis/chassis_controller.cpp | 51 +++++++++++++++++++ .../controller/gimbal/eccentric_dual_yaw.cpp | 9 ++++ 2 files changed, 60 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index db740632..0e360d4d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -119,6 +119,7 @@ class ChassisController mode = *navigation_chassis_behavior_; } + update_spin_stuck_watchdog(mode); *mode_ = mode; } @@ -133,6 +134,51 @@ class ChassisController void reset_all_controls() { *mode_ = rmcs_msgs::ChassisMode::ALIGNMENT; *chassis_control_velocity_ = {kNaN, kNaN, kNaN}; + + spin_stuck_count_ = 0; + spin_recovery_count_ = 0; + } + + auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { + constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; + constexpr auto kSpinRecoveryTicks = std::size_t{2000}; + constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; + + using rmcs_msgs::ChassisMode; + + if (spin_recovery_count_ > 0) { + if (mode == ChassisMode::ALIGNMENT_POWERED) { + if (--spin_recovery_count_ == 0) + mode = mode_before_watchdog_; + } else { + spin_recovery_count_ = 0; + } + + spin_stuck_count_ = 0; + return; + } + + const auto spinning = mode == ChassisMode::SPIN_FAST || mode == ChassisMode::SPIN_SLOW; + if (!spinning || !chassis_velocity_.ready()) { + spin_stuck_count_ = 0; + return; + } + + const auto expected = (mode == ChassisMode::SPIN_FAST ? 0.6 : 0.3) * angular_velocity_max; + if (std::abs(chassis_velocity_->vector[2]) >= kSpinStuckAngularVelocityRatio * expected) { + spin_stuck_count_ = 0; + return; + } + + if (++spin_stuck_count_ < kSpinStuckConfirmTicks) + return; + + mode_before_watchdog_ = mode; + mode = ChassisMode::ALIGNMENT_POWERED; + spin_recovery_count_ = kSpinRecoveryTicks; + spin_stuck_count_ = 0; + + RCLCPP_WARN(get_logger(), "Spin stuck detected, disable spinning for 2s."); } void update_velocity_control() { @@ -288,6 +334,11 @@ class ChassisController OutputInterface mode_; bool spinning_forward_ = true; + + std::size_t spin_stuck_count_ = 0; + std::size_t spin_recovery_count_ = 0; + rmcs_msgs::ChassisMode mode_before_watchdog_ = rmcs_msgs::ChassisMode::AUTO; + pid::PidCalculator following_velocity_controller_{ get_parameter_or("following_velocity_kp", 8.0), get_parameter_or("following_velocity_ki", 0.0), diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 0f2734ec..2249092d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -71,6 +71,15 @@ class EccentricDualYaw // 导航控制。 if (input_.enable_navigation()) { + constexpr auto kGimbalFree = std::numeric_limits::min(); + if (*input_.navigation_enable_control && input_.navigation_toward.ready()) { + const auto& toward = *input_.navigation_toward; + if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { + enter_disabled_state(); + return; + } + } + const auto error = solver_.update( EccentricDualYawSolver::Navigation{ *input_.top_yaw_angle, From 5fbb2e2b798e1075470fa349c2e38bd62e1f6d62 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 22 Jul 2026 18:36:11 +0800 Subject: [PATCH 02/15] feat: Getter for node mixin --- .../controller/chassis/chassis_controller.cpp | 25 +++++++++---------- .../rmcs_utility/rclcpp/node_mixin.hpp | 10 ++++++++ 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 0e360d4d..7b27036e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -8,17 +8,17 @@ #include #include #include +#include namespace rmcs_core::controller::chassis { class ChassisController : public rmcs_executor::Component - , public rclcpp::Node { + , public rclcpp::Node + , public rmcs_utility::NodeMixin { public: ChassisController() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + : Node{get_component_name(), node::options()} { following_velocity_controller_.output_max = angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; @@ -51,12 +51,11 @@ class ChassisController 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."); + node::warn("Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); } if (!gimbal_yaw_angle_error_.ready()) { gimbal_yaw_angle_error_.make_and_bind_directly(0.0); - RCLCPP_WARN( - get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + node::warn("Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } if (!climbing_forward_velocity_.ready()) { @@ -178,7 +177,7 @@ class ChassisController spin_recovery_count_ = kSpinRecoveryTicks; spin_stuck_count_ = 0; - RCLCPP_WARN(get_logger(), "Spin stuck detected, disable spinning for 2s."); + node::warn("Spin stuck detected, disable spinning for 2s."); } void update_velocity_control() { @@ -306,8 +305,8 @@ class ChassisController static constexpr double kInf = std::numeric_limits::infinity(); static constexpr double kNaN = std::numeric_limits::quiet_NaN(); - const double translational_velocity_max{get_parameter_or("translational_velocity_max", 10.0)}; - const double angular_velocity_max{get_parameter_or("angular_velocity_max", 16.0)}; + const double translational_velocity_max{node::param_or("translational_velocity_max", 10.0)}; + const double angular_velocity_max{node::param_or("angular_velocity_max", 16.0)}; InputInterface joystick_right_; InputInterface joystick_left_; @@ -340,9 +339,9 @@ class ChassisController rmcs_msgs::ChassisMode mode_before_watchdog_ = rmcs_msgs::ChassisMode::AUTO; pid::PidCalculator following_velocity_controller_{ - get_parameter_or("following_velocity_kp", 8.0), - get_parameter_or("following_velocity_ki", 0.0), - get_parameter_or("following_velocity_kd", 0.0), + node::param_or("following_velocity_kp", 8.0), + node::param_or("following_velocity_ki", 0.0), + node::param_or("following_velocity_kd", 0.0), }; OutputInterface chassis_control_velocity_; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp index 0a036997..24eb14d1 100644 --- a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/rclcpp/node_mixin.hpp @@ -1,11 +1,16 @@ #pragma once #include +#include namespace rmcs_utility { struct NodeMixin { using node = NodeMixin; + static constexpr auto options() noexcept { + return rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true); + } + template auto info(this const Self& self, std::format_string fmt, Args&&... args) -> void { auto text = std::format(fmt, std::forward(args)...); @@ -39,6 +44,11 @@ struct NodeMixin { requires std::convertible_to { dst = self.template get_parameter_or(name, fallback); } + + template + auto param_or(this const auto& self, const std::string& name, const T& fallback) -> T { + return self.template get_parameter_or(name, fallback); + } }; } // namespace rmcs_utility From ccb0916030ed97064eac549ea6b87341bae7bde9 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 22 Jul 2026 20:14:48 +0800 Subject: [PATCH 03/15] wip: Chassis stuck detection --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 2 ++ .../controller/chassis/chassis_controller.cpp | 32 ++++++------------- .../chassis/chassis_power_controller.cpp | 2 +- .../include/rmcs_msgs/chassis_mode.hpp | 5 ++- 4 files changed, 17 insertions(+), 24 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 3565cc9b..07c5de96 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -123,6 +123,8 @@ gimbal_controller: chassis_controller: ros__parameters: + angular_velocity_max: 8.0 + translational_velocity_max: 10.0 following_velocity_kp: 7.0 following_velocity_ki: 0.0 following_velocity_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 7b27036e..80f9c772 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include @@ -24,18 +23,14 @@ class ChassisController following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/velocity", chassis_velocity_, false); + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, false); register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); @@ -140,31 +135,28 @@ class ChassisController auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; - constexpr auto kSpinRecoveryTicks = std::size_t{2000}; + constexpr auto kSpinRecoveryTicks = std::size_t{1000}; constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; using rmcs_msgs::ChassisMode; if (spin_recovery_count_ > 0) { - if (mode == ChassisMode::ALIGNMENT_POWERED) { - if (--spin_recovery_count_ == 0) - mode = mode_before_watchdog_; - } else { - spin_recovery_count_ = 0; - } + mode = ChassisMode::ALIGNMENT_POWERED; + + if (--spin_recovery_count_ == 0) + mode = mode_before_watchdog_; spin_stuck_count_ = 0; return; } - const auto spinning = mode == ChassisMode::SPIN_FAST || mode == ChassisMode::SPIN_SLOW; - if (!spinning || !chassis_velocity_.ready()) { + if (!rmcs_msgs::is_spining(mode) || !chassis_yaw_velocity_imu_.ready()) { spin_stuck_count_ = 0; return; } const auto expected = (mode == ChassisMode::SPIN_FAST ? 0.6 : 0.3) * angular_velocity_max; - if (std::abs(chassis_velocity_->vector[2]) >= kSpinStuckAngularVelocityRatio * expected) { + if (std::abs(*chassis_yaw_velocity_imu_) >= kSpinStuckAngularVelocityRatio * expected) { spin_stuck_count_ = 0; return; } @@ -177,7 +169,7 @@ class ChassisController spin_recovery_count_ = kSpinRecoveryTicks; spin_stuck_count_ = 0; - node::warn("Spin stuck detected, disable spinning for 2s."); + node::warn("Spin stuck detected, disable spinning for 1s."); } void update_velocity_control() { @@ -309,13 +301,9 @@ class ChassisController const double angular_velocity_max{node::param_or("angular_velocity_max", 16.0)}; InputInterface joystick_right_; - InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; InputInterface keyboard_; - InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -324,7 +312,7 @@ class ChassisController InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; - InputInterface chassis_velocity_; + InputInterface chassis_yaw_velocity_imu_; InputInterface climbing_forward_velocity_; InputInterface navigation_enable_control_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 7c701f63..b4bad88f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -112,7 +112,7 @@ class ChassisPowerController if (boost_mode_ && *supercap_enabled_) power_limit = - rmcs_msgs::need_power(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; + rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else power_limit = *chassis_power_limit_referee_; chassis_power_limit_expected_ = power_limit; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index 182b95cc..f61826ca 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -14,8 +14,11 @@ enum class ChassisMode : uint8_t { ALIGNMENT_POWERED, }; -constexpr auto need_power(ChassisMode mode) noexcept { +constexpr auto is_powered(ChassisMode mode) noexcept { return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP; } +constexpr auto is_spining(ChassisMode mode) noexcept { + return mode == ChassisMode::SPIN_SLOW || mode == ChassisMode::SPIN_FAST; +} } // namespace rmcs_msgs From e1ab051d6f8ecb27833eb4f720ce7c60728b8c85 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Jul 2026 07:46:29 +0800 Subject: [PATCH 04/15] feat: Support nav fusion control with joystick --- .../controller/chassis/chassis_controller.cpp | 9 ++- .../controller/gimbal/eccentric_dual_yaw.cpp | 64 +++++++------------ .../rmcs_msgs/include/rmcs_msgs/robot_id.hpp | 12 +++- 3 files changed, 39 insertions(+), 46 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index 80f9c772..ca5caba5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -185,8 +185,13 @@ class ChassisController if (*navigation_enable_control_) { const auto command = *navigation_command_velocity_; - if (command.array().isFinite().all()) - return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * command; + if (command.array().isFinite().all()) { + Eigen::Vector2d superimposed = command + *joystick_right_ * translational_velocity_max; + if (superimposed.norm() > translational_velocity_max) + superimposed *= translational_velocity_max / superimposed.norm(); + + return Eigen::Rotation2Dd{*gimbal_yaw_angle_} * superimposed; + } } auto keyboard = *keyboard_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 2249092d..bea90034 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -69,53 +69,35 @@ class EccentricDualYaw return; } - // 导航控制。 + const auto yaw_shift = +kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const auto pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); + + auto nav_yshift = double{0.}; + auto nav_pshift = double{0.}; if (input_.enable_navigation()) { constexpr auto kGimbalFree = std::numeric_limits::min(); - if (*input_.navigation_enable_control && input_.navigation_toward.ready()) { - const auto& toward = *input_.navigation_toward; - if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { - enter_disabled_state(); - return; - } + const auto& toward = *input_.navigation_toward; + if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { + enter_disabled_state(); + return; } - - const auto error = solver_.update( - EccentricDualYawSolver::Navigation{ - *input_.top_yaw_angle, - *input_.navigation_toward, - current_bottom_world_yaw(), - actual_yaw_pitch.second, - stored_bottom_yaw_target_, - stored_pitch_target_, - upper_limit_, - lower_limit_, - }); - apply_control(error.bottom_yaw, error.top_yaw, error.pitch); - - stored_bottom_yaw_target_ = limit_rad(current_bottom_world_yaw() + error.bottom_yaw); - stored_pitch_target_ = std::clamp( - limit_rad(actual_yaw_pitch.second + error.pitch), upper_limit_, lower_limit_); - return; + if (std::isfinite(toward.x())) + nav_yshift = limit_rad(toward.x() - stored_bottom_yaw_target_); + if (std::isfinite(toward.y())) + nav_pshift = limit_rad( + std::clamp(toward.y(), upper_limit_, lower_limit_) - stored_pitch_target_); } - // 手动控制。 - { - const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() - + kMouseSensitivity * input_.mouse_velocity->y(); - const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() - - kMouseSensitivity * input_.mouse_velocity->x(); - - stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + yaw_shift); - stored_pitch_target_ = - std::clamp(stored_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); - - const double bottom_yaw_error = - limit_rad(stored_bottom_yaw_target_ - current_bottom_world_yaw()); - const double pitch_error = limit_rad(stored_pitch_target_ - actual_yaw_pitch.second); + stored_bottom_yaw_target_ = limit_rad(stored_bottom_yaw_target_ + nav_yshift + yaw_shift); + stored_pitch_target_ = + std::clamp(stored_pitch_target_ + nav_pshift + pitch_shift, upper_limit_, lower_limit_); - apply_control(bottom_yaw_error, limit_rad(-*input_.top_yaw_angle), pitch_error); - } + apply_control( + limit_rad(+stored_bottom_yaw_target_ - current_bottom_world_yaw()), + limit_rad(-*input_.top_yaw_angle), + limit_rad(+stored_pitch_target_ - actual_yaw_pitch.second)); } private: diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp index 61e063e2..2c82568b 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp @@ -67,11 +67,17 @@ class RobotId { constexpr bool operator==(const Value value) const { return value_ == value; } constexpr bool operator!=(const Value value) const { return value_ != value; } - constexpr RobotColor color() const { + constexpr RobotColor color() const noexcept { + if (value_ == Value::UNKNOWN) { + return RobotColor::UNKNOWN; + } return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } - constexpr ArmorID id() const { + constexpr ArmorID id() const noexcept { + if (value_ == Value::UNKNOWN) { + return ArmorID::Unknown; + } return value_ > 100 ? static_cast(value_ - 100) : static_cast(value_); } @@ -79,4 +85,4 @@ class RobotId { Value value_; }; -} // namespace rmcs_msgs \ No newline at end of file +} // namespace rmcs_msgs From 84e003be2b28f0066a7f1154ff246ced16bda4de Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Jul 2026 07:47:08 +0800 Subject: [PATCH 05/15] feat: Update remote control timeout logic --- rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp | 5 ++++- .../rmcs_core/src/hardware/device/remote_control.hpp | 12 ++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp | 5 ++++- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index ba0ec4fa..51895755 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -154,6 +154,8 @@ class Dr16 { bool valid() const noexcept { return valid_; } + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + double rotary_knob() const { return rotary_knob_; } double mouse_wheel() const { return mouse_wheel_; } @@ -190,7 +192,7 @@ class Dr16 { static constexpr auto kFreshTimeout = std::chrono::milliseconds(500); void refresh_validity(const TimePoint now) { - if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) return; reset_remote_control_state(); @@ -278,6 +280,7 @@ class Dr16 { rmcs_msgs::Switch rotary_knob_switch_ = rmcs_msgs::Switch::UNKNOWN; TimePoint last_remote_control_received_at_ = TimePoint::min(); bool valid_ = false; + bool timeout_enabled_ = true; }; } // namespace rmcs_core::hardware::device diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp index c61b874e..689436bc 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/remote_control.hpp @@ -51,6 +51,8 @@ class RemoteControl { void register_vt13(Vt13* vt13) { vt13_ = vt13; } void update() { + update_timeout_interlock(); + const auto control_source = select_control_source(); const auto snapshot = build_snapshot(control_source); @@ -97,6 +99,16 @@ class RemoteControl { rmcs_msgs::Keyboard keyboard = rmcs_msgs::Keyboard::zero(); }; + // 超时互锁：仅当对方 valid 时本设备才允许超时失效，保证至少一路不失效 + auto update_timeout_interlock() const -> void { + const auto dr16_ok = dr16_ && dr16_->valid(); + const auto vt13_ok = vt13_ && vt13_->valid(); + if (dr16_) + dr16_->set_timeout_enabled(vt13_ok); + if (vt13_) + vt13_->set_timeout_enabled(dr16_ok); + } + ControlSource select_control_source() const { if (vt13_ && vt13_->valid()) { switch (vt13_->mode_switch()) { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp index 4272bfda..32799982 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/vt13.hpp @@ -97,6 +97,8 @@ class Vt13 { ModeSwitch mode_switch() const noexcept { return mode_switch_; } bool valid() const noexcept { return valid_; } + void set_timeout_enabled(bool enabled) { timeout_enabled_ = enabled; } + const Eigen::Vector2d& joystick_left() const noexcept { return joystick_left_; } const Eigen::Vector2d& joystick_right() const noexcept { return joystick_right_; } @@ -278,7 +280,7 @@ class Vt13 { } void refresh_validity(const TimePoint now) { - if (!valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) + if (!timeout_enabled_ || !valid_ || now - last_remote_control_received_at_ <= kFreshTimeout) return; reset_remote_control_state(); @@ -316,6 +318,7 @@ class Vt13 { TimePoint last_statistics_log_time_ = TimePoint::min(); bool valid_ = false; + bool timeout_enabled_ = true; std::size_t peak_readable_ = 0; std::size_t remote_success_count_ = 0; std::size_t verification_failures_ = 0; From 5bcbda8f966dbc6fd6b03494c3afab07d22308f8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Jul 2026 07:47:32 +0800 Subject: [PATCH 06/15] chore: Update sentry config --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 07c5de96..73015dfe 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -24,8 +24,8 @@ rmcs_executor: - rmcs::navigation::Navigation -> rmcs_navigation - # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - rmcs::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster @@ -34,13 +34,13 @@ rmcs_executor: rmcs_navigation: ros__parameters: command_vel_name: "/cmd_vel" - endpoint: "train" + endpoint: "otaku" enable_goal_topic_forward: true auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 3000.0 + exposure_us: 2000.0 gain: 8.0 framerate: 120.0 invert_image: true @@ -59,12 +59,13 @@ auto_aim_recorder: auto_aim_component: ros__parameters: manual_shoot: true + enable_rune: true camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 shoot_delay: 0.1 - offset_yaw: -1.0 - offset_pitch: +0.0 + offset_yaw: -0.5 + offset_pitch: +1.0 attack_window: 120.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 @@ -123,7 +124,7 @@ gimbal_controller: chassis_controller: ros__parameters: - angular_velocity_max: 8.0 + angular_velocity_max: 10.0 translational_velocity_max: 10.0 following_velocity_kp: 7.0 following_velocity_ki: 0.0 From 375f34236e56f60d1574dc04b297e285ca1cbde1 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Jul 2026 07:48:20 +0800 Subject: [PATCH 07/15] build: Add detect path to update-image workflow --- .github/workflows/update-image.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 78e039a0..75a4fbfc 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -8,6 +8,7 @@ on: paths: - Dockerfile - .github/workflows/update-image.yml + - .script/template/ - .script/build-rmcs-cross - rmcs_ws/toolchain.cmake From 1e0562727c34229491712ad92b8a4668f62513df Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Jul 2026 07:55:27 +0800 Subject: [PATCH 08/15] chore: Update auto aim v2 --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index d977534f..ab88ee07 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit d977534fa7f1e50293ede5c1953834b30a02a0ff +Subproject commit ab88ee07910c366f0ceaad1a88264198d7f0047d From a6f13901b9064e2408be1dfa9beda3c0e5bcaaf8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 25 Jul 2026 05:49:51 +0800 Subject: [PATCH 09/15] wip: Fill basic context and utils for climber --- .../chassis/climber/co_schduler.hpp | 218 ++++++++++++++++++ .../chassis/climber/stick_group.hpp | 179 ++++++++++++++ .../chassis/climber/track_group.hpp | 146 ++++++++++++ .../src/controller/chassis/sentry_climber.cpp | 152 ++++++++++++ 4 files changed, 695 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp new file mode 100644 index 00000000..c6c5192b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp @@ -0,0 +1,218 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core { + +// 协程调度器：对齐 rmcs-navigation Lua 调度器语义 +// - 任务携带 resume_request 谓词，每拍先问谓词，true 才 resume +// - 等待原语挂起时替换谓词，且至少让出一拍（控制循环时序确定性） +// - append 返回弱持有 Handle，cancel 为共享标志，任务消亡后 no-op +struct CoSchduler { + struct Task { + struct promise_type { + std::function resume_request{[] { return true; }}; + std::exception_ptr error{}; + + static constexpr auto initial_suspend() noexcept { return std::suspend_always{}; } + static constexpr auto final_suspend() noexcept { return std::suspend_always{}; } + + auto get_return_object(); + + static constexpr auto return_void() noexcept {} + + auto unhandled_exception() noexcept { error = std::current_exception(); } + + // co_yield {} 等价于 Tick：下拍唤醒并重置谓词 + auto yield_value(std::monostate) noexcept { + resume_request = [] { return true; }; + return std::suspend_always{}; + } + }; + + explicit Task(std::coroutine_handle handle) noexcept + : handle_{handle} {} + + Task(const Task&) = delete; + Task& operator=(const Task&) = delete; + + Task(Task&& other) noexcept + : handle_{std::exchange(other.handle_, {})} {} + + auto operator=(Task&& other) noexcept -> Task& { + if (this != &other) { + if (handle_) + handle_.destroy(); + handle_ = std::exchange(other.handle_, {}); + } + return *this; + } + + ~Task() { + if (handle_) + handle_.destroy(); + } + + auto release() noexcept { return std::exchange(handle_, {}); } + + private: + std::coroutine_handle handle_{}; + }; + + // co_await Tick{}：下一拍唤醒 + struct Tick { + static constexpr auto await_ready() noexcept { return false; } + + template + static auto await_suspend(std::coroutine_handle handle) { + handle.promise().resume_request = [] { return true; }; + } + + static constexpr auto await_resume() noexcept {} + }; + + // co_await Sleep{ duration }：到点唤醒（steady_clock，至少一拍） + struct Sleep { + std::chrono::steady_clock::duration duration; + + static constexpr auto await_ready() noexcept { return false; } + + template + auto await_suspend(std::coroutine_handle handle) { + const auto deadline = std::chrono::steady_clock::now() + duration; + handle.promise().resume_request = [deadline] { + return std::chrono::steady_clock::now() >= deadline; + }; + } + + static constexpr auto await_resume() noexcept {} + }; + + // co_await WaitUntil{ .monitor = ..., .timeout = ... }：条件满足或超时唤醒 + // await_resume 返回 is_timeout + struct WaitUntil { + std::function monitor; + std::chrono::steady_clock::duration timeout = std::chrono::steady_clock::duration::max(); + + static constexpr auto await_ready() noexcept { return false; } + + template + auto await_suspend(std::coroutine_handle handle) { + deadline = std::chrono::steady_clock::now() + timeout; + handle.promise().resume_request = [this] { + return monitor() || std::chrono::steady_clock::now() >= deadline; + }; + } + + auto await_resume() const noexcept { + return std::chrono::steady_clock::now() >= deadline && !monitor(); + } + + // 实现细节：保持聚合属性以支持指派初始化，外部不应访问 + std::chrono::steady_clock::time_point deadline{}; + }; + + struct Slot { + explicit Slot(std::coroutine_handle handle) noexcept + : handle{handle} {} + + Slot(const Slot&) = delete; + Slot& operator=(const Slot&) = delete; + + ~Slot() { + if (handle) + handle.destroy(); + } + + std::coroutine_handle handle; + std::atomic_bool cancelled{false}; + }; + + // 取消句柄：弱持有任务，任务消亡后操作均为 no-op + struct Handle { + auto cancel() const { + if (const auto locked = slot.lock()) + locked->cancelled.store(true, std::memory_order::relaxed); + } + + auto done() const { + const auto locked = slot.lock(); + return !locked || locked->cancelled.load(std::memory_order::relaxed) + || locked->handle.done(); + } + + private: + friend struct CoSchduler; + + explicit Handle(std::weak_ptr slot) noexcept + : slot{std::move(slot)} {} + + std::weak_ptr slot; + }; + + // 接管任务所有权，返回取消句柄；fire-and-forget 合法 + auto append(Task task) { + auto slot = std::make_shared(task.release()); + pending_.push_back(slot); + return Handle{slot}; + } + + auto spin_once() { + slots_.insert(slots_.end(), pending_.begin(), pending_.end()); + pending_.clear(); + + auto error = std::exception_ptr{}; + + std::erase_if(slots_, [&](const std::shared_ptr& slot) { + if (slot->cancelled.load(std::memory_order::relaxed)) + return true; + + auto& promise = slot->handle.promise(); + + if (slot->handle.done()) { + if (promise.error && !error) + error = promise.error; + return true; + } + + if (promise.resume_request()) { + slot->handle.resume(); + + if (slot->handle.done()) { + if (promise.error && !error) + error = promise.error; + return true; + } + } + + return false; + }); + + if (error) + std::rethrow_exception(error); + } + + // 急停清场：销毁全部任务帧，协程局部变量正常析构 + auto stop_all() { + slots_.clear(); + pending_.clear(); + } + +private: + std::vector> slots_{}; + std::vector> pending_{}; +}; + +inline auto CoSchduler::Task::promise_type::get_return_object() { + return Task{std::coroutine_handle::from_promise(*this)}; +} + +} // namespace rmcs_core diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp new file mode 100644 index 00000000..b599b8df --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -0,0 +1,179 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::chassis::climber { + +struct StickGroup { + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + template + using InputInterface = rmcs_executor::Component::InputInterface; + + template + using OutputInterface = rmcs_executor::Component::OutputInterface; + + enum class State { + kFree, + kHold, + kDrop, + kRise, + kLand, + } state = State::kFree; + + struct Config { + double speed_drop; + double speed_rise; + + double rise_torque_limit; + + double land_speed_begin; + double land_speed_final; + double land_tau; + double land_torque_limit; + + double blocked_torque_threshold; + double blocked_speed_threshold; + + double kp; + double ki; + double kd; + double sync_coefficient; + + auto get_speed(State state, double land_elapsed = 0.0) const noexcept { + switch (state) { + case State::kFree: return kNaN; + case State::kHold: return 0.0; + case State::kDrop: return +speed_drop; + case State::kRise: return -speed_rise; + case State::kLand: { + const auto diff = land_speed_begin - land_speed_final; + const auto factor = std::exp(-land_elapsed / land_tau); + return -1 * (land_speed_final + diff * factor); + } + } + std::unreachable(); + } + + auto get_torque_limit(State state) const noexcept { + switch (state) { + case State::kRise: return rise_torque_limit; + case State::kLand: return land_torque_limit; + default: return std::numeric_limits::infinity(); + } + } + } config; + + rmcs_executor::Component& command; + + // Interfaces + InputInterface l_velocity; + InputInterface r_velocity; + InputInterface l_torque; + InputInterface r_torque; + + OutputInterface l_control_torque; + OutputInterface r_control_torque; + + // PID + pid::MatrixPidCalculator<2> velocity_pid; + + std::chrono::steady_clock::time_point land_start_timestamp; + + explicit StickGroup(rmcs_executor::Component& command, const Config& config) + : config{config} + , command{command} + , velocity_pid{config.kp, config.ki, config.kd} { + + command.register_input("/chassis/climber/left_back_motor/velocity", l_velocity); + command.register_input("/chassis/climber/right_back_motor/velocity", r_velocity); + command.register_input("/chassis/climber/left_back_motor/torque", l_torque); + command.register_input("/chassis/climber/right_back_motor/torque", r_torque); + + command.register_output( + "/chassis/climber/left_back_motor/control_torque", l_control_torque, kNaN); + command.register_output( + "/chassis/climber/right_back_motor/control_torque", r_control_torque, kNaN); + } + + auto spin_once() { + const auto land_elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - land_start_timestamp) + .count(); + const auto target_speed = config.get_speed(state, land_elapsed); + + if (std::isnan(target_speed)) { + *l_control_torque = kNaN; + *r_control_torque = kNaN; + return; + } + + auto torque = Eigen::Vector2d{}; + { + const auto setpoint_error = Eigen::Vector2d{ + target_speed - *l_velocity, + target_speed - *r_velocity, + }; + const auto relative_velocity = Eigen::Vector2d{ + *l_velocity - *r_velocity, + *r_velocity - *l_velocity, + }; + + torque = + velocity_pid.update(setpoint_error - config.sync_coefficient * relative_velocity); + } + + { + const auto torque_limit = config.get_torque_limit(state); + + if (target_speed < 0.0 && std::isfinite(torque_limit) && torque_limit > 0.0) { + const auto peak = std::max(std::abs(torque[0]), std::abs(torque[1])); + if (peak > torque_limit) { + torque *= torque_limit / peak; + } + } + + *l_control_torque = torque[0]; + *r_control_torque = torque[1]; + } + } + + auto set_state(State target) { + if (state == target) + return; + + state = target; + velocity_pid.reset(); + + if (target == State::kLand) { + land_start_timestamp = std::chrono::steady_clock::now(); + } + } + + auto get_state() const noexcept { return state; } + + auto get_block() const noexcept { + const auto is_blocked = [](double torque, double velocity, double torque_threshold, + double velocity_threshold) { + return std::abs(torque) > torque_threshold && std::abs(velocity) < velocity_threshold; + }; + + return is_blocked( + *l_torque, *l_velocity, config.blocked_torque_threshold, + config.blocked_speed_threshold) + || is_blocked( + *r_torque, *r_velocity, config.blocked_torque_threshold, + config.blocked_speed_threshold); + } +}; + +} // namespace rmcs_core::controller::chassis::climber diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp new file mode 100644 index 00000000..fe821517 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/track_group.hpp @@ -0,0 +1,146 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::chassis::climber { + +struct TrackGroup { + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + template + using InputInterface = rmcs_executor::Component::InputInterface; + + template + using OutputInterface = rmcs_executor::Component::OutputInterface; + + enum class State { + kFree, + kHold, + kRush, + } state = State::kFree; + + struct Config { + double speed_rush; + + double kp; + double ki; + double kd; + double sync_coefficient; + + double power_estimate_bias; + double power_estimate_k_tau2; + double power_estimate_k_mech; + + auto get_speed(State state) const noexcept { + switch (state) { + case State::kFree: return kNaN; + case State::kHold: return 0.0; + case State::kRush: return speed_rush; + } + std::unreachable(); + } + } config; + + rmcs_executor::Component& command; + + // Interfaces + InputInterface l_velocity; + InputInterface r_velocity; + InputInterface l_max_torque; + InputInterface r_max_torque; + InputInterface control_power_limit; + + OutputInterface l_control_torque; + OutputInterface r_control_torque; + + // PID + pid::MatrixPidCalculator<2> velocity_pid; + + explicit TrackGroup(rmcs_executor::Component& command, const Config& config) + : config{config} + , command{command} + , velocity_pid{config.kp, config.ki, config.kd} { + + command.register_input("/chassis/climber/left_front_motor/velocity", l_velocity); + command.register_input("/chassis/climber/right_front_motor/velocity", r_velocity); + command.register_input("/chassis/climber/left_front_motor/max_torque", l_max_torque); + command.register_input("/chassis/climber/right_front_motor/max_torque", r_max_torque); + command.register_input("/chassis/climber/front/control_power_limit", control_power_limit); + + command.register_output( + "/chassis/climber/left_front_motor/control_torque", l_control_torque, kNaN); + command.register_output( + "/chassis/climber/right_front_motor/control_torque", r_control_torque, kNaN); + } + + auto spin_once() { + const auto target_speed = config.get_speed(state); + + if (std::isnan(target_speed)) { + *l_control_torque = kNaN; + *r_control_torque = kNaN; + return; + } + + auto torque = Eigen::Vector2d{}; + { + const auto setpoint_error = Eigen::Vector2d{ + target_speed - *l_velocity, + target_speed - *r_velocity, + }; + const auto relative_speed = Eigen::Vector2d{ + *l_velocity - *r_velocity, + *r_velocity - *l_velocity, + }; + + torque = velocity_pid.update(setpoint_error - config.sync_coefficient * relative_speed); + } + + { + const auto power_limit = *control_power_limit; + if (power_limit <= 0.0) { + *l_control_torque = 0.0; + *r_control_torque = 0.0; + return; + } + + auto l_torque = std::clamp(torque[0], -*l_max_torque, *l_max_torque); + auto r_torque = std::clamp(torque[1], -*r_max_torque, *r_max_torque); + + const auto estimated_power = + config.power_estimate_bias + + config.power_estimate_k_tau2 * (std::pow(l_torque, 2) + std::pow(r_torque, 2)) + + config.power_estimate_k_mech + * (std::abs(l_torque * *l_velocity) + std::abs(r_torque * *r_velocity)); + + if (estimated_power > power_limit && estimated_power > 0.0) { + const auto scale = std::clamp(power_limit / estimated_power, 0.0, 1.0); + l_torque *= scale; + r_torque *= scale; + } + + *l_control_torque = l_torque; + *r_control_torque = r_torque; + } + } + + auto set_state(State target) { + if (state == target) + return; + + state = target; + velocity_pid.reset(); + } + + auto get_state() const noexcept { return state; } +}; + +} // namespace rmcs_core::controller::chassis::climber diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp new file mode 100644 index 00000000..9329f595 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -0,0 +1,152 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/climber/co_schduler.hpp" +#include "controller/chassis/climber/stick_group.hpp" +#include "controller/chassis/climber/track_group.hpp" + +namespace rmcs_core::controller::chassis { + +class SentryClimber + : public rmcs_executor::Component + , public rclcpp::Node + , public rmcs_utility::NodeMixin { + + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + + struct SimpleComponent : public rmcs_executor::Component { + std::function fn; + + template + explicit SimpleComponent(Fn&& fn) + : fn{std::forward(fn)} {} + + auto update() -> void override { fn(); } + }; + + struct Context { + // 遥控 + InputInterface l_switch; + InputInterface r_switch; + InputInterface keyboard; + InputInterface rotary_knob; + + // 姿态 + InputInterface chassis_pitch; + InputInterface gimbal_yaw_angle; + InputInterface gimbal_yaw_error; + InputInterface gimbal_yaw_speed; + + // 输出（下游契约，话题名不可改） + OutputInterface climb_speed; + + auto bind(Component& component) noexcept { + component.register_input("/remote/switch/left", l_switch, false); + component.register_input("/remote/switch/right", r_switch, false); + component.register_input("/remote/keyboard", keyboard, false); + component.register_input("/remote/rotary_knob_switch", rotary_knob, false); + + component.register_input("/chassis/pitch_imu", chassis_pitch, false); + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle, false); + component.register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_error, false); + component.register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_speed, false); + + component.register_output("/chassis/climber/speed", climb_speed, kNaN); + } + + auto load_fallback(std::invocable auto&& handler) { + using namespace rmcs_msgs; + + const auto ensure_bind = + [&](InputInterface& input, T default_value, std::string_view name) { + if (input.ready() == false) { + input.make_and_bind_directly(default_value); + std::invoke(handler, name); + } + }; + + ensure_bind(l_switch, Switch::UNKNOWN, "l_switch"); + ensure_bind(r_switch, Switch::UNKNOWN, "r_switch"); + ensure_bind(keyboard, Keyboard::zero(), "keyboard"); + ensure_bind(rotary_knob, Switch::UNKNOWN, "rotary_knob"); + + ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); + ensure_bind(gimbal_yaw_angle, 0.0, "gimbal_yaw_angle"); + ensure_bind(gimbal_yaw_error, 0.0, "gimbal_yaw_error"); + ensure_bind(gimbal_yaw_speed, 0.0, "gimbal_yaw_speed"); + } + } context; + + std::shared_ptr status_component{ + create_partner_component( + get_component_name() + "_status", [this] { update_status(); }), + }; + OutputInterface climb_status; // T climbing, F done or idle + + std::unique_ptr track_group; + std::unique_ptr stick_group; + CoSchduler schduler; + + auto update_status() -> void {} + +public: + SentryClimber() + : Node{get_component_name(), node::options()} { + using namespace climber; + { + const auto config = TrackGroup::Config{ + .speed_rush = node::param_or("track_group.speed_rush", 20.0), + + .kp = node::param_or("track_group.kp", 1.0), + .ki = node::param_or("track_group.ki", 0.), + .kd = node::param_or("track_group.kd", 0.5), + .sync_coefficient = node::param_or("track_group.sync_coefficient", 0.2), + + .power_estimate_bias = node::param_or("track_group.power_estimate_bias", 0.0), + .power_estimate_k_tau2 = node::param_or("track_group.power_estimate_k_tau2", 1.0), + .power_estimate_k_mech = node::param_or("track_group.power_estimate_k_mech", 1.0), + }; + track_group = std::make_unique(*this, config); + } + { + const auto config = StickGroup::Config{ + .speed_drop = node::param_or("stick_group.speed_drop", 30.0), + .speed_rise = node::param_or("stick_group.speed_rise", 60.0), + + .rise_torque_limit = node::param_or("stick_group.rise_torque_limit", 0.5), + + .land_speed_begin = node::param_or("stick_group.land_speed_begin", 30.0), + .land_speed_final = node::param_or("stick_group.land_speed_final", 2.0), + .land_tau = node::param_or("stick_group.land_tau", 0.4), + .land_torque_limit = node::param_or("stick_group.land_torque_limit", 8.0), + + .blocked_torque_threshold = + node::param_or("stick_group.blocked_torque_threshold", 0.1), + .blocked_speed_threshold = + node::param_or("stick_group.blocked_speed_threshold", 0.1), + + .kp = node::param_or("stick_group.kp", 0.5), + .ki = node::param_or("stick_group.ki", 0.), + .kd = node::param_or("stick_group.kd", 0.), + .sync_coefficient = node::param_or("stick_group.sync_coefficient", 0.2), + }; + stick_group = std::make_unique(*this, config); + } + + context.bind(*this); + } + + auto before_updating() -> void override { + context.load_fallback([this](std::string_view name) { + node::warn("Failed to fetch input '{}'. Bind to fallback.", name); + }); + } +}; + +} // namespace rmcs_core::controller::chassis From ef9c965419a4dbcd0d6591c2946a7ac255a7bf40 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 25 Jul 2026 11:27:30 +0800 Subject: [PATCH 10/15] refactor: Climber for sentry --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 84 ++- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/chassis/chassis_controller.cpp | 85 ++- .../chassis/climber/co_schduler.hpp | 31 +- .../chassis/climber/stick_group.hpp | 15 +- .../chassis/hero_chassis_controller.cpp | 1 + .../src/controller/chassis/sentry_climber.cpp | 548 ++++++++++++++++-- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 5 + .../include/rmcs_msgs/chassis_mode.hpp | 4 +- 9 files changed, 647 insertions(+), 127 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 73015dfe..baa9d408 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -17,16 +17,16 @@ rmcs_executor: - 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::ChassisClimberController -> climber_controller + - rmcs_core::controller::chassis::SentryClimber -> sentry_climber - rmcs_core::controller::chassis::ChassisController -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - - rmcs::navigation::Navigation -> rmcs_navigation + # - rmcs::navigation::Navigation -> rmcs_navigation - - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - - rmcs::AutoAimCapturerComponent -> auto_aim_capturer - - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + # - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -80,7 +80,6 @@ value_broadcaster: - /gimbal/yaw/velocity_imu - /gimbal/pitch/velocity_imu -# The positive direction is the one that battery exists sentry_hardware: ros__parameters: board_serial_bottom_board: "af-b4e5" @@ -130,30 +129,55 @@ chassis_controller: following_velocity_ki: 0.0 following_velocity_kd: 0.0 -climber_controller: - ros__parameters: - front_climber_velocity: 20.0 - back_climber_velocity: 30.0 - auto_climb_support_retract_velocity_fast: 60.0 - auto_climb_support_retract_velocity_slow: 20.0 - auto_climb_approach_chassis_velocity: 1.0 - auto_climb_support_deploy_chassis_velocity: 0.3 - auto_climb_support_retract_chassis_velocity: 0.3 - auto_climb_dash_chassis_velocity: 3.0 - first_stair_dash_leveled_pitch_threshold: 0.05 - second_stair_dash_leveled_pitch_threshold: -0.09 - sync_coefficient: 0.2 - first_stair_approach_pitch: 0.585 - second_stair_approach_pitch: 0.365 - front_kp: 1.0 - front_ki: 0.0 - front_kd: 0.5 - front_power_estimate_bias: 0.0 - front_power_estimate_k_tau2: 1.0 - front_power_estimate_k_mech: 1.0 - back_kp: 0.5 - back_ki: 0.0 - back_kd: 0.0 +sentry_climber: + ros__parameters: + track_group: + speed_rush: 20.0 + kp: 1.0 + ki: 0.0 + kd: 0.5 + sync_coefficient: 0.2 + power_estimate_bias: 0.0 + power_estimate_k_tau2: 1.0 + power_estimate_k_mech: 1.0 + stick_group: + speed_drop: 30.0 + speed_rise: 60.0 + rise_torque_limit: 0.5 + land_speed_begin: 80.0 + land_speed_final: 10.0 + land_duration: 0.5 + land_torque_limit: 8.0 + blocked_torque_threshold: 0.1 + blocked_speed_threshold: 0.1 + kp: 0.5 + ki: 0.0 + kd: 0.0 + sync_coefficient: 0.2 + block_hold: 0.05 + align: + err: 0.20 + w: 0.2 + hold: 0.05 + timeout: 15.0 + climb: + approach_pitch: 0.585 + leveled_pitch: 0.05 + approach_vx: 1.2 + deploy_vx: 0.3 + dash_vx: 3.0 + retract_vx: 0.3 + dash_min: 0.1 + dash_timeout: 3.0 + stick_timeout: 8.0 + approach_timeout: 8.0 + land: + dash_vx: 0.8 + soft_vx: 0.4 + land_pitch: 0.15 + stick_timeout: 8.0 + soft_timeout: 3.0 + settle_timeout: 8.0 friction_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1803a1d4..95777e34 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -16,6 +16,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index ca5caba5..88d03348 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -1,5 +1,8 @@ #include "controller/pid/pid_calculator.hpp" +#include +#include + #include #include #include @@ -19,7 +22,7 @@ class ChassisController ChassisController() : Node{get_component_name(), node::options()} { - following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_max = +angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); @@ -31,7 +34,10 @@ class ChassisController register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, false); - register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + + register_input("/chassis/climber/direction", chassis_climb_direction_, false); + register_input("/chassis/climber/speed", chassis_climb_speed_, false); + register_input("/chassis/climber/measure_yaw", chassis_measure_yaw_, false); register_input("/rmcs_navigation/enable_control", navigation_enable_control_, false); register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); @@ -53,8 +59,14 @@ class ChassisController node::warn("Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); } - if (!climbing_forward_velocity_.ready()) { - climbing_forward_velocity_.make_and_bind_directly(kNaN); + if (!chassis_climb_direction_.ready()) { + chassis_climb_direction_.make_and_bind_directly(kNaN); + } + if (!chassis_climb_speed_.ready()) { + chassis_climb_speed_.make_and_bind_directly(kNaN); + } + if (!chassis_measure_yaw_.ready()) { + chassis_measure_yaw_.make_and_bind_directly(kNaN); } if (!navigation_enable_control_.ready()) { @@ -71,9 +83,9 @@ class ChassisController void update() override { using namespace rmcs_msgs; - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + const auto switch_right = *switch_right_; + const auto switch_left = *switch_left_; + const auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -113,6 +125,12 @@ class ChassisController mode = *navigation_chassis_behavior_; } + if (climb_active()) { + mode = ChassisMode::CLIMB; + } else if (mode == ChassisMode::CLIMB) { + mode = ChassisMode::AUTO; + } + update_spin_stuck_watchdog(mode); *mode_ = mode; } @@ -131,6 +149,7 @@ class ChassisController spin_stuck_count_ = 0; spin_recovery_count_ = 0; + following_velocity_controller_.reset(); } auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { @@ -171,7 +190,6 @@ class ChassisController node::warn("Spin stuck detected, disable spinning for 1s."); } - void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); auto angular_velocity = update_angular_velocity_control(); @@ -179,14 +197,33 @@ class ChassisController chassis_control_velocity_->vector << translational_velocity, angular_velocity; } + auto climb_active() const -> bool { + return std::isfinite(*chassis_climb_direction_) && std::isfinite(*chassis_climb_speed_) + && std::isfinite(*chassis_measure_yaw_); + } + + static auto normalize_signed_angle(double angle) noexcept { + constexpr auto kTwoPi = 2.0 * std::numbers::pi; + while (angle >= std::numbers::pi) + angle -= kTwoPi; + while (angle < -std::numbers::pi) + angle += kTwoPi; + return angle; + } + Eigen::Vector2d update_translational_velocity_control() { - if (!std::isnan(*climbing_forward_velocity_)) - return {*climbing_forward_velocity_, 0.0}; + using namespace rmcs_msgs; + + if (*mode_ == ChassisMode::CLIMB) { + // speed 以底盘正向 direction 为正向：上坡为正前进，下坡为负倒车 + return {*chassis_climb_speed_, 0.0}; + } if (*navigation_enable_control_) { const auto command = *navigation_command_velocity_; if (command.array().isFinite().all()) { - Eigen::Vector2d superimposed = command + *joystick_right_ * translational_velocity_max; + Eigen::Vector2d superimposed = + command + *joystick_right_ * translational_velocity_max; if (superimposed.norm() > translational_velocity_max) superimposed *= translational_velocity_max / superimposed.norm(); @@ -212,17 +249,6 @@ class ChassisController double angular_velocity = 0.0; double chassis_control_angle = kNaN; - if (!std::isnan(*climbing_forward_velocity_)) { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; - angular_velocity = following_velocity_controller_.update(err); - - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = chassis_control_angle; - return angular_velocity; - } - using namespace rmcs_msgs; switch (*mode_) { case ChassisMode::AUTO: break; @@ -278,6 +304,17 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; + + case ChassisMode::CLIMB: { + chassis_control_angle = *chassis_climb_direction_; + + const auto err = normalize_signed_angle(chassis_control_angle - *chassis_measure_yaw_); + angular_velocity = following_velocity_controller_.update(err); + + *chassis_angle_ = *chassis_measure_yaw_; + *chassis_control_angle_ = chassis_control_angle; + return angular_velocity; + } } *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; @@ -318,7 +355,9 @@ class ChassisController OutputInterface chassis_angle_, chassis_control_angle_; InputInterface chassis_yaw_velocity_imu_; - InputInterface climbing_forward_velocity_; + InputInterface chassis_climb_direction_; + InputInterface chassis_climb_speed_; + InputInterface chassis_measure_yaw_; InputInterface navigation_enable_control_; InputInterface navigation_command_velocity_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp index c6c5192b..654a23db 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp @@ -102,22 +102,33 @@ struct CoSchduler { std::function monitor; std::chrono::steady_clock::duration timeout = std::chrono::steady_clock::duration::max(); + struct State { + std::function monitor; + std::chrono::steady_clock::time_point deadline; + bool timed_out = false; + }; + + std::shared_ptr state{}; + static constexpr auto await_ready() noexcept { return false; } template auto await_suspend(std::coroutine_handle handle) { - deadline = std::chrono::steady_clock::now() + timeout; - handle.promise().resume_request = [this] { - return monitor() || std::chrono::steady_clock::now() >= deadline; - }; - } + state = std::make_shared(State{ + .monitor = std::move(monitor), + .deadline = std::chrono::steady_clock::now() + timeout, + }); - auto await_resume() const noexcept { - return std::chrono::steady_clock::now() >= deadline && !monitor(); + handle.promise().resume_request = [state = state] { + if (state->monitor()) + return true; + + state->timed_out = std::chrono::steady_clock::now() >= state->deadline; + return state->timed_out; + }; } - // 实现细节：保持聚合属性以支持指派初始化，外部不应访问 - std::chrono::steady_clock::time_point deadline{}; + auto await_resume() const noexcept { return state->timed_out; } }; struct Slot { @@ -138,6 +149,8 @@ struct CoSchduler { // 取消句柄：弱持有任务，任务消亡后操作均为 no-op struct Handle { + Handle() = default; + auto cancel() const { if (const auto locked = slot.lock()) locked->cancelled.store(true, std::memory_order::relaxed); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp index b599b8df..03b0c977 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -36,9 +36,10 @@ struct StickGroup { double rise_torque_limit; + // kLand：begin→final 速度变化时间(s)；越小越快贴到 final；t≥T 后恒 final 缓收 double land_speed_begin; double land_speed_final; - double land_tau; + double land_duration; double land_torque_limit; double blocked_torque_threshold; @@ -56,9 +57,15 @@ struct StickGroup { case State::kDrop: return +speed_drop; case State::kRise: return -speed_rise; case State::kLand: { - const auto diff = land_speed_begin - land_speed_final; - const auto factor = std::exp(-land_elapsed / land_tau); - return -1 * (land_speed_final + diff * factor); + // 指数进度 α：t=0 → begin，t≥T → final，前快后慢减震 + constexpr auto kShape = 3.0; + const auto T = std::max(land_duration, 1e-3); + if (land_elapsed >= T) + return -land_speed_final; + + const auto u = land_elapsed / T; + const auto alpha = (1.0 - std::exp(-kShape * u)) / (1.0 - std::exp(-kShape)); + return -(land_speed_begin + (land_speed_final - land_speed_begin) * alpha); } } std::unreachable(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 8fd643e5..9291886e 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp @@ -203,6 +203,7 @@ class HeroChassisController } break; case rmcs_msgs::ChassisMode::ALIGNMENT: [[fallthrough]]; case rmcs_msgs::ChassisMode::ALIGNMENT_POWERED: [[fallthrough]]; + case rmcs_msgs::ChassisMode::CLIMB: [[fallthrough]]; case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index 9329f595..e40bbd31 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -1,3 +1,8 @@ +#include +#include +#include +#include +#include #include #include @@ -20,31 +25,128 @@ class SentryClimber static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - struct SimpleComponent : public rmcs_executor::Component { - std::function fn; + struct Config { + climber::TrackGroup::Config track; + climber::StickGroup::Config stick; - template - explicit SimpleComponent(Fn&& fn) - : fn{std::forward(fn)} {} + struct Align { + double err; + double w; + double hold; + double timeout; + } align; - auto update() -> void override { fn(); } + struct Climb { + double approach_pitch; + double leveled_pitch; + double approach_vx; + double deploy_vx; + double dash_vx; + double retract_vx; + double dash_min; + double dash_timeout; + double stick_timeout; + double approach_timeout; + } climb; + + struct Land { + double dash_vx; + double soft_vx; + double land_pitch; + double stick_timeout; + double soft_timeout; + double settle_timeout; + } land; + + double block_hold; + + template + static auto load(ParamOr&& param_or) -> Config { + return Config{ + .track = + { + .speed_rush = param_or("track_group.speed_rush", 20.0), + .kp = param_or("track_group.kp", 1.0), + .ki = param_or("track_group.ki", 0.0), + .kd = param_or("track_group.kd", 0.5), + .sync_coefficient = param_or("track_group.sync_coefficient", 0.2), + .power_estimate_bias = param_or("track_group.power_estimate_bias", 0.0), + .power_estimate_k_tau2 = param_or("track_group.power_estimate_k_tau2", 1.0), + .power_estimate_k_mech = param_or("track_group.power_estimate_k_mech", 1.0), + }, + .stick = + { + .speed_drop = param_or("stick_group.speed_drop", 30.0), + .speed_rise = param_or("stick_group.speed_rise", 60.0), + .rise_torque_limit = param_or("stick_group.rise_torque_limit", 0.5), + .land_speed_begin = param_or("stick_group.land_speed_begin", 30.0), + .land_speed_final = param_or("stick_group.land_speed_final", 2.0), + .land_duration = param_or("stick_group.land_duration", 0.5), + .land_torque_limit = param_or("stick_group.land_torque_limit", 8.0), + .blocked_torque_threshold = + param_or("stick_group.blocked_torque_threshold", 0.1), + .blocked_speed_threshold = + param_or("stick_group.blocked_speed_threshold", 0.1), + .kp = param_or("stick_group.kp", 0.5), + .ki = param_or("stick_group.ki", 0.0), + .kd = param_or("stick_group.kd", 0.0), + .sync_coefficient = param_or("stick_group.sync_coefficient", 0.2), + }, + .align = + { + .err = param_or("align.err", 0.10), + .w = param_or("align.w", 0.2), + .hold = param_or("align.hold", 0.05), + .timeout = param_or("align.timeout", 15.0), + }, + .climb = + { + .approach_pitch = param_or("climb.approach_pitch", 0.585), + .leveled_pitch = param_or("climb.leveled_pitch", 0.05), + .approach_vx = param_or("climb.approach_vx", 1.2), + .deploy_vx = param_or("climb.deploy_vx", 0.3), + .dash_vx = param_or("climb.dash_vx", 3.0), + .retract_vx = param_or("climb.retract_vx", 0.3), + .dash_min = param_or("climb.dash_min", 0.1), + .dash_timeout = param_or("climb.dash_timeout", 3.0), + .stick_timeout = param_or("climb.stick_timeout", 8.0), + .approach_timeout = param_or("climb.approach_timeout", 8.0), + }, + .land = + { + .dash_vx = param_or("land.dash_vx", 0.8), + .soft_vx = param_or("land.soft_vx", 0.4), + .land_pitch = param_or("land.land_pitch", 0.15), + .stick_timeout = param_or("land.stick_timeout", 8.0), + .soft_timeout = param_or("land.soft_timeout", 3.0), + .settle_timeout = param_or("land.settle_timeout", 8.0), + }, + .block_hold = param_or("block_hold", 0.05), + }; + } }; + // 仅输入与派生；不负责 output struct Context { - // 遥控 InputInterface l_switch; InputInterface r_switch; InputInterface keyboard; InputInterface rotary_knob; - // 姿态 InputInterface chassis_pitch; - InputInterface gimbal_yaw_angle; - InputInterface gimbal_yaw_error; - InputInterface gimbal_yaw_speed; + InputInterface chassis_yaw_rate; + // 台阶方向（Odom XY）；NaN 表示由触发时 measure 推导 + InputInterface target_yaw; + InputInterface measure_yaw; - // 输出（下游契约，话题名不可改） - OutputInterface climb_speed; + static auto normalize_angle(double angle) noexcept { + constexpr auto kTwoPi = 2.0 * std::numbers::pi; + while (angle >= std::numbers::pi) + angle -= kTwoPi; + while (angle < -std::numbers::pi) + angle += kTwoPi; + return angle; + } auto bind(Component& component) noexcept { component.register_input("/remote/switch/left", l_switch, false); @@ -53,11 +155,9 @@ class SentryClimber component.register_input("/remote/rotary_knob_switch", rotary_knob, false); component.register_input("/chassis/pitch_imu", chassis_pitch, false); - component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle, false); - component.register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_error, false); - component.register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_speed, false); - - component.register_output("/chassis/climber/speed", climb_speed, kNaN); + component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_rate, false); + component.register_input("/chassis/climber/target_yaw", target_yaw, false); + component.register_input("/chassis/climber/measure_yaw", measure_yaw, false); } auto load_fallback(std::invocable auto&& handler) { @@ -77,69 +177,381 @@ class SentryClimber ensure_bind(rotary_knob, Switch::UNKNOWN, "rotary_knob"); ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); - ensure_bind(gimbal_yaw_angle, 0.0, "gimbal_yaw_angle"); - ensure_bind(gimbal_yaw_error, 0.0, "gimbal_yaw_error"); - ensure_bind(gimbal_yaw_speed, 0.0, "gimbal_yaw_speed"); + ensure_bind(chassis_yaw_rate, 0.0, "chassis_yaw_rate"); + ensure_bind(target_yaw, kNaN, "target_yaw"); + ensure_bind(measure_yaw, kNaN, "measure_yaw"); } + + auto is_estop() const { + using namespace rmcs_msgs; + const auto l = *l_switch; + const auto r = *r_switch; + return l == Switch::UNKNOWN || r == Switch::UNKNOWN + || (l == Switch::DOWN && r == Switch::DOWN); + } + + auto align_error(double goal) const noexcept { + return normalize_angle(*measure_yaw - goal); + } + + auto wait_align( + double goal, double err_limit, double w_limit, std::chrono::steady_clock::duration hold, + std::chrono::steady_clock::duration timeout) const { + constexpr auto kSinceInit = std::optional{}; + return CoSchduler::WaitUntil{ + .monitor = + [this, goal, err_limit, w_limit, hold, hold_since = kSinceInit]() mutable { + if (!std::isfinite(*measure_yaw)) + return false; + + const auto stable = std::abs(align_error(goal)) < err_limit + && std::abs(*chassis_yaw_rate) < w_limit; + + const auto now = std::chrono::steady_clock::now(); + if (stable) { + if (!hold_since.has_value()) + hold_since = now; + else if (now - *hold_since >= hold) + return true; + } else { + hold_since.reset(); + } + return false; + }, + .timeout = timeout, + }; + } + } context; - std::shared_ptr status_component{ + // direction：底盘正向方向；speed：以其为正向的有符号速度（上正下负） + OutputInterface chassis_climb_direction; + OutputInterface chassis_climb_speed; + OutputInterface climb_status; + + struct SimpleComponent : public rmcs_executor::Component { + std::function fn; + + template + explicit SimpleComponent(Fn&& fn) + : fn{std::forward(fn)} {} + + auto update() -> void override { fn(); } + }; + + // 伙伴组件注册并发布底盘契约输出（依赖序在主组件之后） + std::shared_ptr output_component{ create_partner_component( - get_component_name() + "_status", [this] { update_status(); }), + get_component_name() + "_output", + [this] { + // 输出由业务协程写入，此处仅占位以形成 partner 更新节点 + std::ignore = this; + }), }; - OutputInterface climb_status; // T climbing, F done or idle std::unique_ptr track_group; std::unique_ptr stick_group; CoSchduler schduler; - auto update_status() -> void {} + Config config; + CoSchduler::Handle task_handler; -public: - SentryClimber() - : Node{get_component_name(), node::options()} { - using namespace climber; + static auto seconds_to_duration(double seconds) noexcept { + return std::chrono::duration_cast( + std::chrono::duration{seconds}); + } + + auto release_chassis() { + *chassis_climb_direction = kNaN; + *chassis_climb_speed = kNaN; + *climb_status = 0.0; + } + + auto wait_block(std::chrono::steady_clock::duration timeout) { + constexpr auto kSinceInit = std::optional{}; + const auto hold = seconds_to_duration(config.block_hold); + return CoSchduler::WaitUntil{ + .monitor = + [this, hold, hold_since = kSinceInit]() mutable { + const auto now = std::chrono::steady_clock::now(); + if (stick_group->get_block()) { + if (!hold_since.has_value()) + hold_since = now; + else if (now - *hold_since >= hold) + return true; + } else { + hold_since.reset(); + } + return false; + }, + .timeout = timeout, + }; + } + + auto spin_context() -> CoSchduler::Task { + using namespace rmcs_msgs; + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + + auto last_keyboard = Keyboard::zero(); + auto last_rotary = Switch::UNKNOWN; + + const auto cancel_task = [this] { + if (!task_handler.done()) { + task_handler.cancel(); + task_handler = {}; + } + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kFree); + release_chassis(); + }; + + while (true) { + const auto keyboard = *context.keyboard; + const auto rotary = *context.rotary_knob; + + const auto land_intent = last_rotary != Switch::DOWN && rotary == Switch::DOWN; + const auto rise_intent = (last_keyboard.g == false && keyboard.g == true) + || (last_rotary != Switch::UP && rotary == Switch::UP); + const auto stop_intent = (last_rotary == Switch::UP && rotary != Switch::UP) + || (last_rotary == Switch::DOWN && rotary != Switch::DOWN); + + if (context.is_estop() || stop_intent) { + cancel_task(); + } else if (rise_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + node::error("climb start rejected: measure_yaw invalid"); + } else { + // 底盘正向：有外部台阶方向直接用，否则锁存当前航向（默认前脸对台阶） + const auto direction = std::isfinite(*context.target_yaw) + ? *context.target_yaw + : *context.measure_yaw; + task_handler = schduler.append(climb(direction)); + } + } else if (land_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + node::error("land start rejected: measure_yaw invalid"); + } else { + // 底盘正向：外部给台阶方向时背对台阶（+π），否则保持当前航向（默认背对台阶） + const auto direction = + std::isfinite(*context.target_yaw) + ? Context::normalize_angle(*context.target_yaw + std::numbers::pi) + : *context.measure_yaw; + task_handler = schduler.append(land(direction)); + } + } + + *climb_status = task_handler.done() ? 0.0 : 1.0; + last_keyboard = keyboard; + last_rotary = rotary; + + co_await CoSchduler::Tick{}; + } + } + + auto spin_groups() -> CoSchduler::Task { + while (true) { + track_group->spin_once(); + stick_group->spin_once(); + co_await CoSchduler::Tick{}; + } + } + + // direction：底盘正向方向；speed 以其为正向 + auto climb(double direction) -> CoSchduler::Task { + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + + node::info("Climb start, direction={:.3f}", direction); + *chassis_climb_direction = direction; + + // ALIGN：对齐底盘正向，speed=0 仍保持 CLIMB + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = 0.0; { - const auto config = TrackGroup::Config{ - .speed_rush = node::param_or("track_group.speed_rush", 20.0), + const auto timed_out = co_await context.wait_align( + direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), + seconds_to_duration(config.align.timeout)); + if (timed_out || !std::isfinite(*context.measure_yaw)) { + node::warn("climb ALIGN failed"); + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kFree); + release_chassis(); + co_return; + } + } + + // APPROACH + track_group->set_state(TrackState::kRush); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = config.climb.approach_vx; + { + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = [this] { return *context.chassis_pitch > config.climb.approach_pitch; }, + .timeout = seconds_to_duration(config.climb.approach_timeout), + }; + if (timed_out) + node::warn("climb APPROACH timeout, continue"); + } - .kp = node::param_or("track_group.kp", 1.0), - .ki = node::param_or("track_group.ki", 0.), - .kd = node::param_or("track_group.kd", 0.5), - .sync_coefficient = node::param_or("track_group.sync_coefficient", 0.2), + // DEPLOY + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = config.climb.deploy_vx; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); + if (timed_out) + node::warn("climb DEPLOY stick timeout, continue"); + } - .power_estimate_bias = node::param_or("track_group.power_estimate_bias", 0.0), - .power_estimate_k_tau2 = node::param_or("track_group.power_estimate_k_tau2", 1.0), - .power_estimate_k_mech = node::param_or("track_group.power_estimate_k_mech", 1.0), + // DASH + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = config.climb.dash_vx; + { + const auto dash_start = std::chrono::steady_clock::now(); + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = + [this, dash_start] { + return std::chrono::steady_clock::now() - dash_start + >= seconds_to_duration(config.climb.dash_min) + && *context.chassis_pitch < config.climb.leveled_pitch; + }, + .timeout = seconds_to_duration(config.climb.dash_timeout), }; - track_group = std::make_unique(*this, config); + if (timed_out) + node::warn("climb DASH timeout, continue"); + } + + // RETRACT + track_group->set_state(TrackState::kRush); + stick_group->set_state(StickState::kRise); + *chassis_climb_speed = config.climb.retract_vx; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.climb.stick_timeout)); + if (timed_out) + node::warn("climb RETRACT stick timeout, continue"); + } + + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kHold); + release_chassis(); + } + + auto land(double direction) -> CoSchduler::Task { + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + + node::info("Land start, direction={:.3f}", direction); + *chassis_climb_direction = direction; + + // PREPARE：先收支臂，暂不接管底盘速度 + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kRise); + *chassis_climb_speed = kNaN; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.stick_timeout)); + if (timed_out) + node::warn("land PREPARE stick timeout, continue"); + } + + // ALIGN：对齐底盘正向，speed=0 + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = 0.0; + { + const auto timed_out = co_await context.wait_align( + direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), + seconds_to_duration(config.align.timeout)); + if (timed_out || !std::isfinite(*context.measure_yaw)) { + node::warn("land ALIGN failed"); + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kFree); + release_chassis(); + co_return; + } } + + // DEPLOY / SETTLE / SOFT：相对 direction 为负向 + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = -config.land.dash_vx; { - const auto config = StickGroup::Config{ - .speed_drop = node::param_or("stick_group.speed_drop", 30.0), - .speed_rise = node::param_or("stick_group.speed_rise", 60.0), - - .rise_torque_limit = node::param_or("stick_group.rise_torque_limit", 0.5), - - .land_speed_begin = node::param_or("stick_group.land_speed_begin", 30.0), - .land_speed_final = node::param_or("stick_group.land_speed_final", 2.0), - .land_tau = node::param_or("stick_group.land_tau", 0.4), - .land_torque_limit = node::param_or("stick_group.land_torque_limit", 8.0), - - .blocked_torque_threshold = - node::param_or("stick_group.blocked_torque_threshold", 0.1), - .blocked_speed_threshold = - node::param_or("stick_group.blocked_speed_threshold", 0.1), - - .kp = node::param_or("stick_group.kp", 0.5), - .ki = node::param_or("stick_group.ki", 0.), - .kd = node::param_or("stick_group.kd", 0.), - .sync_coefficient = node::param_or("stick_group.sync_coefficient", 0.2), + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.stick_timeout)); + if (timed_out) + node::warn("land DEPLOY stick timeout, continue"); + } + + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kDrop); + *chassis_climb_speed = -config.land.dash_vx; + { + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = + [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, + .timeout = seconds_to_duration(config.land.settle_timeout), }; - stick_group = std::make_unique(*this, config); + if (timed_out) + node::warn("land SETTLE timeout, continue"); } + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kLand); + *chassis_climb_speed = -config.land.soft_vx; + co_await CoSchduler::Sleep{seconds_to_duration(config.stick.land_duration)}; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.soft_timeout)); + if (timed_out) + node::warn("land SOFT stick timeout, continue"); + } + + // FINAL + track_group->set_state(TrackState::kFree); + stick_group->set_state(StickState::kRise); + *chassis_climb_speed = kNaN; + { + const auto timed_out = + co_await wait_block(seconds_to_duration(config.land.stick_timeout)); + if (timed_out) + node::warn("land FINAL stick timeout, continue"); + } + + stick_group->set_state(StickState::kHold); + release_chassis(); + } + +public: + SentryClimber() + : Node{get_component_name(), node::options()} { + const auto read_parameter = [this](std::string_view name, double fallback) { + return node::param_or(std::string{name}, fallback); + }; + + config = Config::load(read_parameter); + + track_group = std::make_unique(*this, config.track); + stick_group = std::make_unique(*this, config.stick); + context.bind(*this); + + // 底盘契约输出挂在 partner 上，保证更新序在主逻辑之后对下游可见 + output_component->register_output( + "/chassis/climber/direction", chassis_climb_direction, kNaN); + output_component->register_output("/chassis/climber/speed", chassis_climb_speed, kNaN); + output_component->register_output("/chassis/climber/status", climb_status, 0.0); + + schduler.append(spin_context()); + schduler.append(spin_groups()); } auto before_updating() -> void override { @@ -147,6 +559,22 @@ class SentryClimber node::warn("Failed to fetch input '{}'. Bind to fallback.", name); }); } + + auto update() -> void override { + try { + schduler.spin_once(); + } catch (const std::exception& e) { + node::error("climber routine exception: {}", e.what()); + task_handler.cancel(); + task_handler = {}; + track_group->set_state(climber::TrackGroup::State::kFree); + stick_group->set_state(climber::StickGroup::State::kFree); + release_chassis(); + } + } }; } // namespace rmcs_core::controller::chassis + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::SentryClimber, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 4233d7c7..1b72ab00 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -309,6 +309,7 @@ class Sentry sentry.register_output("/referee/serial", referee_serial_); sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); + sentry.register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, 0.0); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( @@ -390,6 +391,9 @@ class Sentry const auto& q = snapshot->orientation; *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *chassis_measure_yaw_ = std::atan2( + 2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); } } @@ -563,6 +567,7 @@ class Sentry OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; + OutputInterface chassis_measure_yaw_; StatusMonitor monitor_{}; std::unique_ptr board_; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index f61826ca..a279b92c 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -12,10 +12,12 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, + CLIMB, }; constexpr auto is_powered(ChassisMode mode) noexcept { - return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP; + return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP + || mode == ChassisMode::CLIMB; } constexpr auto is_spining(ChassisMode mode) noexcept { return mode == ChassisMode::SPIN_SLOW || mode == ChassisMode::SPIN_FAST; From 665c57eec74261d475f9618fd61fae3bb96f50fb Mon Sep 17 00:00:00 2001 From: zlq040222 <1542498005@qq.com> Date: Sat, 25 Jul 2026 16:25:30 +0800 Subject: [PATCH 11/15] feat(climber): add kRetracted state for stick auto-retract with stall detection MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - StickGroup: add kRetracted state with speed_rise + rise_torque_limit + hold_torque - kRetracted performs PID ascent, switches to constant hold_torque (0.25Nm) on stall - release_chassis() now sets track→kFree + stick→kRetracted for safe idle state - Add hold_torque param (default 0.25) to YAML config --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 12 +++--- .../chassis/climber/stick_group.hpp | 12 +++++- .../src/controller/chassis/sentry_climber.cpp | 38 +++++++++++-------- 3 files changed, 40 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index baa9d408..de6f840c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -143,17 +143,18 @@ sentry_climber: stick_group: speed_drop: 30.0 speed_rise: 60.0 - rise_torque_limit: 0.5 - land_speed_begin: 80.0 + rise_torque_limit: 1.2 + land_speed_begin: 100.0 land_speed_final: 10.0 land_duration: 0.5 land_torque_limit: 8.0 blocked_torque_threshold: 0.1 blocked_speed_threshold: 0.1 - kp: 0.5 + kp: 1.0 ki: 0.0 kd: 0.0 sync_coefficient: 0.2 + hold_torque: 0.01 block_hold: 0.05 align: err: 0.20 @@ -168,13 +169,14 @@ sentry_climber: dash_vx: 3.0 retract_vx: 0.3 dash_min: 0.1 - dash_timeout: 3.0 + dash_duration: 0.8 stick_timeout: 8.0 approach_timeout: 8.0 land: - dash_vx: 0.8 + dash_vx: 1.0 soft_vx: 0.4 land_pitch: 0.15 + land_delay: 0.2 stick_timeout: 8.0 soft_timeout: 3.0 settle_timeout: 8.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp index 03b0c977..8bd1f60a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -28,6 +28,7 @@ struct StickGroup { kDrop, kRise, kLand, + kRetacted, } state = State::kFree; struct Config { @@ -49,6 +50,7 @@ struct StickGroup { double ki; double kd; double sync_coefficient; + double hold_torque; auto get_speed(State state, double land_elapsed = 0.0) const noexcept { switch (state) { @@ -56,6 +58,7 @@ struct StickGroup { case State::kHold: return 0.0; case State::kDrop: return +speed_drop; case State::kRise: return -speed_rise; + case State::kRetacted: return -speed_rise; case State::kLand: { // 指数进度 α：t=0 → begin，t≥T → final，前快后慢减震 constexpr auto kShape = 3.0; @@ -74,6 +77,7 @@ struct StickGroup { auto get_torque_limit(State state) const noexcept { switch (state) { case State::kRise: return rise_torque_limit; + case State::kRetacted: return rise_torque_limit; case State::kLand: return land_torque_limit; default: return std::numeric_limits::infinity(); } @@ -113,6 +117,12 @@ struct StickGroup { } auto spin_once() { + if (state == State::kRetacted && get_block()) { + *l_control_torque = -config.hold_torque; + *r_control_torque = -config.hold_torque; + return; + } + const auto land_elapsed = std::chrono::duration(std::chrono::steady_clock::now() - land_start_timestamp) .count(); @@ -168,7 +178,7 @@ struct StickGroup { auto get_state() const noexcept { return state; } - auto get_block() const noexcept { + auto get_block() const noexcept -> bool { const auto is_blocked = [](double torque, double velocity, double torque_threshold, double velocity_threshold) { return std::abs(torque) > torque_threshold && std::abs(velocity) < velocity_threshold; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index e40bbd31..0594361d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -44,7 +44,7 @@ class SentryClimber double dash_vx; double retract_vx; double dash_min; - double dash_timeout; + double dash_duration; double stick_timeout; double approach_timeout; } climb; @@ -53,6 +53,7 @@ class SentryClimber double dash_vx; double soft_vx; double land_pitch; + double land_delay; double stick_timeout; double soft_timeout; double settle_timeout; @@ -91,6 +92,7 @@ class SentryClimber .ki = param_or("stick_group.ki", 0.0), .kd = param_or("stick_group.kd", 0.0), .sync_coefficient = param_or("stick_group.sync_coefficient", 0.2), + .hold_torque = param_or("stick_group.hold_torque", 0.01), }, .align = { @@ -108,7 +110,7 @@ class SentryClimber .dash_vx = param_or("climb.dash_vx", 3.0), .retract_vx = param_or("climb.retract_vx", 0.3), .dash_min = param_or("climb.dash_min", 0.1), - .dash_timeout = param_or("climb.dash_timeout", 3.0), + .dash_duration = param_or("climb.dash_duration", 3.0), .stick_timeout = param_or("climb.stick_timeout", 8.0), .approach_timeout = param_or("climb.approach_timeout", 8.0), }, @@ -117,6 +119,7 @@ class SentryClimber .dash_vx = param_or("land.dash_vx", 0.8), .soft_vx = param_or("land.soft_vx", 0.4), .land_pitch = param_or("land.land_pitch", 0.15), + .land_delay = param_or("land.land_delay", 0.2), .stick_timeout = param_or("land.stick_timeout", 8.0), .soft_timeout = param_or("land.soft_timeout", 3.0), .settle_timeout = param_or("land.settle_timeout", 8.0), @@ -265,6 +268,8 @@ class SentryClimber *chassis_climb_direction = kNaN; *chassis_climb_speed = kNaN; *climb_status = 0.0; + track_group->set_state(climber::TrackGroup::State::kFree); + stick_group->set_state(climber::StickGroup::State::kRetacted); } auto wait_block(std::chrono::steady_clock::duration timeout) { @@ -366,6 +371,8 @@ class SentryClimber using TrackState = climber::TrackGroup::State; using StickState = climber::StickGroup::State; + using namespace std::chrono_literals; + node::info("Climb start, direction={:.3f}", direction); *chassis_climb_direction = direction; @@ -414,20 +421,7 @@ class SentryClimber track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kHold); *chassis_climb_speed = config.climb.dash_vx; - { - const auto dash_start = std::chrono::steady_clock::now(); - const auto timed_out = co_await CoSchduler::WaitUntil{ - .monitor = - [this, dash_start] { - return std::chrono::steady_clock::now() - dash_start - >= seconds_to_duration(config.climb.dash_min) - && *context.chassis_pitch < config.climb.leveled_pitch; - }, - .timeout = seconds_to_duration(config.climb.dash_timeout), - }; - if (timed_out) - node::warn("climb DASH timeout, continue"); - } + co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)}; // RETRACT track_group->set_state(TrackState::kRush); @@ -442,6 +436,7 @@ class SentryClimber track_group->set_state(TrackState::kFree); stick_group->set_state(StickState::kHold); + release_chassis(); } @@ -503,6 +498,8 @@ class SentryClimber if (timed_out) node::warn("land SETTLE timeout, continue"); } + using namespace std::chrono_literals; + co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)}; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kLand); @@ -514,6 +511,15 @@ class SentryClimber if (timed_out) node::warn("land SOFT stick timeout, continue"); } + { + const auto timed_out = co_await CoSchduler::WaitUntil{ + .monitor = + [this] { return std::abs(*context.chassis_pitch) < config.land.land_pitch; }, + .timeout = seconds_to_duration(config.land.settle_timeout), + }; + if (timed_out) + node::warn("land SETTLE timeout, continue"); + } // FINAL track_group->set_state(TrackState::kFree); From 45df6281b3ff9ded261e1bc1e02ca61e7378d1cd Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Jul 2026 04:04:32 +0800 Subject: [PATCH 12/15] wip: Cleanup code --- .../chassis/climber/stick_group.hpp | 12 +- .../src/controller/chassis/sentry_climber.cpp | 169 ++++++++---------- 2 files changed, 83 insertions(+), 98 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp index 8bd1f60a..294cf362 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -28,7 +28,7 @@ struct StickGroup { kDrop, kRise, kLand, - kRetacted, + kKeep, } state = State::kFree; struct Config { @@ -57,8 +57,8 @@ struct StickGroup { case State::kFree: return kNaN; case State::kHold: return 0.0; case State::kDrop: return +speed_drop; - case State::kRise: return -speed_rise; - case State::kRetacted: return -speed_rise; + case State::kRise: [[fallthrough]]; + case State::kKeep: return -speed_rise; case State::kLand: { // 指数进度 α：t=0 → begin，t≥T → final，前快后慢减震 constexpr auto kShape = 3.0; @@ -76,8 +76,8 @@ struct StickGroup { auto get_torque_limit(State state) const noexcept { switch (state) { - case State::kRise: return rise_torque_limit; - case State::kRetacted: return rise_torque_limit; + case State::kRise: [[fallthrough]]; + case State::kKeep: return rise_torque_limit; case State::kLand: return land_torque_limit; default: return std::numeric_limits::infinity(); } @@ -117,7 +117,7 @@ struct StickGroup { } auto spin_once() { - if (state == State::kRetacted && get_block()) { + if (state == State::kKeep && get_block()) { *l_control_torque = -config.hold_torque; *r_control_torque = -config.hold_torque; return; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index 0594361d..35614731 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -25,6 +25,9 @@ class SentryClimber static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + struct Config { climber::TrackGroup::Config track; climber::StickGroup::Config stick; @@ -142,7 +145,7 @@ class SentryClimber InputInterface target_yaw; InputInterface measure_yaw; - static auto normalize_angle(double angle) noexcept { + static constexpr auto normalize_angle(double angle) noexcept { constexpr auto kTwoPi = 2.0 * std::numbers::pi; while (angle >= std::numbers::pi) angle -= kTwoPi; @@ -227,10 +230,9 @@ class SentryClimber } context; - // direction：底盘正向方向；speed：以其为正向的有符号速度（上正下负） - OutputInterface chassis_climb_direction; - OutputInterface chassis_climb_speed; - OutputInterface climb_status; + OutputInterface chassis_track_direction; // 以履带方向为正向 + OutputInterface chassis_climb_speed; // 正向为基准的速度值 + OutputInterface chassis_climb_status; struct SimpleComponent : public rmcs_executor::Component { std::function fn; @@ -259,17 +261,17 @@ class SentryClimber Config config; CoSchduler::Handle task_handler; - static auto seconds_to_duration(double seconds) noexcept { + static constexpr auto seconds_to_duration(double seconds) noexcept { return std::chrono::duration_cast( std::chrono::duration{seconds}); } - auto release_chassis() { - *chassis_climb_direction = kNaN; + auto release_climber() noexcept { + *chassis_track_direction = kNaN; *chassis_climb_speed = kNaN; - *climb_status = 0.0; - track_group->set_state(climber::TrackGroup::State::kFree); - stick_group->set_state(climber::StickGroup::State::kRetacted); + track_group->set_state(TrackState::kFree); + stick_group->set_state(// + context.is_estop() ? StickState::kFree : StickState::kKeep); } auto wait_block(std::chrono::steady_clock::duration timeout) { @@ -295,8 +297,6 @@ class SentryClimber auto spin_context() -> CoSchduler::Task { using namespace rmcs_msgs; - using TrackState = climber::TrackGroup::State; - using StickState = climber::StickGroup::State; auto last_keyboard = Keyboard::zero(); auto last_rotary = Switch::UNKNOWN; @@ -306,51 +306,52 @@ class SentryClimber task_handler.cancel(); task_handler = {}; } - track_group->set_state(TrackState::kFree); - stick_group->set_state(StickState::kFree); - release_chassis(); + *chassis_climb_status = 0.0; + release_climber(); }; while (true) { const auto keyboard = *context.keyboard; const auto rotary = *context.rotary_knob; - const auto land_intent = last_rotary != Switch::DOWN && rotary == Switch::DOWN; - const auto rise_intent = (last_keyboard.g == false && keyboard.g == true) - || (last_rotary != Switch::UP && rotary == Switch::UP); - const auto stop_intent = (last_rotary == Switch::UP && rotary != Switch::UP) - || (last_rotary == Switch::DOWN && rotary != Switch::DOWN); + const auto stop_intent = (last_rotary != Switch::MIDDLE && rotary == Switch::MIDDLE); + const auto land_intent = (last_rotary != Switch::DOWN && rotary == Switch::DOWN); + const auto rise_intent = (last_rotary != Switch::UP && rotary == Switch::UP) + || (last_keyboard.g == false && keyboard.g == true); - if (context.is_estop() || stop_intent) { - cancel_task(); - } else if (rise_intent) { - if (!task_handler.done()) { + const auto step_direction = + std::isfinite(*context.target_yaw) ? *context.target_yaw : *context.measure_yaw; + + do { + if (context.is_estop() || stop_intent) { cancel_task(); - } else if (!std::isfinite(*context.measure_yaw)) { - node::error("climb start rejected: measure_yaw invalid"); - } else { - // 底盘正向：有外部台阶方向直接用，否则锁存当前航向（默认前脸对台阶） - const auto direction = std::isfinite(*context.target_yaw) - ? *context.target_yaw - : *context.measure_yaw; - task_handler = schduler.append(climb(direction)); + break; } - } else if (land_intent) { - if (!task_handler.done()) { - cancel_task(); - } else if (!std::isfinite(*context.measure_yaw)) { - node::error("land start rejected: measure_yaw invalid"); - } else { - // 底盘正向：外部给台阶方向时背对台阶（+π），否则保持当前航向（默认背对台阶） - const auto direction = - std::isfinite(*context.target_yaw) - ? Context::normalize_angle(*context.target_yaw + std::numbers::pi) - : *context.measure_yaw; - task_handler = schduler.append(land(direction)); + if (rise_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + node::error("climb start rejected: measure_yaw invalid"); + } else { + task_handler = schduler.append(climb(step_direction)); + } + break; } - } + if (land_intent) { + if (!task_handler.done()) { + cancel_task(); + } else if (!std::isfinite(*context.measure_yaw)) { + node::error("land start rejected: measure_yaw invalid"); + } else { + task_handler = schduler.append(land(step_direction)); + } + break; + } + } while (false); + + if (!context.is_estop() && task_handler.done()) + release_climber(); - *climb_status = task_handler.done() ? 0.0 : 1.0; last_keyboard = keyboard; last_rotary = rotary; @@ -366,17 +367,13 @@ class SentryClimber } } - // direction：底盘正向方向；speed 以其为正向 auto climb(double direction) -> CoSchduler::Task { - using TrackState = climber::TrackGroup::State; - using StickState = climber::StickGroup::State; - - using namespace std::chrono_literals; + *chassis_climb_status = 0.0; node::info("Climb start, direction={:.3f}", direction); - *chassis_climb_direction = direction; + *chassis_track_direction = direction; - // ALIGN：对齐底盘正向，speed=0 仍保持 CLIMB + // [] 将底盘与台阶方向对齐 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kHold); *chassis_climb_speed = 0.0; @@ -386,14 +383,13 @@ class SentryClimber seconds_to_duration(config.align.timeout)); if (timed_out || !std::isfinite(*context.measure_yaw)) { node::warn("climb ALIGN failed"); - track_group->set_state(TrackState::kFree); - stick_group->set_state(StickState::kFree); - release_chassis(); + release_climber(); + *chassis_climb_status = -1; co_return; } } - // APPROACH + // [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度 track_group->set_state(TrackState::kRush); stick_group->set_state(StickState::kHold); *chassis_climb_speed = config.climb.approach_vx; @@ -406,7 +402,7 @@ class SentryClimber node::warn("climb APPROACH timeout, continue"); } - // DEPLOY + // [] 伸出撑杆，同时慢速向台阶方向前进 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = config.climb.deploy_vx; @@ -417,14 +413,14 @@ class SentryClimber node::warn("climb DEPLOY stick timeout, continue"); } - // DASH + // [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kHold); *chassis_climb_speed = config.climb.dash_vx; co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)}; - // RETRACT - track_group->set_state(TrackState::kRush); + // [] 上台阶完毕，收回撑杆 + track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kRise); *chassis_climb_speed = config.climb.retract_vx; { @@ -434,31 +430,17 @@ class SentryClimber node::warn("climb RETRACT stick timeout, continue"); } - track_group->set_state(TrackState::kFree); - stick_group->set_state(StickState::kHold); - - release_chassis(); + *chassis_climb_status = 1.0; + release_climber(); } auto land(double direction) -> CoSchduler::Task { - using TrackState = climber::TrackGroup::State; - using StickState = climber::StickGroup::State; + *chassis_climb_status = 0.0; node::info("Land start, direction={:.3f}", direction); - *chassis_climb_direction = direction; + *chassis_track_direction = direction + std::numbers::pi; - // PREPARE：先收支臂，暂不接管底盘速度 - track_group->set_state(TrackState::kHold); - stick_group->set_state(StickState::kRise); - *chassis_climb_speed = kNaN; - { - const auto timed_out = - co_await wait_block(seconds_to_duration(config.land.stick_timeout)); - if (timed_out) - node::warn("land PREPARE stick timeout, continue"); - } - - // ALIGN：对齐底盘正向，speed=0 + // [] 底盘对齐方向，准备下台阶 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kHold); *chassis_climb_speed = 0.0; @@ -466,16 +448,15 @@ class SentryClimber const auto timed_out = co_await context.wait_align( direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout)); - if (timed_out || !std::isfinite(*context.measure_yaw)) { + if (timed_out) { node::warn("land ALIGN failed"); - track_group->set_state(TrackState::kFree); - stick_group->set_state(StickState::kFree); - release_chassis(); + release_climber(); + *chassis_climb_status = -1; co_return; } } - // DEPLOY / SETTLE / SOFT：相对 direction 为负向 + // [] 伸出撑杆，以较快速度冲下台阶 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = -config.land.dash_vx; @@ -486,6 +467,7 @@ class SentryClimber node::warn("land DEPLOY stick timeout, continue"); } + // [] 保持撑杆伸出，直到撑杆从台阶落下，底盘倾角低于某个阈值，趋近水平 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = -config.land.dash_vx; @@ -501,6 +483,7 @@ class SentryClimber using namespace std::chrono_literals; co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)}; + // [] 撑杆按照速度曲线收回，减少落地震动，并缓慢前进，让履带顺着台阶落下 track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kLand); *chassis_climb_speed = -config.land.soft_vx; @@ -511,6 +494,8 @@ class SentryClimber if (timed_out) node::warn("land SOFT stick timeout, continue"); } + + // [] 等待完全落地，底盘倾角趋近水平 { const auto timed_out = co_await CoSchduler::WaitUntil{ .monitor = @@ -521,7 +506,7 @@ class SentryClimber node::warn("land SETTLE timeout, continue"); } - // FINAL + // [] 完全收回撑杆，结束下台阶 track_group->set_state(TrackState::kFree); stick_group->set_state(StickState::kRise); *chassis_climb_speed = kNaN; @@ -532,8 +517,8 @@ class SentryClimber node::warn("land FINAL stick timeout, continue"); } - stick_group->set_state(StickState::kHold); - release_chassis(); + *chassis_climb_status = 1.0; + release_climber(); } public: @@ -552,9 +537,9 @@ class SentryClimber // 底盘契约输出挂在 partner 上，保证更新序在主逻辑之后对下游可见 output_component->register_output( - "/chassis/climber/direction", chassis_climb_direction, kNaN); + "/chassis/climber/direction", chassis_track_direction, kNaN); output_component->register_output("/chassis/climber/speed", chassis_climb_speed, kNaN); - output_component->register_output("/chassis/climber/status", climb_status, 0.0); + output_component->register_output("/chassis/climber/status", chassis_climb_status, 0.0); schduler.append(spin_context()); schduler.append(spin_groups()); @@ -575,7 +560,7 @@ class SentryClimber task_handler = {}; track_group->set_state(climber::TrackGroup::State::kFree); stick_group->set_state(climber::StickGroup::State::kFree); - release_chassis(); + release_climber(); } } }; From e1cf3b93cb9713dcdd490a666c98f9ded04bad87 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Jul 2026 09:27:39 +0800 Subject: [PATCH 13/15] feat: Adapt climb request from navigation --- .../src/controller/chassis/sentry_climber.cpp | 62 ++++++++++++------- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 16 ++--- 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index 35614731..772f9cdb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -139,22 +139,28 @@ class SentryClimber InputInterface keyboard; InputInterface rotary_knob; + InputInterface nav_cross_direction; + InputInterface nav_is_climb; + InputInterface chassis_pitch; InputInterface chassis_yaw_rate; - // 台阶方向（Odom XY）；NaN 表示由触发时 measure 推导 - InputInterface target_yaw; InputInterface measure_yaw; static constexpr auto normalize_angle(double angle) noexcept { - constexpr auto kTwoPi = 2.0 * std::numbers::pi; while (angle >= std::numbers::pi) - angle -= kTwoPi; + angle -= 2.0 * std::numbers::pi; while (angle < -std::numbers::pi) - angle += kTwoPi; + angle += 2.0 * std::numbers::pi; return angle; } auto bind(Component& component) noexcept { + // 当 nav_cross_direction 发生 isnan -> !isnan 的变化时，视作一次跨越地形事件请求 + // 反之，会立刻取消请求，终止当前事件 + component.register_input( + "/rmcs_navigation/request/cross_direction", nav_cross_direction, false); + component.register_input("/rmcs_navigation/request/is_climb", nav_is_climb, false); + component.register_input("/remote/switch/left", l_switch, false); component.register_input("/remote/switch/right", r_switch, false); component.register_input("/remote/keyboard", keyboard, false); @@ -162,7 +168,6 @@ class SentryClimber component.register_input("/chassis/pitch_imu", chassis_pitch, false); component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_rate, false); - component.register_input("/chassis/climber/target_yaw", target_yaw, false); component.register_input("/chassis/climber/measure_yaw", measure_yaw, false); } @@ -177,6 +182,9 @@ class SentryClimber } }; + ensure_bind(nav_cross_direction, kNaN, "nav_cross_direction"); + ensure_bind(nav_is_climb, false, "nav_is_climb"); + ensure_bind(l_switch, Switch::UNKNOWN, "l_switch"); ensure_bind(r_switch, Switch::UNKNOWN, "r_switch"); ensure_bind(keyboard, Keyboard::zero(), "keyboard"); @@ -184,7 +192,6 @@ class SentryClimber ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); ensure_bind(chassis_yaw_rate, 0.0, "chassis_yaw_rate"); - ensure_bind(target_yaw, kNaN, "target_yaw"); ensure_bind(measure_yaw, kNaN, "measure_yaw"); } @@ -206,7 +213,7 @@ class SentryClimber constexpr auto kSinceInit = std::optional{}; return CoSchduler::WaitUntil{ .monitor = - [this, goal, err_limit, w_limit, hold, hold_since = kSinceInit]() mutable { + [=, this, hold_since = kSinceInit]() mutable { if (!std::isfinite(*measure_yaw)) return false; @@ -232,7 +239,7 @@ class SentryClimber OutputInterface chassis_track_direction; // 以履带方向为正向 OutputInterface chassis_climb_speed; // 正向为基准的速度值 - OutputInterface chassis_climb_status; + OutputInterface chassis_climb_status; // 事件进度 struct SimpleComponent : public rmcs_executor::Component { std::function fn; @@ -244,14 +251,9 @@ class SentryClimber auto update() -> void override { fn(); } }; - // 伙伴组件注册并发布底盘契约输出（依赖序在主组件之后） std::shared_ptr output_component{ create_partner_component( - get_component_name() + "_output", - [this] { - // 输出由业务协程写入，此处仅占位以形成 partner 更新节点 - std::ignore = this; - }), + get_component_name() + "_output", [this] { std::ignore = this; }), }; std::unique_ptr track_group; @@ -301,6 +303,8 @@ class SentryClimber auto last_keyboard = Keyboard::zero(); auto last_rotary = Switch::UNKNOWN; + auto last_nav_cross_dir = kNaN; + const auto cancel_task = [this] { if (!task_handler.done()) { task_handler.cancel(); @@ -314,13 +318,25 @@ class SentryClimber const auto keyboard = *context.keyboard; const auto rotary = *context.rotary_knob; - const auto stop_intent = (last_rotary != Switch::MIDDLE && rotary == Switch::MIDDLE); - const auto land_intent = (last_rotary != Switch::DOWN && rotary == Switch::DOWN); - const auto rise_intent = (last_rotary != Switch::UP && rotary == Switch::UP) - || (last_keyboard.g == false && keyboard.g == true); + const auto nav_cross_dir = *context.nav_cross_direction; + const auto nav_is_climb = *context.nav_is_climb; - const auto step_direction = - std::isfinite(*context.target_yaw) ? *context.target_yaw : *context.measure_yaw; + const auto nav_request = + !std::isfinite(last_nav_cross_dir) && std::isfinite(nav_cross_dir); + const auto nav_canceled = + std::isfinite(last_nav_cross_dir) && !std::isfinite(nav_cross_dir); + + const auto step_direction = std::isfinite(*context.nav_cross_direction) + ? *context.nav_cross_direction + : *context.measure_yaw; + + const auto stop_intent = + (last_rotary != Switch::MIDDLE && rotary == Switch::MIDDLE) || nav_canceled; + const auto land_intent = (last_rotary != Switch::DOWN && rotary == Switch::DOWN) + || (nav_request && !nav_is_climb); + const auto rise_intent = (last_rotary != Switch::UP && rotary == Switch::UP) + || (last_keyboard.g == false && keyboard.g == true) + || (nav_request && nav_is_climb); do { if (context.is_estop() || stop_intent) { @@ -331,6 +347,7 @@ class SentryClimber if (!task_handler.done()) { cancel_task(); } else if (!std::isfinite(*context.measure_yaw)) { + *chassis_climb_status = -1; node::error("climb start rejected: measure_yaw invalid"); } else { task_handler = schduler.append(climb(step_direction)); @@ -341,6 +358,7 @@ class SentryClimber if (!task_handler.done()) { cancel_task(); } else if (!std::isfinite(*context.measure_yaw)) { + *chassis_climb_status = -1; node::error("land start rejected: measure_yaw invalid"); } else { task_handler = schduler.append(land(step_direction)); @@ -355,6 +373,8 @@ class SentryClimber last_keyboard = keyboard; last_rotary = rotary; + last_nav_cross_dir = nav_cross_dir; + co_await CoSchduler::Tick{}; } } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 1b72ab00..fecbe501 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -38,13 +38,15 @@ class Sentry get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); + register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, kNaN); register_output("/auto_aim/camera_transform", camera_transform_); register_output("/auto_aim/barrel_direction", barrel_direction_); - register_output( - "/auto_aim/yaw_velocity", yaw_velocity_, std::numeric_limits::quiet_NaN()); + register_output("/auto_aim/yaw_velocity", yaw_velocity_, kNaN); // 提供 remote-status 命令服务。 using Srv = std_srvs::srv::Trigger; @@ -78,6 +80,10 @@ class Sentry *barrel_direction_ = *fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); *yaw_velocity_ = gimbal_board_->yaw_velocity(); + + const auto chassis_direction = + fast_tf::cast(BaseLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + *chassis_measure_yaw_ = std::atan2(chassis_direction->y(), chassis_direction->x()); } private: @@ -309,7 +315,6 @@ class Sentry sentry.register_output("/referee/serial", referee_serial_); sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0.0); sentry.register_output("/chassis/pitch_imu", chassis_pitch_imu_, 0.0); - sentry.register_output("/chassis/climber/measure_yaw", chassis_measure_yaw_, 0.0); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( @@ -391,9 +396,6 @@ class Sentry const auto& q = snapshot->orientation; *chassis_pitch_imu_ = -std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); *chassis_yaw_velocity_imu_ = snapshot->gyro_body.z(); - *chassis_measure_yaw_ = std::atan2( - 2.0 * (q.w() * q.z() + q.x() * q.y()), - 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); } } @@ -567,7 +569,6 @@ class Sentry OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_imu_; - OutputInterface chassis_measure_yaw_; StatusMonitor monitor_{}; std::unique_ptr board_; @@ -633,6 +634,7 @@ class Sentry OutputInterface camera_transform_; OutputInterface barrel_direction_; OutputInterface yaw_velocity_; + OutputInterface chassis_measure_yaw_; std::unique_ptr gimbal_board_; std::unique_ptr chassis_board_; From 3f23cbc1d8c56f77360e04b6a78fc58e147ba8d7 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Jul 2026 09:28:15 +0800 Subject: [PATCH 14/15] chore: Update auto aim v2 --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index ab88ee07..27ec7806 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit ab88ee07910c366f0ceaad1a88264198d7f0047d +Subproject commit 27ec780607ab84a3dd7c8a0ca7ecd974bda0bc6f From d18e764de9c463ee31430a4d8ac470c62feed991 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Jul 2026 21:18:29 +0800 Subject: [PATCH 15/15] wip: Improve climber --- .../rmcs_core/src/controller/chassis/sentry_climber.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index 772f9cdb..3b60a0f5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -388,6 +388,8 @@ class SentryClimber } auto climb(double direction) -> CoSchduler::Task { + using namespace std::chrono_literals; + *chassis_climb_status = 0.0; node::info("Climb start, direction={:.3f}", direction); @@ -450,6 +452,12 @@ class SentryClimber node::warn("climb RETRACT stick timeout, continue"); } + *chassis_climb_speed = 0; + co_await CoSchduler::Sleep{500ms}; + + *chassis_climb_speed = config.climb.dash_vx; + co_await CoSchduler::Sleep{500ms}; + *chassis_climb_status = 1.0; release_climber(); }