diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 78e039a00..75a4fbfc0 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 diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index d977534fa..27ec78060 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit d977534fa7f1e50293ede5c1953834b30a02a0ff +Subproject commit 27ec780607ab84a3dd7c8a0ca7ecd974bda0bc6f diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 3565cc9b3..de6f840c8 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::AutoAimComponent -> auto_aim_component # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_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 @@ -79,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" @@ -123,34 +123,63 @@ gimbal_controller: chassis_controller: ros__parameters: + angular_velocity_max: 10.0 + translational_velocity_max: 10.0 following_velocity_kp: 7.0 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: 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: 1.0 + ki: 0.0 + kd: 0.0 + sync_coefficient: 0.2 + hold_torque: 0.01 + 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_duration: 0.8 + stick_timeout: 8.0 + approach_timeout: 8.0 + land: + 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 friction_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1803a1d48..95777e34d 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 db740632b..88d033483 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,42 +1,43 @@ #include "controller/pid/pid_calculator.hpp" +#include +#include + #include #include #include #include #include #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_max = +angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob", rotary_knob_); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); - register_input("/chassis/velocity", chassis_velocity_, false); - register_input("/chassis/climbing_forward_velocity", climbing_forward_velocity_, false); + register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 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); @@ -51,16 +52,21 @@ 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()) { - 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()) { @@ -77,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) @@ -119,6 +125,13 @@ 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; } @@ -133,8 +146,50 @@ 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; + following_velocity_controller_.reset(); } + auto update_spin_stuck_watchdog(rmcs_msgs::ChassisMode& mode) -> void { + constexpr auto kSpinStuckConfirmTicks = std::size_t{300}; + constexpr auto kSpinRecoveryTicks = std::size_t{1000}; + constexpr auto kSpinStuckAngularVelocityRatio = double{0.2}; + + using rmcs_msgs::ChassisMode; + + if (spin_recovery_count_ > 0) { + mode = ChassisMode::ALIGNMENT_POWERED; + + if (--spin_recovery_count_ == 0) + mode = mode_before_watchdog_; + + spin_stuck_count_ = 0; + return; + } + + 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_yaw_velocity_imu_) >= 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; + + 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(); @@ -142,14 +197,38 @@ 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()) - 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_; @@ -170,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; @@ -236,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_; @@ -260,17 +339,13 @@ 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_; 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; @@ -279,8 +354,10 @@ class ChassisController InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; - InputInterface chassis_velocity_; - InputInterface climbing_forward_velocity_; + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_climb_direction_; + InputInterface chassis_climb_speed_; + InputInterface chassis_measure_yaw_; InputInterface navigation_enable_control_; InputInterface navigation_command_velocity_; @@ -288,10 +365,15 @@ 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), - 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_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 7c701f633..b4bad88fb 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_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 000000000..654a23dbb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/co_schduler.hpp @@ -0,0 +1,231 @@ +#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(); + + 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) { + state = std::make_shared(State{ + .monitor = std::move(monitor), + .deadline = std::chrono::steady_clock::now() + timeout, + }); + + 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; + }; + } + + auto await_resume() const noexcept { return state->timed_out; } + }; + + 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 { + Handle() = default; + + 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 000000000..294cf362f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/climber/stick_group.hpp @@ -0,0 +1,196 @@ +#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, + kKeep, + } state = State::kFree; + + struct Config { + double speed_drop; + double speed_rise; + + double rise_torque_limit; + + // kLand：begin→final 速度变化时间(s)；越小越快贴到 final；t≥T 后恒 final 缓收 + double land_speed_begin; + double land_speed_final; + double land_duration; + double land_torque_limit; + + double blocked_torque_threshold; + double blocked_speed_threshold; + + double kp; + double ki; + double kd; + double sync_coefficient; + double hold_torque; + + 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: [[fallthrough]]; + case State::kKeep: return -speed_rise; + case State::kLand: { + // 指数进度 α：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(); + } + + auto get_torque_limit(State state) const noexcept { + switch (state) { + case State::kRise: [[fallthrough]]; + case State::kKeep: 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() { + if (state == State::kKeep && 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(); + 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 -> 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; + }; + + 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 000000000..fe8215176 --- /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/hero_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/hero_chassis_controller.cpp index 8fd643e54..9291886ed 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 new file mode 100644 index 000000000..3b60a0f54 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -0,0 +1,599 @@ +#include +#include +#include +#include +#include +#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(); + + using TrackState = climber::TrackGroup::State; + using StickState = climber::StickGroup::State; + + struct Config { + climber::TrackGroup::Config track; + climber::StickGroup::Config stick; + + struct Align { + double err; + double w; + double hold; + double timeout; + } align; + + 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_duration; + double stick_timeout; + double approach_timeout; + } climb; + + struct Land { + double dash_vx; + double soft_vx; + double land_pitch; + double land_delay; + 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), + .hold_torque = param_or("stick_group.hold_torque", 0.01), + }, + .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_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), + }, + .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), + .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), + }, + .block_hold = param_or("block_hold", 0.05), + }; + } + }; + + // 仅输入与派生；不负责 output + struct Context { + InputInterface l_switch; + InputInterface r_switch; + InputInterface keyboard; + InputInterface rotary_knob; + + InputInterface nav_cross_direction; + InputInterface nav_is_climb; + + InputInterface chassis_pitch; + InputInterface chassis_yaw_rate; + InputInterface measure_yaw; + + static constexpr auto normalize_angle(double angle) noexcept { + while (angle >= std::numbers::pi) + angle -= 2.0 * std::numbers::pi; + while (angle < -std::numbers::pi) + 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); + component.register_input("/remote/rotary_knob_switch", rotary_knob, false); + + 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/measure_yaw", measure_yaw, false); + } + + 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(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"); + ensure_bind(rotary_knob, Switch::UNKNOWN, "rotary_knob"); + + ensure_bind(chassis_pitch, 0.0, "chassis_pitch"); + ensure_bind(chassis_yaw_rate, 0.0, "chassis_yaw_rate"); + 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, 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; + + OutputInterface chassis_track_direction; // 以履带方向为正向 + OutputInterface chassis_climb_speed; // 正向为基准的速度值 + OutputInterface chassis_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() + "_output", [this] { std::ignore = this; }), + }; + + std::unique_ptr track_group; + std::unique_ptr stick_group; + CoSchduler schduler; + + Config config; + CoSchduler::Handle task_handler; + + static constexpr auto seconds_to_duration(double seconds) noexcept { + return std::chrono::duration_cast( + std::chrono::duration{seconds}); + } + + auto release_climber() noexcept { + *chassis_track_direction = kNaN; + *chassis_climb_speed = kNaN; + 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) { + 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; + + 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(); + task_handler = {}; + } + *chassis_climb_status = 0.0; + release_climber(); + }; + + while (true) { + const auto keyboard = *context.keyboard; + const auto rotary = *context.rotary_knob; + + const auto nav_cross_dir = *context.nav_cross_direction; + const auto nav_is_climb = *context.nav_is_climb; + + 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) { + cancel_task(); + break; + } + if (rise_intent) { + 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)); + } + break; + } + if (land_intent) { + 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)); + } + break; + } + } while (false); + + if (!context.is_estop() && task_handler.done()) + release_climber(); + + last_keyboard = keyboard; + last_rotary = rotary; + + last_nav_cross_dir = nav_cross_dir; + + co_await CoSchduler::Tick{}; + } + } + + auto spin_groups() -> CoSchduler::Task { + while (true) { + track_group->spin_once(); + stick_group->spin_once(); + co_await CoSchduler::Tick{}; + } + } + + auto climb(double direction) -> CoSchduler::Task { + using namespace std::chrono_literals; + + *chassis_climb_status = 0.0; + + node::info("Climb start, direction={:.3f}", direction); + *chassis_track_direction = direction; + + // [] 将底盘与台阶方向对齐 + 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("climb ALIGN failed"); + release_climber(); + *chassis_climb_status = -1; + co_return; + } + } + + // [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度 + 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"); + } + + // [] 伸出撑杆，同时慢速向台阶方向前进 + 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"); + } + + // [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔 + 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)}; + + // [] 上台阶完毕，收回撑杆 + track_group->set_state(TrackState::kHold); + 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"); + } + + *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(); + } + + auto land(double direction) -> CoSchduler::Task { + *chassis_climb_status = 0.0; + + node::info("Land start, direction={:.3f}", direction); + *chassis_track_direction = direction + std::numbers::pi; + + // [] 底盘对齐方向，准备下台阶 + 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) { + node::warn("land ALIGN failed"); + release_climber(); + *chassis_climb_status = -1; + co_return; + } + } + + // [] 伸出撑杆，以较快速度冲下台阶 + 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 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), + }; + 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); + *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"); + } + + // [] 等待完全落地，底盘倾角趋近水平 + { + 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"); + } + + // [] 完全收回撑杆，结束下台阶 + 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"); + } + + *chassis_climb_status = 1.0; + release_climber(); + } + +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_track_direction, kNaN); + output_component->register_output("/chassis/climber/speed", chassis_climb_speed, kNaN); + output_component->register_output("/chassis/climber/status", chassis_climb_status, 0.0); + + schduler.append(spin_context()); + schduler.append(spin_groups()); + } + + auto before_updating() -> void override { + context.load_fallback([this](std::string_view name) { + 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_climber(); + } + } +}; + +} // 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/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp index 0f2734ec1..bea900342 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,44 +69,35 @@ class EccentricDualYaw return; } - // 导航控制。 - if (input_.enable_navigation()) { - 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); + 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(); - 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; + auto nav_yshift = double{0.}; + auto nav_pshift = double{0.}; + if (input_.enable_navigation()) { + constexpr auto kGimbalFree = std::numeric_limits::min(); + const auto& toward = *input_.navigation_toward; + if (toward.x() == kGimbalFree && toward.y() == kGimbalFree) { + enter_disabled_state(); + 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_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index ba0ec4fac..51895755b 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 c61b874e7..689436bc6 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 4272bfda1..327999820 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; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 4233d7c76..fecbe501f 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: @@ -628,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_; 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 182b95cc7..a279b92c7 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,15 @@ enum class ChassisMode : uint8_t { LAUNCH_RAMP, ALIGNMENT, ALIGNMENT_POWERED, + CLIMB, }; -constexpr auto need_power(ChassisMode mode) noexcept { - return mode == ChassisMode::ALIGNMENT_POWERED || mode == ChassisMode::LAUNCH_RAMP; +constexpr auto is_powered(ChassisMode mode) noexcept { + 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; } } // namespace rmcs_msgs 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 61e063e2a..2c82568ba 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 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 0a0369976..24eb14d19 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