From 27c0c9139a9189cec3a6ff9a883ab4e74fb40751 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Thu, 19 Mar 2026 03:31:26 +0800 Subject: [PATCH 01/19] Preserve first-fill launch preparation flow --- src/manager/dart_manager.cpp | 15 ++- src/manager/task/launch_preparation_task.hpp | 107 +++++++++++++------ 2 files changed, 86 insertions(+), 36 deletions(-) diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 603ad07..9b0dfb2 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -162,6 +162,8 @@ class DartManagerV2 RCLCPP_WARN(logger_, "[DartManagerV2] all tasks cancelled"); } + first_fill_pending_ = true; + enter_belt_wait_zero_velocity_mode(); *lifting_command_ = rmcs_msgs::DartSliderStatus::WAIT; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; @@ -178,7 +180,10 @@ class DartManagerV2 RCLCPP_INFO(logger_, "[DartManagerV2] recovered from ERROR, state=IDLE"); transition_to(State::IDLE); } + + first_fill_pending_ = true; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; + // 无论 ERROR 还是 IDLE，都重新排队传送带复位 submit_task(make_slider_init_task()); RCLCPP_INFO(logger_, "[DartManagerV2] queued SliderInitTask for recovery"); @@ -265,6 +270,11 @@ class DartManagerV2 // 任务工厂 std::shared_ptr make_task(const std::string& cmd) { if (cmd == "launch_prepare" || cmd == "launch-prepare") { + auto launch_mode = first_fill_pending_ + ? LaunchPreparationTask::Mode::FIRST_FILL + : LaunchPreparationTask::Mode::NORMAL; + first_fill_pending_ = false; + return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, @@ -274,10 +284,12 @@ class DartManagerV2 *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_); + lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, launch_mode); } if (cmd == "unload" || cmd == "cancel_launch") { + first_fill_pending_ = true; + return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, @@ -367,6 +379,7 @@ class DartManagerV2 std::shared_ptr current_task_; std::deque> task_queue_; + bool first_fill_pending_{true}; bool first_tick_of_task_{true}; }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index ccebd02..b1d5c6b 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -14,13 +14,25 @@ namespace rmcs_dart_guidance::manager { -// LaunchPreparationTask — 将机构切换到发射准备态： -// 1. 同步带下行到装填位 -// 2. 短暂等待机构稳定 -// 3. 扳机锁定与填装升降下行并行执行 -// 4. 同步带上行复位 +// LaunchPreparationTask — 将机构切换到发射准备态。 +// NORMAL: +// 1. 同步带下行到装填位 +// 2. 短暂等待机构稳定 +// 3. 扳机锁定与填装升降下行并行执行 +// 4. 同步带上行复位 +// FIRST_FILL: +// 1. 填装升降上行 +// 2. 同步带下行到装填位 +// 3. 短暂等待机构稳定 +// 4. 扳机锁定 +// 5. 同步带上行复位 class LaunchPreparationTask : public Task { public: + enum class Mode { + NORMAL, + FIRST_FILL, + }; + LaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, @@ -29,15 +41,15 @@ class LaunchPreparationTask : public Task { rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, double lifting_stall_threshold, uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, - uint64_t lifting_stall_timeout_ticks) + uint64_t lifting_stall_timeout_ticks, Mode mode = Mode::NORMAL) : Task("launch_preparation", "滑块发射准备") { // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 double torque_limit = 5.0; double hold_torque = 1.0; // Wait 时的保持力矩 - then( - std::make_shared( + auto make_belt_move_down = [&]() { + return std::make_shared( "belt_move_down", // 动作名称 belt_command, // 同步带目标状态（输出） belt_target_velocity, // 同步带目标速度（输出） @@ -57,38 +69,41 @@ class LaunchPreparationTask : public Task { 0.5, // 堵转力矩阈值 100, // 堵转确认帧数 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE)); + BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE); + }; - then( - std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 - )); + auto make_belt_wait = [&]() { + return std::make_shared( + "belt_wait", // 动作名称 + 50 // 等待帧数 + ); + }; - auto parallel_prepare = - std::make_shared("parallel_prepare", ActionSet::Policy::ALL_SUCCESS); - parallel_prepare - ->also(std::make_shared( + auto make_trigger_lock = [&]() { + return std::make_shared( trigger_lock_enable, // 扳机锁定使能（输出） true, // 锁定（true） 1000 // 等待锁定完成帧数 - )) - .also( - std::make_shared( - "filling_lift_down", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 - )); - then(parallel_prepare); + ); + }; + + auto make_filling_lift = + [&](const char* name, rmcs_msgs::DartSliderStatus command) { + return std::make_shared( + name, // 动作名称 + lifting_command, // 升降指令（输出） + command, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + lifting_stall_threshold, // 堵转速度阈值 + lifting_stall_confirm_ticks, // 堵转确认帧数 + lifting_stall_min_run_ticks, // 最短运行帧数 + lifting_stall_timeout_ticks // 超时帧数 + ); + }; - then( - std::make_shared( + auto make_belt_reset = [&]() { + return std::make_shared( "belt_reset", // 动作名称 belt_command, // 同步带目标状态（输出） belt_target_velocity, // 同步带目标速度（输出） @@ -108,7 +123,29 @@ class LaunchPreparationTask : public Task { 0.5, // 堵转力矩阈值 200, // 堵转确认帧数 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY); + }; + + if (mode == Mode::FIRST_FILL) { + then(make_filling_lift("filling_lift_up", rmcs_msgs::DartSliderStatus::UP)); + then(make_belt_move_down()); + then(make_belt_wait()); + then(make_trigger_lock()); + then(make_belt_reset()); + return; + } + + then(make_belt_move_down()); + then(make_belt_wait()); + + auto parallel_prepare = + std::make_shared("parallel_prepare", ActionSet::Policy::ALL_SUCCESS); + parallel_prepare + ->also(make_trigger_lock()) + .also(make_filling_lift("filling_lift_down", rmcs_msgs::DartSliderStatus::DOWN)); + then(parallel_prepare); + + then(make_belt_reset()); } }; From 42d7051bb2b78cee09ea2b3a9939af69fc836b5b Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Thu, 19 Mar 2026 04:24:24 +0800 Subject: [PATCH 02/19] Add optional vision-assisted launch preparation --- CMakeLists.txt | 49 +++- package.xml | 1 + src/manager/action/auto_aim_action.hpp | 226 ++++++++++++++ src/manager/action/filling_lift_action.hpp | 12 +- src/manager/auto_aim_feedback.hpp | 68 +++++ src/manager/dart_launch_sequence.hpp | 101 +++++++ src/manager/dart_manager.cpp | 275 +++++++++++++++++- src/manager/task/cancel_launch_task.hpp | 10 +- src/manager/task/launch_preparation_task.hpp | 55 ++-- ...ision_assisted_launch_preparation_task.hpp | 49 ++++ tests/auto_aim_action_test.cpp | 126 ++++++++ tests/dart_launch_sequence_test.cpp | 39 +++ tests/launch_preparation_task_test.cpp | 80 +++++ ..._assisted_launch_preparation_task_test.cpp | 125 ++++++++ 14 files changed, 1169 insertions(+), 47 deletions(-) create mode 100644 src/manager/action/auto_aim_action.hpp create mode 100644 src/manager/auto_aim_feedback.hpp create mode 100644 src/manager/dart_launch_sequence.hpp create mode 100644 src/manager/task/vision_assisted_launch_preparation_task.hpp create mode 100644 tests/auto_aim_action_test.cpp create mode 100644 tests/dart_launch_sequence_test.cpp create mode 100644 tests/launch_preparation_task_test.cpp create mode 100644 tests/vision_assisted_launch_preparation_task_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e3b8e5..086cf06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,6 +48,10 @@ file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp ${PROJECT_SOURCE_DIR}/src/*.cc +) + +file(GLOB DEBUG_COMPONENT_SOURCE + CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/test/*.cpp ${PROJECT_SOURCE_DIR}/test/*.cc ) @@ -55,6 +59,7 @@ file(GLOB_RECURSE PROJECT_SOURCE add_library( ${PROJECT_NAME} SHARED ${PROJECT_SOURCE} + ${DEBUG_COMPONENT_SOURCE} ) ament_target_dependencies(${PROJECT_NAME} @@ -98,4 +103,46 @@ pluginlib_export_plugin_description_file( rmcs_executor plugins.xml ) -ament_package() \ No newline at end of file +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest( + auto_aim_action_test + tests/auto_aim_action_test.cpp + ) + + ament_add_gtest( + launch_preparation_task_test + tests/launch_preparation_task_test.cpp + ) + + ament_add_gtest( + vision_assisted_launch_preparation_task_test + tests/vision_assisted_launch_preparation_task_test.cpp + ) + + ament_add_gtest( + dart_launch_sequence_test + tests/dart_launch_sequence_test.cpp + ) + + foreach(target + auto_aim_action_test + launch_preparation_task_test + vision_assisted_launch_preparation_task_test + dart_launch_sequence_test) + if(TARGET ${target}) + target_include_directories(${target} PRIVATE + ${PROJECT_SOURCE_DIR}/src/ + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ) + + ament_target_dependencies(${target} + rclcpp + ) + endif() + endforeach() +endif() + +ament_package() diff --git a/package.xml b/package.xml index 474d59a..e8a551f 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest ament_cmake diff --git a/src/manager/action/auto_aim_action.hpp b/src/manager/action/auto_aim_action.hpp new file mode 100644 index 0000000..fc9418f --- /dev/null +++ b/src/manager/action/auto_aim_action.hpp @@ -0,0 +1,226 @@ +#pragma once + +#include "action.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_dart_guidance::manager { + +struct AutoAimParams { + Eigen::Vector2d desired_target_px = Eigen::Vector2d::Zero(); + Eigen::Vector2d deadband_px = Eigen::Vector2d::Constant(3.0); + Eigen::Vector2d ready_exit_deadband_px = Eigen::Vector2d::Constant(5.0); + Eigen::Vector2d accept_deadband_px = Eigen::Vector2d::Constant(8.0); + double yaw_gain = 0.0; + double pitch_gain = 0.0; + uint64_t ready_confirm_ticks = 5; + uint64_t timeout_ticks = 3000; + double min_transform_rate = 0.0; + double max_transform_rate = 500.0; +}; + +// AutoAimAction: +// - 在上膛前执行视觉辅助预瞄 +// - 严格对准进入 ready，误差在更宽的 accept_deadband 内也可直接放行 +// - tracking 丢失不失败，等待重新跟踪或超时 +// - 超时始终返回 SUCCESS，后续流程继续执行 +class AutoAimAction : public IAction { +public: + AutoAimAction( + Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, + Eigen::Vector2d& desired_target_px_output, const cv::Point2i& current_target_position, + const bool& tracking, const rclcpp::Logger& logger, AutoAimParams params) + : IAction("auto_aim") + , yaw_pitch_control_velocity_(yaw_pitch_control_velocity) + , aim_ready_(aim_ready) + , aim_error_px_(aim_error_px) + , desired_target_px_output_(desired_target_px_output) + , current_target_position_(current_target_position) + , tracking_(tracking) + , logger_(logger) + , params_(std::move(params)) {} + + void on_enter() override { + strict_ready_ticks_ = 0; + acceptable_ticks_ = 0; + completed_successfully_ = false; + last_valid_error_px_ = Eigen::Vector2d::Zero(); + last_tracking_valid_ = false; + has_seen_valid_target_ = false; + aim_ready_ = false; + aim_error_px_ = Eigen::Vector2d::Zero(); + desired_target_px_output_ = params_.desired_target_px; + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + last_log_time_ = std::chrono::steady_clock::now(); + } + + ActionStatus update() override { + desired_target_px_output_ = params_.desired_target_px; + + if (elapsed_ticks() >= params_.timeout_ticks) { + completed_successfully_ = true; + aim_ready_ = false; + aim_error_px_ = has_seen_valid_target_ ? last_valid_error_px_ : Eigen::Vector2d::Zero(); + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + log_timeout(); + return ActionStatus::SUCCESS; + } + + const bool tracking_valid = + tracking_ && current_target_position_.x >= 0 && current_target_position_.y >= 0; + last_tracking_valid_ = tracking_valid; + if (!tracking_valid) { + strict_ready_ticks_ = 0; + acceptable_ticks_ = 0; + aim_ready_ = false; + aim_error_px_ = Eigen::Vector2d::Zero(); + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + log_status(current_target_position_, aim_error_px_); + return ActionStatus::RUNNING; + } + + const Eigen::Vector2d current_target_px( + static_cast(current_target_position_.x), + static_cast(current_target_position_.y)); + const Eigen::Vector2d error_px = params_.desired_target_px - current_target_px; + has_seen_valid_target_ = true; + last_valid_error_px_ = error_px; + aim_error_px_ = error_px; + + const Eigen::Vector2d ready_exit_deadband_px = + params_.ready_exit_deadband_px.cwiseMax(params_.deadband_px); + const Eigen::Vector2d accept_deadband_px = + params_.accept_deadband_px.cwiseMax(ready_exit_deadband_px); + + const bool within_deadband = + std::abs(error_px.x()) <= params_.deadband_px.x() + && std::abs(error_px.y()) <= params_.deadband_px.y(); + const bool within_ready_hold_deadband = + std::abs(error_px.x()) <= ready_exit_deadband_px.x() + && std::abs(error_px.y()) <= ready_exit_deadband_px.y(); + const bool within_accept_deadband = + std::abs(error_px.x()) <= accept_deadband_px.x() + && std::abs(error_px.y()) <= accept_deadband_px.y(); + + if (aim_ready_ && within_ready_hold_deadband) { + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + completed_successfully_ = true; + log_status(current_target_position_, error_px); + return ActionStatus::SUCCESS; + } + + if (within_deadband) { + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + acceptable_ticks_ = 0; + ++strict_ready_ticks_; + aim_ready_ = strict_ready_ticks_ >= params_.ready_confirm_ticks; + log_status(current_target_position_, error_px); + if (aim_ready_) { + completed_successfully_ = true; + return ActionStatus::SUCCESS; + } + return ActionStatus::RUNNING; + } + + if (within_accept_deadband) { + strict_ready_ticks_ = 0; + acceptable_ticks_++; + aim_ready_ = false; + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + log_status(current_target_position_, error_px); + if (acceptable_ticks_ >= params_.ready_confirm_ticks) { + completed_successfully_ = true; + return ActionStatus::SUCCESS; + } + return ActionStatus::RUNNING; + } + + strict_ready_ticks_ = 0; + acceptable_ticks_ = 0; + aim_ready_ = false; + yaw_pitch_control_velocity_.x() = compute_axis_velocity(error_px.x(), params_.yaw_gain); + yaw_pitch_control_velocity_.y() = compute_axis_velocity(error_px.y(), params_.pitch_gain); + log_status(current_target_position_, error_px); + return ActionStatus::RUNNING; + } + + void on_exit() override { + yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + if (completed_successfully_) { + return; + } + + aim_ready_ = false; + aim_error_px_ = Eigen::Vector2d::Zero(); + } + +private: + void log_status(const cv::Point2i& current_target_position, const Eigen::Vector2d& error_px) { + const auto now = std::chrono::steady_clock::now(); + if (now - last_log_time_ < std::chrono::seconds(1)) { + return; + } + + last_log_time_ = now; + RCLCPP_INFO( + logger_, + "[AutoAimAction] desired=(%.1f, %.1f) current=(%d, %d) error=(%.1f, %.1f)", + params_.desired_target_px.x(), params_.desired_target_px.y(), current_target_position.x, + current_target_position.y, error_px.x(), error_px.y()); + } + + void log_timeout() const { + RCLCPP_WARN( + logger_, + "[AutoAimAction] timeout: tracking=%s last_error=(%.1f, %.1f)", + last_tracking_valid_ ? "true" : "false", aim_error_px_.x(), aim_error_px_.y()); + } + + double compute_axis_velocity(double error_px, double gain) const { + if (std::abs(error_px) <= 0.0 || std::abs(gain) <= 0.0) { + return 0.0; + } + + const double velocity = error_px * gain; + const double bounded_velocity = std::clamp( + velocity, -params_.max_transform_rate, params_.max_transform_rate); + const double bounded_magnitude = std::abs(bounded_velocity); + if (bounded_magnitude <= 0.0) { + return 0.0; + } + + const double final_magnitude = std::clamp( + std::max(bounded_magnitude, params_.min_transform_rate), + 0.0, params_.max_transform_rate); + return std::copysign(final_magnitude, bounded_velocity); + } + + Eigen::Vector2d& yaw_pitch_control_velocity_; + bool& aim_ready_; + Eigen::Vector2d& aim_error_px_; + Eigen::Vector2d& desired_target_px_output_; + + const cv::Point2i& current_target_position_; + const bool& tracking_; + rclcpp::Logger logger_; + AutoAimParams params_; + + uint64_t strict_ready_ticks_{0}; + uint64_t acceptable_ticks_{0}; + bool completed_successfully_{false}; + Eigen::Vector2d last_valid_error_px_{Eigen::Vector2d::Zero()}; + bool last_tracking_valid_{false}; + bool has_seen_valid_target_{false}; + std::chrono::steady_clock::time_point last_log_time_{}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/filling_lift_action.hpp b/src/manager/action/filling_lift_action.hpp index 7ffb98e..220ac59 100644 --- a/src/manager/action/filling_lift_action.hpp +++ b/src/manager/action/filling_lift_action.hpp @@ -26,11 +26,14 @@ class FillingLiftAction : public IAction { /// @param stall_confirm_ticks 连续低速帧数确认堵转（100 ticks = 0.1s @ 1kHz） /// @param stall_min_run_ticks 启动后最少运行帧数，避免启动瞬间误触发 /// @param timeout_ticks 超时帧数，超时返回 FAILURE + /// @param require_motion_before_stall + /// 是否要求先观测到电机进入运动，再允许低速堵转判定成功。 + /// 复位动作可关闭该选项，以便已经在端点时直接判定到位。 FillingLiftAction( const char* name, rmcs_msgs::DartSliderStatus& lifting_command, rmcs_msgs::DartSliderStatus command, const double& left_vel_fb, const double& right_vel_fb, double stall_threshold, uint64_t stall_confirm_ticks, uint64_t stall_min_run_ticks, - uint64_t timeout_ticks = 5000) + uint64_t timeout_ticks = 5000, bool require_motion_before_stall = true) : IAction(name) , lifting_command_(lifting_command) , command_(command) @@ -39,7 +42,8 @@ class FillingLiftAction : public IAction { , stall_threshold_(stall_threshold) , stall_confirm_ticks_(stall_confirm_ticks) , stall_min_run_ticks_(stall_min_run_ticks) - , timeout_ticks_(timeout_ticks) {} + , timeout_ticks_(timeout_ticks) + , require_motion_before_stall_(require_motion_before_stall) {} void on_enter() override { lifting_command_ = command_; @@ -57,7 +61,8 @@ class FillingLiftAction : public IAction { has_started_motion_ = true; } - if (elapsed_ticks() < stall_min_run_ticks_ || !has_started_motion_) + if (elapsed_ticks() < stall_min_run_ticks_ + || (require_motion_before_stall_ && !has_started_motion_)) return ActionStatus::RUNNING; // 任一电机堵转即视为全部堵转（避免单侧堵转导致机构歪斜） @@ -82,6 +87,7 @@ class FillingLiftAction : public IAction { uint64_t stall_confirm_ticks_; uint64_t stall_min_run_ticks_; uint64_t timeout_ticks_; + bool require_motion_before_stall_{true}; uint64_t stall_count_{0}; bool has_started_motion_{false}; }; diff --git a/src/manager/auto_aim_feedback.hpp b/src/manager/auto_aim_feedback.hpp new file mode 100644 index 0000000..4e952b0 --- /dev/null +++ b/src/manager/auto_aim_feedback.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include + +namespace rmcs_dart_guidance::manager { + +class AutoAimFeedback { +public: + void bind( + Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, + Eigen::Vector2d& aim_error_px, Eigen::Vector2d& desired_target_px) { + yaw_pitch_control_velocity_ = &yaw_pitch_control_velocity; + aim_ready_ = &aim_ready; + aim_error_px_ = &aim_error_px; + desired_target_px_ = &desired_target_px; + } + + void reset(const Eigen::Vector2d& desired_target_px) { + ensure_bound(); + *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); + *aim_ready_ = false; + *aim_error_px_ = Eigen::Vector2d::Zero(); + *desired_target_px_ = desired_target_px; + } + + void set_desired_target_px(const Eigen::Vector2d& desired_target_px) { + ensure_bound(); + *desired_target_px_ = desired_target_px; + } + + Eigen::Vector2d& yaw_pitch_control_velocity() { + ensure_bound(); + return *yaw_pitch_control_velocity_; + } + + bool& aim_ready() { + ensure_bound(); + return *aim_ready_; + } + + Eigen::Vector2d& aim_error_px() { + ensure_bound(); + return *aim_error_px_; + } + + Eigen::Vector2d& desired_target_px() { + ensure_bound(); + return *desired_target_px_; + } + +private: + void ensure_bound() const { + if ( + yaw_pitch_control_velocity_ == nullptr || aim_ready_ == nullptr + || aim_error_px_ == nullptr || desired_target_px_ == nullptr) { + throw std::runtime_error("AutoAimFeedback is not bound"); + } + } + + Eigen::Vector2d* yaw_pitch_control_velocity_{nullptr}; + bool* aim_ready_{nullptr}; + Eigen::Vector2d* aim_error_px_{nullptr}; + Eigen::Vector2d* desired_target_px_{nullptr}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_launch_sequence.hpp b/src/manager/dart_launch_sequence.hpp new file mode 100644 index 0000000..a9df988 --- /dev/null +++ b/src/manager/dart_launch_sequence.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +struct DartLaunchSequenceConfig { + size_t dart_count{4}; + Eigen::Vector2d aim_reference_pixel{Eigen::Vector2d::Zero()}; + std::vector aim_dart_offsets_px{}; +}; + +struct DartLaunchSequenceRawConfig { + int64_t dart_count{4}; + std::vector aim_reference_pixel{}; + std::vector aim_dart_offsets_px{}; +}; + +class DartLaunchSequence { +public: + void configure_from_parameter_values(DartLaunchSequenceRawConfig raw_config) { + if (raw_config.dart_count <= 0 || raw_config.dart_count > 255) { + throw std::runtime_error("dart_count must be within 1..255"); + } + if (raw_config.aim_reference_pixel.size() != 2) { + throw std::runtime_error("aim_reference_pixel must contain exactly 2 values"); + } + + const size_t dart_count = static_cast(raw_config.dart_count); + if (raw_config.aim_dart_offsets_px.size() != dart_count * 2) { + throw std::runtime_error("aim_dart_offsets_px size must equal 2 * dart_count"); + } + + std::vector aim_dart_offsets_px; + aim_dart_offsets_px.reserve(dart_count); + for (size_t i = 0; i < dart_count; ++i) { + aim_dart_offsets_px.emplace_back( + raw_config.aim_dart_offsets_px[2 * i], + raw_config.aim_dart_offsets_px[2 * i + 1]); + } + + configure(DartLaunchSequenceConfig{ + .dart_count = dart_count, + .aim_reference_pixel = Eigen::Vector2d( + raw_config.aim_reference_pixel[0], raw_config.aim_reference_pixel[1]), + .aim_dart_offsets_px = std::move(aim_dart_offsets_px), + }); + } + + void configure(DartLaunchSequenceConfig config) { + if (config.dart_count == 0 || config.dart_count > 255) { + throw std::runtime_error("dart_count must be within 1..255"); + } + if (config.aim_dart_offsets_px.size() != config.dart_count) { + throw std::runtime_error("aim_dart_offsets_px size must equal dart_count"); + } + + config_ = std::move(config); + reset(); + } + + void reset() { current_dart_index_ = 0; } + + bool advance_after_fire() { + if (current_dart_index_ + 1 >= config_.dart_count) { + return false; + } + + ++current_dart_index_; + return true; + } + + size_t current_dart_index() const { return current_dart_index_; } + + uint8_t current_dart_index_u8() const { + return static_cast(current_dart_index_); + } + + Eigen::Vector2d current_desired_target_px() const { + if (config_.aim_dart_offsets_px.empty()) { + return config_.aim_reference_pixel; + } + + const size_t clamped_index = + std::min(current_dart_index_, config_.aim_dart_offsets_px.size() - 1); + return config_.aim_reference_pixel + config_.aim_dart_offsets_px[clamped_index]; + } + +private: + DartLaunchSequenceConfig config_{}; + size_t current_dart_index_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 9b0dfb2..e9c93eb 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -1,21 +1,29 @@ #include "manager/action/action.hpp" #include "manager/action/manual_angle_control.hpp" #include "manager/action/manual_force_control.hpp" +#include "manager/auto_aim_feedback.hpp" +#include "manager/dart_launch_sequence.hpp" #include "manager/task/cancel_launch_task.hpp" #include "manager/task/fire_and_preload_task.hpp" #include "manager/task/launch_preparation_task.hpp" #include "manager/task/silder_init_task.hpp" #include "manager/task/task.hpp" +#include "manager/task/vision_assisted_launch_preparation_task.hpp" +#include #include #include #include +#include #include +#include #include +#include #include #include #include +#include #include #include #include @@ -56,6 +64,8 @@ class DartManagerV2 register_input("/remote/joystick/left", joystick_left_, false); register_input("/remote/joystick/right", joystick_right_, false); + register_input("/dart_guidance/camera/target_position", target_position_input_, false); + register_input("/dart_guidance/tracker/tracking", target_tracking_input_, false); register_output("/dart/manager/belt/command", belt_command_, rmcs_msgs::DartSliderStatus::WAIT); register_output("/dart/manager/belt/target_velocity", belt_target_velocity_, 0.0); @@ -66,6 +76,15 @@ class DartManagerV2 register_output("/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); register_output("/force/control/velocity", force_control_velocity_, 0.0); + register_output("/dart/manager/aim/ready", aim_ready_, false); + register_output( + "/dart/manager/aim/current_dart_index", aim_current_dart_index_, + static_cast(0)); + register_output("/dart/manager/aim/error_px", aim_error_px_, Eigen::Vector2d::Zero()); + register_output( + "/dart/manager/aim/desired_target_px", aim_desired_target_px_, Eigen::Vector2d::Zero()); + auto_aim_feedback_.bind( + *yaw_pitch_control_velocity_, *aim_ready_, *aim_error_px_, *aim_desired_target_px_); // 升降指令总线 register_output( @@ -85,6 +104,13 @@ class DartManagerV2 } catch (...) { manual_force_scale_ = 5.0; } + launch_prepare_enable_visual_assist_ = + has_parameter("launch_prepare_enable_visual_assist") + ? get_parameter("launch_prepare_enable_visual_assist").as_bool() + : false; + if (launch_prepare_enable_visual_assist_) { + load_auto_aim_configuration(); + } limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); @@ -98,11 +124,14 @@ class DartManagerV2 state_pub_ = create_publisher("/dart/manager/state", 10); + sync_current_dart_outputs(); + clear_auto_aim_feedback(); submit_task(make_slider_init_task()); RCLCPP_INFO(logger_, "[DartManagerV2] initialized, queued SliderInitTask on startup"); } void update() override { + refresh_auto_aim_inputs(); poll_command(); switch (state_) { @@ -169,6 +198,7 @@ class DartManagerV2 *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); *force_control_velocity_ = 0.0; + clear_auto_aim_feedback(); transition_to(State::IDLE); } @@ -183,6 +213,10 @@ class DartManagerV2 first_fill_pending_ = true; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; + if (launch_prepare_enable_visual_assist_) { + reset_dart_sequence(); + } + clear_auto_aim_feedback(); // 无论 ERROR 还是 IDLE，都重新排队传送带复位 submit_task(make_slider_init_task()); @@ -195,6 +229,7 @@ class DartManagerV2 current_task_ = std::move(task_queue_.front()); task_queue_.pop_front(); + prepare_outputs_for_task(*current_task_); RCLCPP_INFO( logger_, "[DartManagerV2] dispatching task: '%s'", current_task_->name().c_str()); @@ -212,9 +247,13 @@ class DartManagerV2 first_tick_of_task_ = false; if (status == ActionStatus::SUCCESS) { + const std::string completed_task_name = current_task_->name(); current_task_->on_exit(); RCLCPP_INFO( - logger_, "[DartManagerV2] task '%s' SUCCESS", current_task_->name().c_str()); + logger_, "[DartManagerV2] task '%s' SUCCESS", completed_task_name.c_str()); + if (launch_prepare_enable_visual_assist_ && completed_task_name == "fire") { + advance_dart_sequence_after_fire(); + } current_task_.reset(); transition_to(State::IDLE); @@ -236,6 +275,7 @@ class DartManagerV2 *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); *force_control_velocity_ = 0.0; + clear_auto_aim_feedback(); transition_to(State::ERROR); } @@ -258,6 +298,178 @@ class DartManagerV2 } } + void refresh_auto_aim_inputs() { + if (target_position_input_.ready()) { + current_target_position_ = *target_position_input_; + } else { + current_target_position_ = cv::Point2i(-1, -1); + } + + current_target_tracking_ = + target_tracking_input_.ready() ? *target_tracking_input_ : false; + } + + double get_numeric_parameter_or_throw(const std::string& name) const { + if (!has_parameter(name)) { + throw std::runtime_error("Missing required parameter: " + name); + } + + const auto parameter = get_parameter(name); + switch (parameter.get_type()) { + case rclcpp::PARAMETER_DOUBLE: return parameter.as_double(); + case rclcpp::PARAMETER_INTEGER: return static_cast(parameter.as_int()); + default: throw std::runtime_error("Parameter must be numeric: " + name); + } + } + + uint64_t get_uint_parameter_or_throw(const std::string& name) const { + if (!has_parameter(name)) { + throw std::runtime_error("Missing required parameter: " + name); + } + + const auto value = get_parameter(name).as_int(); + if (value <= 0) { + throw std::runtime_error("Parameter must be positive: " + name); + } + return static_cast(value); + } + + std::vector get_numeric_array_parameter_or_throw(const std::string& name) const { + if (!has_parameter(name)) { + throw std::runtime_error("Missing required parameter: " + name); + } + + const auto parameter = get_parameter(name); + switch (parameter.get_type()) { + case rclcpp::PARAMETER_DOUBLE_ARRAY: return parameter.as_double_array(); + case rclcpp::PARAMETER_INTEGER_ARRAY: { + const auto values = parameter.as_integer_array(); + std::vector result; + result.reserve(values.size()); + for (const auto value : values) { + result.push_back(static_cast(value)); + } + return result; + } + default: throw std::runtime_error("Parameter must be a numeric array: " + name); + } + } + + Eigen::Vector2d get_vector2_parameter_or_throw(const std::string& name) const { + const auto values = get_numeric_array_parameter_or_throw(name); + if (values.size() != 2) { + throw std::runtime_error("Parameter must contain exactly 2 values: " + name); + } + return Eigen::Vector2d(values[0], values[1]); + } + + void load_auto_aim_configuration() { + int64_t dart_count = 4; + if (has_parameter("dart_count")) { + dart_count = get_parameter("dart_count").as_int(); + } + + aim_deadband_px_ = has_parameter("aim_deadband_px") + ? get_vector2_parameter_or_throw("aim_deadband_px") + : Eigen::Vector2d::Constant(3.0); + aim_ready_exit_deadband_px_ = + has_parameter("aim_ready_exit_deadband_px") + ? get_vector2_parameter_or_throw("aim_ready_exit_deadband_px") + : Eigen::Vector2d::Constant(5.0); + aim_accept_deadband_px_ = + has_parameter("aim_accept_deadband_px") + ? get_vector2_parameter_or_throw("aim_accept_deadband_px") + : aim_ready_exit_deadband_px_; + aim_yaw_gain_ = get_numeric_parameter_or_throw("aim_yaw_gain"); + aim_pitch_gain_ = get_numeric_parameter_or_throw("aim_pitch_gain"); + aim_ready_confirm_ticks_ = has_parameter("aim_ready_confirm_ticks") + ? get_uint_parameter_or_throw("aim_ready_confirm_ticks") + : 5; + aim_timeout_ticks_ = get_uint_parameter_or_throw("aim_timeout_ticks"); + aim_min_transform_rate_ = has_parameter("aim_min_transform_rate") + ? get_numeric_parameter_or_throw("aim_min_transform_rate") + : 0.0; + auto_aim_max_transform_rate_ = + has_parameter("auto_aim_max_transform_rate") + ? get_numeric_parameter_or_throw("auto_aim_max_transform_rate") + : max_transform_rate_; + + if (aim_deadband_px_.x() < 0.0 || aim_deadband_px_.y() < 0.0) { + throw std::runtime_error("Parameter aim_deadband_px must be non-negative"); + } + if (aim_ready_exit_deadband_px_.x() < aim_deadband_px_.x() + || aim_ready_exit_deadband_px_.y() < aim_deadband_px_.y()) { + throw std::runtime_error( + "Parameter aim_ready_exit_deadband_px must be >= aim_deadband_px"); + } + if (aim_accept_deadband_px_.x() < aim_ready_exit_deadband_px_.x() + || aim_accept_deadband_px_.y() < aim_ready_exit_deadband_px_.y()) { + throw std::runtime_error( + "Parameter aim_accept_deadband_px must be >= aim_ready_exit_deadband_px"); + } + if (aim_min_transform_rate_ < 0.0) { + throw std::runtime_error("Parameter aim_min_transform_rate must be non-negative"); + } + if (auto_aim_max_transform_rate_ < 0.0) { + throw std::runtime_error("Parameter auto_aim_max_transform_rate must be non-negative"); + } + if (aim_min_transform_rate_ > auto_aim_max_transform_rate_) { + throw std::runtime_error( + "Parameter aim_min_transform_rate must be <= auto_aim_max_transform_rate"); + } + + dart_launch_sequence_.configure_from_parameter_values( + DartLaunchSequenceRawConfig{ + .dart_count = dart_count, + .aim_reference_pixel = get_numeric_array_parameter_or_throw("aim_reference_pixel"), + .aim_dart_offsets_px = get_numeric_array_parameter_or_throw("aim_dart_offsets_px"), + }); + } + + Eigen::Vector2d current_desired_target_px() const { + return launch_prepare_enable_visual_assist_ + ? dart_launch_sequence_.current_desired_target_px() + : Eigen::Vector2d::Zero(); + } + + void sync_current_dart_outputs() { + *aim_current_dart_index_ = + launch_prepare_enable_visual_assist_ ? dart_launch_sequence_.current_dart_index_u8() + : static_cast(0); + auto_aim_feedback_.set_desired_target_px(current_desired_target_px()); + } + + void clear_auto_aim_feedback() { + *aim_current_dart_index_ = + launch_prepare_enable_visual_assist_ ? dart_launch_sequence_.current_dart_index_u8() + : static_cast(0); + auto_aim_feedback_.reset(current_desired_target_px()); + } + + void reset_dart_sequence() { + if (launch_prepare_enable_visual_assist_) { + dart_launch_sequence_.reset(); + } + sync_current_dart_outputs(); + } + + void advance_dart_sequence_after_fire() { + if (!dart_launch_sequence_.advance_after_fire()) { + RCLCPP_WARN(logger_, "[DartManagerV2] current dart index already at the last dart"); + } + sync_current_dart_outputs(); + clear_auto_aim_feedback(); + } + + void prepare_outputs_for_task(const Task& task) { + if ( + task.name() == "launch_preparation" || task.name() == "fire" + || task.name() == "cancel_launch" || task.name() == "slider_init" + || task.name() == "manual_angle" || task.name() == "manual_force") { + clear_auto_aim_feedback(); + } + } + std::shared_ptr make_slider_init_task() { return std::make_shared( *belt_command_, @@ -275,14 +487,37 @@ class DartManagerV2 : LaunchPreparationTask::Mode::NORMAL; first_fill_pending_ = false; + if (launch_prepare_enable_visual_assist_) { + return std::make_shared( + *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, + *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, + *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, + *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, + lifting_stall_threshold_, lifting_stall_confirm_ticks_, + lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, launch_mode, + auto_aim_feedback_.yaw_pitch_control_velocity(), + auto_aim_feedback_.aim_ready(), auto_aim_feedback_.aim_error_px(), + auto_aim_feedback_.desired_target_px(), current_target_position_, + current_target_tracking_, logger_, + AutoAimParams{ + .desired_target_px = current_desired_target_px(), + .deadband_px = aim_deadband_px_, + .ready_exit_deadband_px = aim_ready_exit_deadband_px_, + .accept_deadband_px = aim_accept_deadband_px_, + .yaw_gain = aim_yaw_gain_, + .pitch_gain = aim_pitch_gain_, + .ready_confirm_ticks = aim_ready_confirm_ticks_, + .timeout_ticks = aim_timeout_ticks_, + .min_transform_rate = aim_min_transform_rate_, + .max_transform_rate = auto_aim_max_transform_rate_, + }); + } + return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, - *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, - *trigger_lock_enable_, - *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, + *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, + *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, + *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, lifting_stall_threshold_, lifting_stall_confirm_ticks_, lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, launch_mode); } @@ -315,9 +550,9 @@ class DartManagerV2 if (cmd == "manual_angle") { auto task = std::make_shared("manual_angle", "手动 yaw/pitch 调整"); task->then(std::make_shared( - (*yaw_pitch_control_velocity_)[0], (*yaw_pitch_control_velocity_)[1], - *joystick_left_, *joystick_right_, - max_transform_rate_)); + auto_aim_feedback_.yaw_pitch_control_velocity()[0], + auto_aim_feedback_.yaw_pitch_control_velocity()[1], *joystick_left_, + *joystick_right_, max_transform_rate_)); return task; } @@ -342,6 +577,8 @@ class DartManagerV2 InputInterface joystick_left_; InputInterface joystick_right_; + InputInterface target_position_input_; + InputInterface target_tracking_input_; // 升降速度反馈（FillingLiftAction 堵转检测用） InputInterface lifting_left_vel_fb_; @@ -356,18 +593,36 @@ class DartManagerV2 OutputInterface yaw_pitch_control_velocity_; OutputInterface force_control_velocity_; + OutputInterface aim_ready_; + OutputInterface aim_current_dart_index_; + OutputInterface aim_error_px_; + OutputInterface aim_desired_target_px_; OutputInterface lifting_command_; OutputInterface limiting_command_; double max_transform_rate_{500.0}; double manual_force_scale_{5.0}; + double auto_aim_max_transform_rate_{500.0}; uint64_t limiting_fill_ticks_{500}; double lifting_stall_threshold_{0.5}; uint64_t lifting_stall_confirm_ticks_{100}; uint64_t lifting_stall_min_run_ticks_{500}; uint64_t lifting_stall_timeout_ticks_{5000}; + bool launch_prepare_enable_visual_assist_{false}; + AutoAimFeedback auto_aim_feedback_; + DartLaunchSequence dart_launch_sequence_; + Eigen::Vector2d aim_deadband_px_{Eigen::Vector2d::Constant(3.0)}; + Eigen::Vector2d aim_ready_exit_deadband_px_{Eigen::Vector2d::Constant(5.0)}; + Eigen::Vector2d aim_accept_deadband_px_{Eigen::Vector2d::Constant(8.0)}; + double aim_yaw_gain_{0.0}; + double aim_pitch_gain_{0.0}; + uint64_t aim_ready_confirm_ticks_{5}; + uint64_t aim_timeout_ticks_{3000}; + double aim_min_transform_rate_{0.0}; + cv::Point2i current_target_position_{-1, -1}; + bool current_target_tracking_{false}; InputInterface remote_command_input_; InputInterface web_command_input_; diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index 122cbc9..45bbf5d 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -33,7 +33,7 @@ class CancelLaunchTask : public Task { : Task("cancel_launch", "取消发射") { // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double down_velocity = 15.0; + double down_velocity = 10.0; double up_velocity = 10.0; double hold_torque = 1.0; // Wait 时的保持力矩 @@ -51,9 +51,9 @@ class CancelLaunchTask : public Task { right_belt_torque, // 右同步带力矩（输入） rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 down_velocity, // 设定速度 - 5.0, // 设定力矩限制 + 50.0, // 设定力矩限制 hold_torque, // 设定保持力矩 - 10000, // 超时帧数 + 20000, // 超时帧数 1.0, // 堵转速度阈值 0.5, // 堵转力矩阈值 100, // 堵转确认帧数 @@ -87,9 +87,9 @@ class CancelLaunchTask : public Task { right_belt_torque, // 右同步带力矩（输入） rmcs_msgs::DartSliderStatus::UP, // 指令状态 up_velocity, // 设定速度 - 1.0, // 设定力矩限制 + 25.0, // 设定力矩限制 hold_torque, // 设定保持力矩 - 10000, // 超时帧数 + 20000, // 超时帧数 1.0, // 堵转速度阈值 0.5, // 堵转力矩阈值 100, // 堵转确认帧数 diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index b1d5c6b..88c2307 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -14,7 +14,7 @@ namespace rmcs_dart_guidance::manager { -// LaunchPreparationTask — 将机构切换到发射准备态。 +// LaunchPreparationTask — 纯机械发射准备任务。 // NORMAL: // 1. 同步带下行到装填位 // 2. 短暂等待机构稳定 @@ -45,8 +45,8 @@ class LaunchPreparationTask : public Task { : Task("launch_preparation", "滑块发射准备") { // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double torque_limit = 5.0; - double hold_torque = 1.0; // Wait 时的保持力矩 + double torque_limit = 50.0; + double hold_torque = 5.0; // Wait 时的保持力矩 auto make_belt_move_down = [&]() { return std::make_shared( @@ -64,7 +64,7 @@ class LaunchPreparationTask : public Task { 10.0, // 设定速度 torque_limit, // 设定力矩限制 hold_torque, // 设定保持力矩 - 10000, // 超时帧数 + 20000, // 超时帧数 1.0, // 堵转速度阈值 0.5, // 堵转力矩阈值 100, // 堵转确认帧数 @@ -74,33 +74,33 @@ class LaunchPreparationTask : public Task { auto make_belt_wait = [&]() { return std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 + "belt_wait", // 动作名称 + 50 // 等待帧数 ); }; auto make_trigger_lock = [&]() { return std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 1000 // 等待锁定完成帧数 + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 1000 // 等待锁定完成帧数 ); }; - auto make_filling_lift = - [&](const char* name, rmcs_msgs::DartSliderStatus command) { - return std::make_shared( - name, // 动作名称 - lifting_command, // 升降指令（输出） - command, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 - ); - }; + auto make_filling_lift = [&](const char* name, rmcs_msgs::DartSliderStatus command, + bool require_motion_before_stall = true) { + return std::make_shared( + name, // 动作名称 + lifting_command, // 升降指令（输出） + command, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + lifting_stall_threshold, // 堵转速度阈值 + lifting_stall_confirm_ticks, // 堵转确认帧数 + lifting_stall_min_run_ticks, // 最短运行帧数 + lifting_stall_timeout_ticks, // 超时帧数 + require_motion_before_stall); + }; auto make_belt_reset = [&]() { return std::make_shared( @@ -115,10 +115,10 @@ class LaunchPreparationTask : public Task { left_belt_torque, // 左同步带力矩（输入） right_belt_torque, // 右同步带力矩（输入） rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 20.0, // 设定速度 + 10.0, // 设定速度 torque_limit, // 设定力矩限制 hold_torque, // 设定保持力矩 - 5000, // 超时帧数 + 20000, // 超时帧数 0.5, // 堵转速度阈值 0.5, // 堵转力矩阈值 200, // 堵转确认帧数 @@ -127,7 +127,7 @@ class LaunchPreparationTask : public Task { }; if (mode == Mode::FIRST_FILL) { - then(make_filling_lift("filling_lift_up", rmcs_msgs::DartSliderStatus::UP)); + then(make_filling_lift("filling_lift_up", rmcs_msgs::DartSliderStatus::UP, false)); then(make_belt_move_down()); then(make_belt_wait()); then(make_trigger_lock()); @@ -140,8 +140,7 @@ class LaunchPreparationTask : public Task { auto parallel_prepare = std::make_shared("parallel_prepare", ActionSet::Policy::ALL_SUCCESS); - parallel_prepare - ->also(make_trigger_lock()) + parallel_prepare->also(make_trigger_lock()) .also(make_filling_lift("filling_lift_down", rmcs_msgs::DartSliderStatus::DOWN)); then(parallel_prepare); diff --git a/src/manager/task/vision_assisted_launch_preparation_task.hpp b/src/manager/task/vision_assisted_launch_preparation_task.hpp new file mode 100644 index 0000000..8a8a391 --- /dev/null +++ b/src/manager/task/vision_assisted_launch_preparation_task.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include "manager/action/auto_aim_action.hpp" +#include "manager/task/launch_preparation_task.hpp" +#include "manager/task/task.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace rmcs_dart_guidance::manager { + +// VisionAssistedLaunchPreparationTask: +// 1. 先执行视觉辅助预瞄 +// 2. 预瞄成功或超时后，再进入原有机械发射准备流程 +class VisionAssistedLaunchPreparationTask : public Task { +public: + VisionAssistedLaunchPreparationTask( + rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, + const double& left_belt_velocity, const double& right_belt_velocity, + const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, + rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, + const double& lifting_right_vel_fb, double lifting_stall_threshold, + uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, + uint64_t lifting_stall_timeout_ticks, LaunchPreparationTask::Mode mode, + Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, + Eigen::Vector2d& desired_target_px_output, const cv::Point2i& target_position, + const bool& target_tracking, const rclcpp::Logger& logger, AutoAimParams auto_aim_params) + : Task("launch_preparation", "视觉辅助滑块发射准备") { + + then(std::make_shared( + yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, + target_position, target_tracking, logger, std::move(auto_aim_params))); + + then(std::make_shared( + belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, + belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, left_belt_torque, + right_belt_torque, trigger_lock_enable, lifting_command, lifting_left_vel_fb, + lifting_right_vel_fb, lifting_stall_threshold, lifting_stall_confirm_ticks, + lifting_stall_min_run_ticks, lifting_stall_timeout_ticks, mode)); + } +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/tests/auto_aim_action_test.cpp b/tests/auto_aim_action_test.cpp new file mode 100644 index 0000000..76828e5 --- /dev/null +++ b/tests/auto_aim_action_test.cpp @@ -0,0 +1,126 @@ +#include "manager/action/auto_aim_action.hpp" + +#include + +#include + +#include +#include +#include + +namespace rmcs_dart_guidance::manager::test { + +class AutoAimActionTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + if (!rclcpp::ok()) { + ::setenv("ROS_LOG_DIR", "/tmp/rmcs_dart_guidance_test_logs", 1); + int argc = 0; + char** argv = nullptr; + rclcpp::init(argc, argv); + } + } + + static void TearDownTestSuite() { + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } + + struct Harness { + explicit Harness(AutoAimParams params) + : action( + yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, + target_position, tracking, rclcpp::get_logger("auto_aim_action_test"), + std::move(params)) {} + + ActionStatus tick_first() { return action.tick_first(); } + + ActionStatus tick() { return action.tick(); } + + Eigen::Vector2d yaw_pitch_control_velocity{Eigen::Vector2d::Zero()}; + bool aim_ready{false}; + Eigen::Vector2d aim_error_px{Eigen::Vector2d::Zero()}; + Eigen::Vector2d desired_target_px_output{Eigen::Vector2d::Zero()}; + cv::Point2i target_position{-1, -1}; + bool tracking{false}; + AutoAimAction action; + }; + + static AutoAimParams make_default_params() { + AutoAimParams params; + params.desired_target_px = Eigen::Vector2d(100.0, 100.0); + params.deadband_px = Eigen::Vector2d::Constant(3.0); + params.ready_exit_deadband_px = Eigen::Vector2d::Constant(5.0); + params.accept_deadband_px = Eigen::Vector2d::Constant(8.0); + params.yaw_gain = 0.1; + params.pitch_gain = 0.1; + params.ready_confirm_ticks = 1; + params.timeout_ticks = 4; + params.min_transform_rate = 0.0; + params.max_transform_rate = 10.0; + return params; + } +}; + +TEST_F(AutoAimActionTest, StrictReadyReturnsSuccessAndSetsReady) { + Harness harness(make_default_params()); + harness.tracking = true; + harness.target_position = cv::Point2i(98, 100); + + EXPECT_EQ(harness.tick_first(), ActionStatus::SUCCESS); + EXPECT_TRUE(harness.aim_ready); + EXPECT_TRUE(harness.yaw_pitch_control_velocity.isZero(1e-9)); + EXPECT_DOUBLE_EQ(harness.aim_error_px.x(), 2.0); + EXPECT_DOUBLE_EQ(harness.aim_error_px.y(), 0.0); +} + +TEST_F(AutoAimActionTest, AcceptableErrorReturnsSuccessWithoutReady) { + Harness harness(make_default_params()); + harness.tracking = true; + harness.target_position = cv::Point2i(94, 100); + + EXPECT_EQ(harness.tick_first(), ActionStatus::SUCCESS); + EXPECT_FALSE(harness.aim_ready); + EXPECT_TRUE(harness.yaw_pitch_control_velocity.isZero(1e-9)); + EXPECT_DOUBLE_EQ(harness.aim_error_px.x(), 6.0); + EXPECT_DOUBLE_EQ(harness.aim_error_px.y(), 0.0); +} + +TEST_F(AutoAimActionTest, UsesMinimumTransformRateOutsideAcceptDeadband) { + AutoAimParams params = make_default_params(); + params.min_transform_rate = 1.0; + + Harness harness(params); + harness.tracking = true; + harness.target_position = cv::Point2i(90, 100); + + EXPECT_EQ(harness.tick_first(), ActionStatus::RUNNING); + EXPECT_DOUBLE_EQ(harness.yaw_pitch_control_velocity.x(), 1.0); + EXPECT_DOUBLE_EQ(harness.yaw_pitch_control_velocity.y(), 0.0); + EXPECT_FALSE(harness.aim_ready); +} + +TEST_F(AutoAimActionTest, TimeoutPreservesLastValidErrorAfterTrackingLoss) { + Harness harness(make_default_params()); + harness.tracking = true; + harness.target_position = cv::Point2i(90, 100); + + EXPECT_EQ(harness.tick_first(), ActionStatus::RUNNING); + EXPECT_DOUBLE_EQ(harness.aim_error_px.x(), 10.0); + EXPECT_DOUBLE_EQ(harness.aim_error_px.y(), 0.0); + + harness.tracking = false; + harness.target_position = cv::Point2i(-1, -1); + + EXPECT_EQ(harness.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.tick(), ActionStatus::SUCCESS); + + EXPECT_FALSE(harness.aim_ready); + EXPECT_TRUE(harness.yaw_pitch_control_velocity.isZero(1e-9)); + EXPECT_DOUBLE_EQ(harness.aim_error_px.x(), 10.0); + EXPECT_DOUBLE_EQ(harness.aim_error_px.y(), 0.0); +} + +} // namespace rmcs_dart_guidance::manager::test diff --git a/tests/dart_launch_sequence_test.cpp b/tests/dart_launch_sequence_test.cpp new file mode 100644 index 0000000..2d46298 --- /dev/null +++ b/tests/dart_launch_sequence_test.cpp @@ -0,0 +1,39 @@ +#include "manager/dart_launch_sequence.hpp" + +#include + +#include + +namespace rmcs_dart_guidance::manager::test { + +TEST(DartLaunchSequenceTest, TracksDesiredTargetAcrossFiresAndReset) { + DartLaunchSequence sequence; + sequence.configure_from_parameter_values(DartLaunchSequenceRawConfig{ + .dart_count = 4, + .aim_reference_pixel = {100.0, 200.0}, + .aim_dart_offsets_px = {0.0, 0.0, 2.0, -1.0, 4.0, -2.0, 6.0, -3.0}, + }); + + EXPECT_EQ(sequence.current_dart_index(), 0U); + EXPECT_TRUE( + sequence.current_desired_target_px().isApprox(Eigen::Vector2d(100.0, 200.0), 1e-9)); + + EXPECT_TRUE(sequence.advance_after_fire()); + EXPECT_EQ(sequence.current_dart_index(), 1U); + EXPECT_TRUE( + sequence.current_desired_target_px().isApprox(Eigen::Vector2d(102.0, 199.0), 1e-9)); + + EXPECT_TRUE(sequence.advance_after_fire()); + EXPECT_TRUE(sequence.advance_after_fire()); + EXPECT_FALSE(sequence.advance_after_fire()); + EXPECT_EQ(sequence.current_dart_index(), 3U); + EXPECT_TRUE( + sequence.current_desired_target_px().isApprox(Eigen::Vector2d(106.0, 197.0), 1e-9)); + + sequence.reset(); + EXPECT_EQ(sequence.current_dart_index(), 0U); + EXPECT_TRUE( + sequence.current_desired_target_px().isApprox(Eigen::Vector2d(100.0, 200.0), 1e-9)); +} + +} // namespace rmcs_dart_guidance::manager::test diff --git a/tests/launch_preparation_task_test.cpp b/tests/launch_preparation_task_test.cpp new file mode 100644 index 0000000..bbb18b2 --- /dev/null +++ b/tests/launch_preparation_task_test.cpp @@ -0,0 +1,80 @@ +#include "manager/task/launch_preparation_task.hpp" + +#include + +#include + +namespace rmcs_dart_guidance::manager::test { + +class LaunchPreparationTaskTest : public ::testing::Test { +protected: + struct Harness { + explicit Harness(LaunchPreparationTask::Mode mode) + : task( + belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, + belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, + left_belt_torque, right_belt_torque, trigger_lock_enable, lifting_command, + lifting_left_vel_fb, lifting_right_vel_fb, 1.0, 1, 0, 2000, mode) {} + + rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; + double belt_target_velocity{0.0}; + double belt_torque_limit{0.0}; + double belt_hold_torque{0.0}; + bool belt_wait_zero_velocity{false}; + double left_belt_velocity{0.0}; + double right_belt_velocity{0.0}; + double left_belt_torque{0.0}; + double right_belt_torque{0.0}; + bool trigger_lock_enable{false}; + + rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; + double lifting_left_vel_fb{0.0}; + double lifting_right_vel_fb{0.0}; + + LaunchPreparationTask task; + }; +}; + +TEST_F(LaunchPreparationTaskTest, FirstFillStartsWithLiftReset) { + Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); + harness.lifting_left_vel_fb = 2.0; + harness.lifting_right_vel_fb = 2.0; + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 0.0); + EXPECT_FALSE(harness.trigger_lock_enable); +} + +TEST_F(LaunchPreparationTaskTest, FirstFillMovesBeltDownAfterLiftResetCompletes) { + Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); + harness.lifting_left_vel_fb = 2.0; + harness.lifting_right_vel_fb = 2.0; + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); + + harness.lifting_left_vel_fb = 0.0; + harness.lifting_right_vel_fb = 0.0; + EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + + EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); + EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); +} + +TEST_F(LaunchPreparationTaskTest, NormalModeStartsWithBeltMove) { + Harness harness(LaunchPreparationTask::Mode::NORMAL); + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); + EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); + EXPECT_FALSE(harness.trigger_lock_enable); +} + +} // namespace rmcs_dart_guidance::manager::test diff --git a/tests/vision_assisted_launch_preparation_task_test.cpp b/tests/vision_assisted_launch_preparation_task_test.cpp new file mode 100644 index 0000000..0d2506b --- /dev/null +++ b/tests/vision_assisted_launch_preparation_task_test.cpp @@ -0,0 +1,125 @@ +#include "manager/task/vision_assisted_launch_preparation_task.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rmcs_dart_guidance::manager::test { + +class VisionAssistedLaunchPreparationTaskTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + if (!rclcpp::ok()) { + ::setenv("ROS_LOG_DIR", "/tmp/rmcs_dart_guidance_test_logs", 1); + int argc = 0; + char** argv = nullptr; + rclcpp::init(argc, argv); + } + } + + static void TearDownTestSuite() { + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } + + static AutoAimParams make_auto_aim_params() { + AutoAimParams params; + params.desired_target_px = Eigen::Vector2d(100.0, 100.0); + params.deadband_px = Eigen::Vector2d::Constant(2.0); + params.ready_exit_deadband_px = Eigen::Vector2d::Constant(3.0); + params.accept_deadband_px = Eigen::Vector2d::Constant(6.0); + params.ready_confirm_ticks = 1; + params.timeout_ticks = 2; + params.yaw_gain = 0.1; + params.pitch_gain = 0.1; + params.max_transform_rate = 10.0; + return params; + } + + struct Harness { + explicit Harness(LaunchPreparationTask::Mode mode) + : task( + belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, + belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, + left_belt_torque, right_belt_torque, trigger_lock_enable, lifting_command, + lifting_left_vel_fb, lifting_right_vel_fb, 1.0, 1, 0, 2000, mode, + yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, + target_position, target_tracking, + rclcpp::get_logger("vision_assisted_launch_preparation_task_test"), + make_auto_aim_params()) {} + + rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; + double belt_target_velocity{0.0}; + double belt_torque_limit{0.0}; + double belt_hold_torque{0.0}; + bool belt_wait_zero_velocity{false}; + double left_belt_velocity{0.0}; + double right_belt_velocity{0.0}; + double left_belt_torque{0.0}; + double right_belt_torque{0.0}; + bool trigger_lock_enable{false}; + + rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; + double lifting_left_vel_fb{0.0}; + double lifting_right_vel_fb{0.0}; + + Eigen::Vector2d yaw_pitch_control_velocity{Eigen::Vector2d::Zero()}; + bool aim_ready{false}; + Eigen::Vector2d aim_error_px{Eigen::Vector2d::Zero()}; + Eigen::Vector2d desired_target_px_output{Eigen::Vector2d::Zero()}; + cv::Point2i target_position{-1, -1}; + bool target_tracking{false}; + + VisionAssistedLaunchPreparationTask task; + }; +}; + +TEST_F(VisionAssistedLaunchPreparationTaskTest, StartsWithAutoAimBeforeMechanicalSequence) { + Harness harness(LaunchPreparationTask::Mode::NORMAL); + harness.target_tracking = true; + harness.target_position = cv::Point2i(90, 100); + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); + EXPECT_GT(harness.yaw_pitch_control_velocity.x(), 0.0); + EXPECT_FALSE(harness.aim_ready); +} + +TEST_F(VisionAssistedLaunchPreparationTaskTest, TimeoutContinuesIntoMechanicalSequence) { + Harness harness(LaunchPreparationTask::Mode::NORMAL); + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + + EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + + EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); + EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); +} + +TEST_F(VisionAssistedLaunchPreparationTaskTest, FirstFillModePreservesLiftResetAfterAim) { + Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); + harness.target_tracking = true; + harness.target_position = cv::Point2i(100, 100); + + EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); + EXPECT_TRUE(harness.aim_ready); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + + harness.lifting_left_vel_fb = 2.0; + harness.lifting_right_vel_fb = 2.0; + EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); + EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); + EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); +} + +} // namespace rmcs_dart_guidance::manager::test From a99b0187ea56e5eaedd3d4248d3986c58dec4224 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Tue, 31 Mar 2026 16:53:39 +0800 Subject: [PATCH 03/19] Merge position control from backup branch - Added belt angle input interfaces for position feedback - Added belt_zero_calibration output for zero point calibration - Added belt_down_distance and belt_pulley_radius parameters - Added BeltConstantVelocityMoveAction for position-based control - Added ZeroCalibrationAction for zero point recording - Updated LaunchPreparationTask to use position control instead of stall detection - Updated CancelLaunchTask with position control - Updated SliderInitTask to trigger zero calibration after homing Key features preserved: - Downward movement uses position feedback to determine arrival - Distance (m) to angle (rad) conversion using pulley radius - Zero point calibration on each upward stall (mechanical limit) --- .../belt_constant_velocity_move_action.hpp | 204 +++++++++++++++ .../action/belt_hold_torque_action.hpp | 55 ++++ src/manager/action/belt_zero_calibration.hpp | 31 +++ src/manager/dart_manager.cpp | 149 +++++++---- src/manager/task/cancel_launch_task.hpp | 161 +++++++----- src/manager/task/launch_preparation_task.hpp | 247 +++++++++--------- src/manager/task/silder_init_task.hpp | 66 +++-- 7 files changed, 649 insertions(+), 264 deletions(-) create mode 100644 src/manager/action/belt_constant_velocity_move_action.hpp create mode 100644 src/manager/action/belt_hold_torque_action.hpp create mode 100644 src/manager/action/belt_zero_calibration.hpp diff --git a/src/manager/action/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp new file mode 100644 index 0000000..5b2486c --- /dev/null +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -0,0 +1,204 @@ +#pragma once + +#include "action.hpp" + +#include +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +// BeltConstantVelocityMoveAction - 匀速运动到目标位置 +// 不使用PID位置控制，直接使用速度控制模式 +// 完成条件：到达目标位置（基于多圈角度反馈） +// 优点：避免PID error过大导致的问题，运动更平滑 +// +// 重要：目标位置基于初始化后的实际角度计算，而非概念上的"零点" +// - 输入参数是距离（m），在函数内换算为角度（rad） +// - 目标位置 = 初始角度 + (目标距离 / 滑轮半径) +class BeltConstantVelocityMoveAction : public IAction { +public: + BeltConstantVelocityMoveAction( + std::string name, + rmcs_msgs::DartSliderStatus& belt_command, + double& belt_target_velocity, + double& belt_torque_limit, + const double& left_belt_angle, + const double& right_belt_angle, + const double& left_belt_velocity, + const double& right_belt_velocity, + double target_distance, // 目标距离（m，正=上行，负=下行） + double pulley_radius, // 滑轮半径（m） + double velocity, // 运动速度（rad/s） + double torque_limit, // 扭矩限制（N⋅m） + uint64_t timeout_ticks, + uint64_t min_running_ticks = 50, + double position_tolerance = 0.01, // 位置到达容差（rad） + double stall_velocity_threshold = 0.3, + uint64_t stall_confirm_ticks = 200, + double max_down_distance = 0.20) // 下行最大距离限制（m，正值，防止过度下行） + : IAction(std::move(name)) + , belt_command_(belt_command) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_limit_(belt_torque_limit) + , left_belt_angle_(left_belt_angle) + , right_belt_angle_(right_belt_angle) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , target_distance_(target_distance) + , pulley_radius_(pulley_radius) + , velocity_(velocity) + , torque_limit_(torque_limit) + , timeout_ticks_(timeout_ticks) + , min_running_ticks_(min_running_ticks) + , position_tolerance_(position_tolerance) + , stall_velocity_threshold_(stall_velocity_threshold) + , stall_confirm_ticks_(stall_confirm_ticks) + , max_down_distance_(max_down_distance) {} + + void on_enter() override { + stall_counter_ = 0; + + // 读取初始角度（左传送带，右传送带使用同步控制） + initial_angle_ = left_belt_angle_; + + // 将目标距离（m）换算为角度偏移（rad） + // angle_offset = distance / radius + double target_angle_offset = target_distance_ / pulley_radius_; + + // 计算实际目标位置 = 初始角度 + 角度偏移 + target_position_ = initial_angle_ + target_angle_offset; + + // 下行最大距离限制检查（防止过度下行） + // 将最大下行距离（m）换算为角度偏移（rad） + double max_down_angle_offset = -max_down_distance_ / pulley_radius_; // 负值表示下行 + double max_down_position = initial_angle_ + max_down_angle_offset; + + if (target_position_ < max_down_position) { + printf("[%s] ERROR: target_position=%.4f exceeds max_down_position=%.4f\n", + name().c_str(), target_position_, max_down_position); + printf("[%s] target_distance=%.4f m, max_down_distance=%.4f m\n", + name().c_str(), target_distance_, max_down_distance_); + printf("[%s] Limiting target to max_down_position=%.4f\n", + name().c_str(), max_down_position); + target_position_ = max_down_position; + } + + // 根据目标位置自动确定运动方向 + // 实际测量：角度增大=下行，角度减小=上行 + double distance_to_target = target_position_ - initial_angle_; + if (distance_to_target > 0) { + // 目标位置 > 初始位置，角度增大方向 = 下行 + belt_command_ = rmcs_msgs::DartSliderStatus::DOWN; + belt_target_velocity_ = std::abs(velocity_); + } else { + // 目标位置 < 初始位置，角度减小方向 = 上行 + belt_command_ = rmcs_msgs::DartSliderStatus::UP; + belt_target_velocity_ = std::abs(velocity_); + } + + belt_torque_limit_ = torque_limit_; + + printf("[%s] on_enter: initial_angle=%.4f, target_distance=%.4f m, target_angle_offset=%.4f rad\n", + name().c_str(), initial_angle_, target_distance_, target_angle_offset); + printf("[%s] target_position=%.4f, velocity=%.4f, direction=%s\n", + name().c_str(), target_position_, belt_target_velocity_, + (distance_to_target > 0) ? "DOWN" : "UP"); + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + printf("[%s] TIMEOUT after %lu ticks\n", name().c_str(), elapsed_ticks()); + return ActionStatus::FAILURE; + } + + double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; + double position_error = target_position_ - avg_angle; // 保留符号，用于判断方向 + double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + + // 每100帧打印一次调试信息 + if (elapsed_ticks() % 100 == 0) { + printf("[%s] tick=%lu, current=%.4f, target=%.4f, error=%.4f, vel=%.4f\n", + name().c_str(), elapsed_ticks(), avg_angle, target_position_, + position_error, avg_velocity); + printf("[%s] left_angle=%.4f, right_angle=%.4f, initial=%.4f\n", + name().c_str(), left_belt_angle_, right_belt_angle_, initial_angle_); + } + + // 位置到达判断（优先级最高） + // 判断条件： + // 1. 在容差范围内：|error| < tolerance + // 2. 或者已经越过目标（考虑运动方向）： + // - 下行（target > initial）：error < 0 表示已经越过（current > target） + // - 上行（target < initial）：error > 0 表示已经越过（current < target） + if (elapsed_ticks() > min_running_ticks_) { + bool within_tolerance = std::abs(position_error) < position_tolerance_; + bool overshot = false; + + if (target_distance_ > 0) { + // 下行：目标位置 > 初始位置，期望 current 增大 + // 如果 error < 0，说明 current > target，已经越过 + overshot = (position_error < 0); + } else { + // 上行：目标位置 < 初始位置，期望 current 减小 + // 如果 error > 0，说明 current < target，已经越过 + overshot = (position_error > 0); + } + + if (within_tolerance || overshot) { + printf("[%s] SUCCESS: position reached (error=%.4f, tolerance=%.4f, overshot=%d)\n", + name().c_str(), position_error, position_tolerance_, overshot); + return ActionStatus::SUCCESS; + } + } + + // 堵转检测 + if (elapsed_ticks() > min_running_ticks_) { + if (avg_velocity < stall_velocity_threshold_) { + ++stall_counter_; + if (stall_counter_ >= stall_confirm_ticks_) { + printf("[%s] FAILURE: stalled (vel=%.4f < threshold=%.4f for %lu ticks)\n", + name().c_str(), avg_velocity, stall_velocity_threshold_, stall_counter_); + return ActionStatus::FAILURE; + } + } else { + stall_counter_ = 0; + } + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + // 不在这里停止运动，让下一个 action 接管控制 + // 这样可以避免速度突变 + } + +private: + rmcs_msgs::DartSliderStatus& belt_command_; + double& belt_target_velocity_; + double& belt_torque_limit_; + const double& left_belt_angle_; + const double& right_belt_angle_; + const double& left_belt_velocity_; + const double& right_belt_velocity_; + + double target_distance_; // 目标距离（m） + double pulley_radius_; // 滑轮半径（m） + double velocity_; // 运动速度（rad/s） + double torque_limit_; // 扭矩限制（N⋅m） + uint64_t timeout_ticks_; + uint64_t min_running_ticks_; + double position_tolerance_; // 位置到达容差（rad） + double stall_velocity_threshold_; + uint64_t stall_confirm_ticks_; + double max_down_distance_; // 下行最大距离限制（m，正值） + + double initial_angle_{0.0}; // 初始角度（rad，在on_enter中读取） + double target_position_{0.0}; // 实际目标位置（rad，在on_enter中计算） + uint64_t stall_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp new file mode 100644 index 0000000..5be9c37 --- /dev/null +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "action.hpp" + +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +// BeltHoldTorqueAction - 保持传送带张力 +// 持续输出 WAIT 命令 + hold_torque，用于在扳机锁定期间保持传送带张力 +class BeltHoldTorqueAction : public IAction { +public: + BeltHoldTorqueAction( + std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_hold_torque, bool& belt_wait_zero_velocity, double hold_torque_value, + uint64_t hold_duration_ticks) + : IAction(std::move(name)) + , belt_command_(belt_command) + , belt_target_velocity_(belt_target_velocity) + , belt_hold_torque_(belt_hold_torque) + , belt_wait_zero_velocity_(belt_wait_zero_velocity) + , hold_torque_value_(hold_torque_value) + , hold_duration_ticks_(hold_duration_ticks) {} + + void on_enter() override { + // 立即设置WAIT命令和保持扭矩，确保无缝过渡 + belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; + belt_target_velocity_ = 0.0; + belt_hold_torque_ = hold_torque_value_; + belt_wait_zero_velocity_ = false; // 使用 HOLD_TORQUE 模式 + } + + ActionStatus update() override { + if (elapsed_ticks() >= hold_duration_ticks_) { + return ActionStatus::SUCCESS; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override {} + +private: + rmcs_msgs::DartSliderStatus& belt_command_; + double& belt_target_velocity_; + double& belt_hold_torque_; + bool& belt_wait_zero_velocity_; + double hold_torque_value_; + uint64_t hold_duration_ticks_; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_zero_calibration.hpp b/src/manager/action/belt_zero_calibration.hpp new file mode 100644 index 0000000..3b0d364 --- /dev/null +++ b/src/manager/action/belt_zero_calibration.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "action.hpp" + +namespace rmcs_dart_guidance::manager { + +class ZeroCalibrationAction : public IAction { +public: + explicit ZeroCalibrationAction(bool& zero_calibration_flag, uint64_t hold_ticks = 10) + : IAction("zero_calibration") + , zero_calibration_flag_(zero_calibration_flag) + , hold_ticks_(hold_ticks) {} + + void on_enter() override { + zero_calibration_flag_ = true; + tick_count_ = 0; + } + + ActionStatus update() override { + ++tick_count_; + return (tick_count_ >= hold_ticks_) ? ActionStatus::SUCCESS : ActionStatus::RUNNING; + } + + void on_exit() override { zero_calibration_flag_ = false; } + +private: + bool& zero_calibration_flag_; + uint64_t hold_ticks_; + uint64_t tick_count_{0}; +}; +} // namespace rmcs_dart_guidance::manager \ No newline at end of file diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index e9c93eb..331a5e9 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -56,6 +56,8 @@ class DartManagerV2 register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); register_input("/dart/drive_belt/left/torque", left_belt_torque_); register_input("/dart/drive_belt/right/torque", right_belt_torque_); + register_input("/dart/drive_belt/left/angle", left_belt_angle_); + register_input("/dart/drive_belt/right/angle", right_belt_angle_); register_input("/dart/lifting_left/velocity", lifting_left_vel_fb_); register_input("/dart/lifting_right/velocity", lifting_right_vel_fb_); @@ -72,6 +74,7 @@ class DartManagerV2 register_output("/dart/manager/belt/torque_limit", belt_torque_limit_, 0.0); register_output("/dart/manager/belt/hold_torque", belt_hold_torque_, 0.0); register_output("/dart/manager/belt/wait_zero_velocity", belt_wait_zero_velocity_, false); + register_output("/dart/manager/belt/zero_calibration", belt_zero_calibration_, false); register_output("/dart/manager/trigger/lock_enable", trigger_lock_enable_, false); register_output("/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); @@ -114,6 +117,57 @@ class DartManagerV2 limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); + belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m + belt_pulley_radius_ = get_parameter("belt_pulley_radius").as_double(); // m + + // 传送带速度和扭矩限制参数（集中管理） + try { + belt_prepare_down_velocity_first_ = + get_parameter("belt_prepare_down_velocity_first").as_double(); + } catch (...) { + belt_prepare_down_velocity_first_ = 5.0; + } + try { + belt_prepare_down_velocity_ = get_parameter("belt_prepare_down_velocity").as_double(); + } catch (...) { + belt_prepare_down_velocity_ = 10.0; + } + try { + belt_prepare_torque_limit_ = get_parameter("belt_prepare_torque_limit").as_double(); + } catch (...) { + belt_prepare_torque_limit_ = 5.0; + } + try { + belt_prepare_down_ramp_ticks_ = + (uint64_t)get_parameter("belt_prepare_down_ramp_ticks").as_int(); + } catch (...) { + belt_prepare_down_ramp_ticks_ = 400; + } + try { + belt_prepare_down_hold_torque_ = + get_parameter("belt_prepare_down_hold_torque").as_double(); + } catch (...) { + belt_prepare_down_hold_torque_ = 5.0; + } + try { + belt_prepare_down_zero_velocity_threshold_ = + get_parameter("belt_prepare_down_zero_velocity_threshold").as_double(); + } catch (...) { + belt_prepare_down_zero_velocity_threshold_ = 0.15; + } + try { + belt_prepare_down_zero_confirm_ticks_ = + (uint64_t)get_parameter("belt_prepare_down_zero_confirm_ticks").as_int(); + } catch (...) { + belt_prepare_down_zero_confirm_ticks_ = 60; + } + try { + belt_prepare_down_ramp_timeout_ticks_ = + (uint64_t)get_parameter("belt_prepare_down_ramp_timeout_ticks").as_int(); + } catch (...) { + belt_prepare_down_ramp_timeout_ticks_ = 2000; + } + lifting_stall_threshold_ = get_parameter("lifting_stall_threshold").as_double(); lifting_stall_confirm_ticks_ = (uint64_t)get_parameter("lifting_stall_confirm_ticks").as_int(); @@ -476,65 +530,54 @@ class DartManagerV2 *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_); + *left_belt_torque_, *right_belt_torque_, + *belt_zero_calibration_); } // 任务工厂 std::shared_ptr make_task(const std::string& cmd) { if (cmd == "launch_prepare" || cmd == "launch-prepare") { - auto launch_mode = first_fill_pending_ - ? LaunchPreparationTask::Mode::FIRST_FILL - : LaunchPreparationTask::Mode::NORMAL; - first_fill_pending_ = false; - - if (launch_prepare_enable_visual_assist_) { - return std::make_shared( - *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, - *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, - lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, launch_mode, - auto_aim_feedback_.yaw_pitch_control_velocity(), - auto_aim_feedback_.aim_ready(), auto_aim_feedback_.aim_error_px(), - auto_aim_feedback_.desired_target_px(), current_target_position_, - current_target_tracking_, logger_, - AutoAimParams{ - .desired_target_px = current_desired_target_px(), - .deadband_px = aim_deadband_px_, - .ready_exit_deadband_px = aim_ready_exit_deadband_px_, - .accept_deadband_px = aim_accept_deadband_px_, - .yaw_gain = aim_yaw_gain_, - .pitch_gain = aim_pitch_gain_, - .ready_confirm_ticks = aim_ready_confirm_ticks_, - .timeout_ticks = aim_timeout_ticks_, - .min_transform_rate = aim_min_transform_rate_, - .max_transform_rate = auto_aim_max_transform_rate_, - }); + // 根据当前 fire_count 选择下降速度（fire_count=0 表示第一次准备） + double down_velocity = (fire_count_ == 0) ? belt_prepare_down_velocity_first_ + : belt_prepare_down_velocity_; + bool require_lifting_down = (fire_count_ > 0); + + // 打印角度反馈状态 + RCLCPP_INFO(logger_, "[DartManager] Creating launch_prepare task, fire_count=%u", fire_count_); + if (left_belt_angle_.ready() && right_belt_angle_.ready()) { + RCLCPP_INFO(logger_, "[DartManager] Belt angles: left=%.4f, right=%.4f", + *left_belt_angle_, *right_belt_angle_); + } else { + RCLCPP_WARN(logger_, "[DartManager] Belt angle feedback NOT READY!"); } + RCLCPP_INFO(logger_, "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m, down_velocity=%.2f rad/s", + belt_down_distance_, belt_pulley_radius_, down_velocity); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, - *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, - lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, launch_mode); + *belt_wait_zero_velocity_, *left_belt_angle_, *right_belt_angle_, + *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, + *right_belt_torque_, *trigger_lock_enable_, belt_down_distance_, + belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, + belt_prepare_down_ramp_ticks_, belt_prepare_down_hold_torque_, + belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, + belt_prepare_down_ramp_timeout_ticks_, require_lifting_down, *lifting_command_, + *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_); } if (cmd == "unload" || cmd == "cancel_launch") { - first_fill_pending_ = true; - + // 取消发射使用与准备发射相同的下行参数 + double down_velocity = belt_prepare_down_velocity_; return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, - *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, - *trigger_lock_enable_, - *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, - lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_); + *belt_wait_zero_velocity_, *left_belt_angle_, *right_belt_angle_, + *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, + *right_belt_torque_, *trigger_lock_enable_, *lifting_command_, + *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, + belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, + belt_prepare_down_ramp_ticks_, belt_prepare_down_hold_torque_, + belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, + belt_prepare_down_ramp_timeout_ticks_, *belt_zero_calibration_); } if (cmd == "fire") { @@ -574,6 +617,8 @@ class DartManagerV2 InputInterface right_belt_velocity_; InputInterface left_belt_torque_; InputInterface right_belt_torque_; + InputInterface left_belt_angle_; + InputInterface right_belt_angle_; InputInterface joystick_left_; InputInterface joystick_right_; @@ -589,6 +634,7 @@ class DartManagerV2 OutputInterface belt_torque_limit_; OutputInterface belt_hold_torque_; OutputInterface belt_wait_zero_velocity_; + OutputInterface belt_zero_calibration_; OutputInterface trigger_lock_enable_; OutputInterface yaw_pitch_control_velocity_; @@ -606,6 +652,18 @@ class DartManagerV2 double auto_aim_max_transform_rate_{500.0}; uint64_t limiting_fill_ticks_{500}; + double belt_down_distance_{0.0}; // m + double belt_pulley_radius_{0.0}; // m + + double belt_prepare_down_velocity_first_{5.0}; // rad/s + double belt_prepare_down_velocity_{10.0}; // rad/s + double belt_prepare_torque_limit_{5.0}; // N⋅m + uint64_t belt_prepare_down_ramp_ticks_{400}; // ticks + double belt_prepare_down_hold_torque_{5.0}; // N⋅m + double belt_prepare_down_zero_velocity_threshold_{0.15}; // rad/s + uint64_t belt_prepare_down_zero_confirm_ticks_{60}; // ticks + uint64_t belt_prepare_down_ramp_timeout_ticks_{2000}; // ticks + double lifting_stall_threshold_{0.5}; uint64_t lifting_stall_confirm_ticks_{100}; uint64_t lifting_stall_min_run_ticks_{500}; @@ -635,6 +693,7 @@ class DartManagerV2 std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; }; diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index 45bbf5d..a9f0151 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -1,11 +1,15 @@ #pragma once +#include "manager/action/belt_constant_velocity_move_action.hpp" #include "manager/action/belt_move_action.hpp" +#include "manager/action/belt_velocity_ramp_action.hpp" +#include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" +#include #include #include @@ -14,8 +18,8 @@ namespace rmcs_dart_guidance::manager { // CancelLaunchTask — 取消当前发射流程并回到安全待机位： -// 1. 同步带下行到卸载位 -// 2. 短暂等待机构稳定 +// 1. 同步带匀速下行到卸载位 +// 2. 斜坡减速到零 // 3. 解锁扳机 // 4. 同步带上行复位 // 5. 填装升降上行回到初始位 @@ -24,90 +28,109 @@ class CancelLaunchTask : public Task { CancelLaunchTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, double lifting_stall_threshold, - uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, - uint64_t lifting_stall_timeout_ticks) + const double& lifting_right_vel_fb, double belt_down_distance, double belt_pulley_radius, + double down_velocity, double torque_limit, uint64_t down_ramp_ticks, + double down_hold_torque, double down_zero_velocity_threshold, + uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, + bool& belt_zero_calibration) : Task("cancel_launch", "取消发射") { - // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double down_velocity = 10.0; - double up_velocity = 10.0; - double hold_torque = 1.0; // Wait 时的保持力矩 - + // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） then( - std::make_shared( - "belt_move_down", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 - down_velocity, // 设定速度 - 50.0, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 20000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE)); + std::make_shared( + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 扭矩限幅（输出） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance, // 目标距离（m，正值=下行） + belt_pulley_radius, // 滑轮半径（m） + down_velocity, // 运动速度（rad/s） + torque_limit, // 扭矩限制（N⋅m） + 10000, // 超时帧数 + 100, // 最小运行帧数 + 0.5, // 位置到达容差（rad）- 增大容差 + 0.3, // 堵转速度阈值（rad/s） + 200, // 堵转确认帧数 + 0.20)); // 下行最大距离限制（m，正值） + // 步骤2：下行速度斜坡减速到 0，成功后自动进入 HOLD_TORQUE 保持张力 + const double ramp_step_per_tick = + down_ramp_ticks > 0 ? (std::abs(down_velocity) / static_cast(down_ramp_ticks)) + : std::abs(down_velocity); then( - std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 - )); + std::make_shared( + "belt_down_ramp_to_zero", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 力矩限幅（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT 模式选择（输出） + left_belt_velocity, // 左皮带速度反馈（输入） + right_belt_velocity, // 右皮带速度反馈（输入） + rmcs_msgs::DartSliderStatus::DOWN, // 下行方向 + down_velocity, // 斜坡初速度 + ramp_step_per_tick, // 每 tick 减速步长 + torque_limit, // 力矩限制 + down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 + down_zero_velocity_threshold, // 实测零速阈值 + down_zero_confirm_ticks, // 零速确认帧数 + down_ramp_timeout_ticks)); // 斜坡阶段超时 + // 步骤3：解锁扳机 then( std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000 // 等待释放完成帧数 - )); + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 1000)); // 等待释放完成帧数 + // 步骤4：同步带上行复位到机械限位 then( std::make_shared( - "belt_reset", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - up_velocity, // 设定速度 - 25.0, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 20000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); + "belt_reset", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输入） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 10, // 设定速度 + 1.0, // 设定力矩限制 + 0.5, // 设定保持力矩 + 10000, // 超时帧数 + 1.0, // 堵转速度阈值 + 0.5, // 堵转力矩阈值 + 100, // 堵转确认帧数 + 50, // 最短运行帧数 + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); // 退出模式 + // 步骤5：填装升降上行回到初始位 then( std::make_shared( - "filling_lift_up", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 - )); + "filling_lift_up", // 动作名称 + lifting_command, // 升降指令（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + 0.1, // 堵转速度阈值 + 100, // 堵转确认帧数 + 500, // 最短运行帧数 + 2000)); // 超时帧数 + + then(std::make_shared("stabilize_wait", 50)); + + then(std::make_shared(belt_zero_calibration)); } }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 88c2307..ccf955f 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -1,12 +1,16 @@ #pragma once #include "manager/action/action_set.hpp" +#include "manager/action/belt_constant_velocity_move_action.hpp" #include "manager/action/belt_move_action.hpp" +#include "manager/action/belt_velocity_ramp_action.hpp" +#include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" +#include #include #include @@ -14,137 +18,138 @@ namespace rmcs_dart_guidance::manager { -// LaunchPreparationTask — 纯机械发射准备任务。 -// NORMAL: -// 1. 同步带下行到装填位 -// 2. 短暂等待机构稳定 -// 3. 扳机锁定与填装升降下行并行执行 -// 4. 同步带上行复位 -// FIRST_FILL: -// 1. 填装升降上行 -// 2. 同步带下行到装填位 -// 3. 短暂等待机构稳定 -// 4. 扳机锁定 -// 5. 同步带上行复位 +// LaunchPreparationTask — 每次发射前的准备动作（传送带下行+扳机锁定） +// 速度和扭矩限制由 manager 传入，不再硬编码 class LaunchPreparationTask : public Task { public: - enum class Mode { - NORMAL, - FIRST_FILL, - }; - LaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, + double belt_down_distance, double belt_pulley_radius, double down_velocity, + double torque_limit, uint64_t down_ramp_ticks, double down_hold_torque, + double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, + uint64_t down_ramp_timeout_ticks, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, double lifting_stall_threshold, - uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, - uint64_t lifting_stall_timeout_ticks, Mode mode = Mode::NORMAL) - : Task("launch_preparation", "滑块发射准备") { - - // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double torque_limit = 50.0; - double hold_torque = 5.0; // Wait 时的保持力矩 - - auto make_belt_move_down = [&]() { - return std::make_shared( - "belt_move_down", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 - 10.0, // 设定速度 - torque_limit, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 20000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE); - }; - - auto make_belt_wait = [&]() { - return std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 - ); - }; - - auto make_trigger_lock = [&]() { - return std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 1000 // 等待锁定完成帧数 - ); - }; - - auto make_filling_lift = [&](const char* name, rmcs_msgs::DartSliderStatus command, - bool require_motion_before_stall = true) { - return std::make_shared( - name, // 动作名称 - lifting_command, // 升降指令（输出） - command, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks, // 超时帧数 - require_motion_before_stall); - }; - - auto make_belt_reset = [&]() { - return std::make_shared( - "belt_reset", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 10.0, // 设定速度 - torque_limit, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 20000, // 超时帧数 - 0.5, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 200, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY); - }; - - if (mode == Mode::FIRST_FILL) { - then(make_filling_lift("filling_lift_up", rmcs_msgs::DartSliderStatus::UP, false)); - then(make_belt_move_down()); - then(make_belt_wait()); - then(make_trigger_lock()); - then(make_belt_reset()); - return; + const double& lifting_right_vel_fb, bool& belt_zero_calibration) + : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { + + // 距离转角度：distance (m) = angle (rad) × radius (m) + // angle (rad) = distance (m) / radius (m) + // 约定：根据实际测量，下行=角度增大=正方向，上行=角度减小=负方向 + // 目标位置基于初始化后的实际角度计算，而非概念上的"零点" + + printf( + "[LaunchPreparationTask] belt_down_distance=%.4f m, belt_pulley_radius=%.4f m\n", + belt_down_distance, belt_pulley_radius); + + // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） + // 注意：target_distance为正值表示下行（角度增大方向） + then( + std::make_shared( + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 扭矩限幅（输出） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance, // 目标距离（m，正值=下行） + belt_pulley_radius, // 滑轮半径（m） + down_velocity, // 运动速度（rad/s） + torque_limit, // 扭矩限制（N⋅m） + 10000, // 超时帧数 + 100, // 最小运行帧数 + 0.5, // 位置到达容差（rad）- 增大容差 + 0.3, // 堵转速度阈值（rad/s） + 200, // 堵转确认帧数 + 0.20)); // 下行最大距离限制（m，正值） + + // 步骤2：下行速度斜坡减速到 0，成功后自动进入 HOLD_TORQUE 保持张力 + const double ramp_step_per_tick = + down_ramp_ticks > 0 ? (std::abs(down_velocity) / static_cast(down_ramp_ticks)) + : std::abs(down_velocity); + then( + std::make_shared( + "belt_down_ramp_to_zero", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 力矩限幅（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT 模式选择（输出） + left_belt_velocity, // 左皮带速度反馈（输入） + right_belt_velocity, // 右皮带速度反馈（输入） + rmcs_msgs::DartSliderStatus::DOWN, // 下行方向 + down_velocity, // 斜坡初速度 + ramp_step_per_tick, // 每 tick 减速步长 + torque_limit, // 力矩限制 + down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 + down_zero_velocity_threshold, // 实测零速阈值 + down_zero_confirm_ticks, // 零速确认帧数 + down_ramp_timeout_ticks)); // 斜坡阶段超时 + + if (require_lifting_down) { + // 步骤3（2-4发）：升降平台下行与扳机锁定并行 + auto parallel_lock_and_lift_down = + std::make_shared("lock_and_lift_down", ActionSet::Policy::ALL_SUCCESS); + + parallel_lock_and_lift_down + ->also( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 1000)) // 等待锁定完成帧数 + .also( + std::make_shared( + "filling_lift_down", // 动作名称 + lifting_command, // 升降指令（输出） + rmcs_msgs::DartSliderStatus::DOWN, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + 0.1, // 堵转速度阈值（rad/s） + 100, // 堵转确认帧数 + 500, // 最短运行帧数 + 2000)); // 超时帧数 + then(parallel_lock_and_lift_down); + } else { + then( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 1000)); // 等待锁定完成帧数 } - then(make_belt_move_down()); - then(make_belt_wait()); - - auto parallel_prepare = - std::make_shared("parallel_prepare", ActionSet::Policy::ALL_SUCCESS); - parallel_prepare->also(make_trigger_lock()) - .also(make_filling_lift("filling_lift_down", rmcs_msgs::DartSliderStatus::DOWN)); - then(parallel_prepare); - - then(make_belt_reset()); + // 步骤4：传送带上行到机械限位（速度控制 + 堵转检测） + then( + std::make_shared( + "belt_reset_up", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 15, // 设定速度（rad/s） + 2.5, // 设定力矩限制（N⋅m） + 0.5, // 设定保持力矩（N⋅m） + 5000, // 超时帧数 + 0.5, // 堵转速度阈值（rad/s） + 0.8, // 堵转力矩阈值（N⋅m） + 100, // 堵转确认帧数 + 50, // 最短运行帧数 + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 + true)); // 超时也返回 SUCCESS + + then(std::make_shared("stabilize_wait", 50)); + + then(std::make_shared(belt_zero_calibration)); } }; diff --git a/src/manager/task/silder_init_task.hpp b/src/manager/task/silder_init_task.hpp index ed33192..3788189 100644 --- a/src/manager/task/silder_init_task.hpp +++ b/src/manager/task/silder_init_task.hpp @@ -1,52 +1,60 @@ #pragma once #include "manager/action/belt_move_action.hpp" +#include "manager/action/belt_zero_calibration.hpp" +#include "manager/action/delay_action.hpp" #include "manager/task/task.hpp" - #include #include namespace rmcs_dart_guidance::manager { -// SliderInitTask — 上电/恢复时执行一次同步带上行复位： -// 1. 同步带上行到复位位 +// SliderInitTask — 上电/恢复时执行一次同步带上行复位并校准零点： +// 1. 同步带上行到机械限位（堵转检测） +// 2. 短暂延时稳定 +// 3. 发送零点校准命令 class SliderInitTask : public Task { public: SliderInitTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque) - : Task("slider_init", "传送带上行复位") { - // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double velocity = 15.0; - double torque_limit = 1.0; - double hold_torque = 1.0; // Wait 时的保持力矩 + const double& left_belt_torque, const double& right_belt_torque, + bool& belt_zero_calibration) + : Task("slider_init", "传送带上行复位并校准零点") { + // 步骤1：同步带上行到机械限位（堵转检测） then( std::make_shared( - "belt_reset", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - velocity, // 设定速度 - torque_limit, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 10000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); + "belt_reset", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 10, // 设定速度 + 1.0, // 设定力矩限制 + 0.5, // 设定保持力矩 + 5000, // 超时帧数（可配置） + 1.0, // 堵转速度阈值（rad/s） + 0.5, // 堵转力矩阈值（N⋅m） + 100, // 堵转确认帧数 + 50, // 最短运行帧数 + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 + true)); // 超时也返回 SUCCESS（测试环境无接驳件） + + then(std::make_shared("stabilize_wait", 50)); + + then(std::make_shared(belt_zero_calibration)); } + +private: }; } // namespace rmcs_dart_guidance::manager From 35de9a70b0b6a2e4c00aae2b371681d972f18207 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Tue, 31 Mar 2026 22:09:45 +0800 Subject: [PATCH 04/19] Fix manager layer initialization and task flow - Added fire_count increment after fire task completion - Added fire_count reset after cancel_launch task completion - Restored belt_velocity_ramp_action.hpp (BeltModerateAction) - Added logging for fire_count changes Task flow now works correctly: 1. slider_init: calibrates zero point 2. launch_prepare: uses fire_count to select velocity (first vs subsequent) 3. fire: increments fire_count 4. cancel_launch: resets fire_count to 0 This ensures proper position control behavior across multiple launch cycles. --- .../action/belt_velocity_ramp_action.hpp | 127 ++++++++++++++++++ src/manager/dart_manager.cpp | 17 ++- 2 files changed, 142 insertions(+), 2 deletions(-) create mode 100644 src/manager/action/belt_velocity_ramp_action.hpp diff --git a/src/manager/action/belt_velocity_ramp_action.hpp b/src/manager/action/belt_velocity_ramp_action.hpp new file mode 100644 index 0000000..11028d4 --- /dev/null +++ b/src/manager/action/belt_velocity_ramp_action.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include "action.hpp" + +#include +#include +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +class BeltModerateAction : public IAction { +public: + BeltModerateAction( + std::string name, rmcs_msgs::DartSliderStatus& belt_command, + double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, + bool& belt_wait_zero_velocity, const double& left_belt_velocity, + const double& right_belt_velocity, rmcs_msgs::DartSliderStatus move_command, + double start_velocity, double ramp_step_per_tick, double torque_limit, double hold_torque, + double zero_velocity_threshold, uint64_t zero_confirm_ticks, uint64_t timeout_ticks) + : IAction(std::move(name)) + , belt_command_(belt_command) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_limit_(belt_torque_limit) + , belt_hold_torque_(belt_hold_torque) + , belt_wait_zero_velocity_(belt_wait_zero_velocity) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , move_command_(move_command) + , start_velocity_(std::max(0.0, std::abs(start_velocity))) + , ramp_step_per_tick_(std::max(0.0, std::abs(ramp_step_per_tick))) + , torque_limit_(std::max(0.0, std::abs(torque_limit))) + , hold_torque_(hold_torque) + , zero_velocity_threshold_(std::max(0.0, std::abs(zero_velocity_threshold))) + , zero_confirm_ticks_(zero_confirm_ticks) + , timeout_ticks_(timeout_ticks) {} + + void on_enter() override { + // 保持当前运动方向，不改变 belt_command_（避免方向突变） + belt_torque_limit_ = torque_limit_; + belt_hold_torque_ = hold_torque_; + + // 读取当前实际速度作为起始速度（避免速度突变） + const double avg_vel = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + + // 使用实际速度和设定速度中的较大值，确保平滑过渡 + current_velocity_ = std::max(avg_vel, start_velocity_); + belt_target_velocity_ = current_velocity_; + + zero_counter_ = 0; + + printf("[%s] on_enter: start_velocity=%.4f, actual_velocity=%.4f, using=%.4f\n", + name().c_str(), start_velocity_, avg_vel, current_velocity_); + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + printf("[%s] TIMEOUT after %lu ticks\n", name().c_str(), elapsed_ticks()); + return ActionStatus::FAILURE; + } + + // 平滑减速：每个 tick 减少固定步长 + current_velocity_ = std::max(0.0, current_velocity_ - ramp_step_per_tick_); + belt_target_velocity_ = current_velocity_; + + // 每100帧打印一次调试信息 + if (elapsed_ticks() % 100 == 0) { + const double avg_vel = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + printf("[%s] tick=%lu, target_vel=%.4f, actual_vel=%.4f\n", + name().c_str(), elapsed_ticks(), current_velocity_, avg_vel); + } + + // 当目标速度接近0时，检查实际速度是否也接近0 + if (current_velocity_ <= 1e-6) { + const double avg_vel = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + if (avg_vel < zero_velocity_threshold_) { + ++zero_counter_; + if (zero_counter_ >= zero_confirm_ticks_) { + printf("[%s] SUCCESS: velocity reached zero (actual_vel=%.4f < threshold=%.4f)\n", + name().c_str(), avg_vel, zero_velocity_threshold_); + return ActionStatus::SUCCESS; + } + } else { + zero_counter_ = 0; + } + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + // 减速完成，进入 WAIT 模式 + belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; + belt_target_velocity_ = 0.0; // 目标速度为 0 + belt_torque_limit_ = torque_limit_; + belt_hold_torque_ = hold_torque_; + belt_wait_zero_velocity_ = false; + } + +private: + rmcs_msgs::DartSliderStatus& belt_command_; + double& belt_target_velocity_; + double& belt_torque_limit_; + double& belt_hold_torque_; + bool& belt_wait_zero_velocity_; + const double& left_belt_velocity_; + const double& right_belt_velocity_; + + rmcs_msgs::DartSliderStatus move_command_; + double start_velocity_; + double ramp_step_per_tick_; + double torque_limit_; + double hold_torque_; + double zero_velocity_threshold_; + uint64_t zero_confirm_ticks_; + uint64_t timeout_ticks_; + + double current_velocity_{0.0}; + uint64_t zero_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 331a5e9..4221c22 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -305,9 +305,22 @@ class DartManagerV2 current_task_->on_exit(); RCLCPP_INFO( logger_, "[DartManagerV2] task '%s' SUCCESS", completed_task_name.c_str()); - if (launch_prepare_enable_visual_assist_ && completed_task_name == "fire") { - advance_dart_sequence_after_fire(); + + // 处理 fire 任务完成 + if (completed_task_name == "fire") { + fire_count_++; + RCLCPP_INFO(logger_, "[DartManagerV2] fire completed, fire_count=%u", fire_count_); + if (launch_prepare_enable_visual_assist_) { + advance_dart_sequence_after_fire(); + } + } + + // 处理 cancel_launch 任务完成 - 重置 fire_count + if (completed_task_name == "cancel_launch") { + fire_count_ = 0; + RCLCPP_INFO(logger_, "[DartManagerV2] cancel_launch completed, fire_count reset to 0"); } + current_task_.reset(); transition_to(State::IDLE); From 0f687fed56f0795edd92d16415419a0dea1405a2 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Tue, 31 Mar 2026 22:20:39 +0800 Subject: [PATCH 05/19] Fix task layer action initialization parameters BeltMoveAction changes: - Added timeout_returns_success parameter (default: false) - Updated update() to use this parameter for timeout handling - Added member variable declaration Task updates: - silder_init_task: Already correct (timeout_returns_success=true) - launch_preparation_task: Added timeout_returns_success=false to belt_reset_up - cancel_launch_task: Added timeout_returns_success=false to belt_reset This ensures all BeltMoveAction calls have the correct number of parameters and proper timeout behavior. --- src/manager/action/belt_move_action.hpp | 9 +- src/manager/dart_manager.cpp | 168 +++++++++---------- src/manager/task/cancel_launch_task.hpp | 3 +- src/manager/task/launch_preparation_task.hpp | 40 ++--- 4 files changed, 111 insertions(+), 109 deletions(-) diff --git a/src/manager/action/belt_move_action.hpp b/src/manager/action/belt_move_action.hpp index 5babf2f..008cfff 100644 --- a/src/manager/action/belt_move_action.hpp +++ b/src/manager/action/belt_move_action.hpp @@ -27,7 +27,8 @@ class BeltMoveAction : public IAction { uint64_t timeout_ticks, double stall_velocity_threshold = 1.0, double stall_torque_threshold = 0.5, uint64_t stall_confirm_ticks = 20, uint64_t min_running_ticks = 50, - ExitMode exit_mode = ExitMode::WAIT_HOLD_TORQUE) + ExitMode exit_mode = ExitMode::WAIT_HOLD_TORQUE, + bool timeout_returns_success = false) : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -47,7 +48,8 @@ class BeltMoveAction : public IAction { , stall_torque_threshold_(stall_torque_threshold) , stall_confirm_ticks_(stall_confirm_ticks) , min_running_ticks_(min_running_ticks) - , exit_mode_(exit_mode) {} + , exit_mode_(exit_mode) + , timeout_returns_success_(timeout_returns_success) {} void on_enter() override { belt_command_ = command_; @@ -59,7 +61,7 @@ class BeltMoveAction : public IAction { ActionStatus update() override { if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::FAILURE; + return timeout_returns_success_ ? ActionStatus::SUCCESS : ActionStatus::FAILURE; } if (elapsed_ticks() > min_running_ticks_) { @@ -115,6 +117,7 @@ class BeltMoveAction : public IAction { uint64_t stall_confirm_ticks_; uint64_t min_running_ticks_; ExitMode exit_mode_; + bool timeout_returns_success_; uint64_t stall_counter_{0}; }; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 4221c22..17ab83a 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -41,9 +41,9 @@ class DartManagerV2 , public rclcpp::Node { public: enum class State : uint8_t { - IDLE = 0, + IDLE = 0, RUNNING = 1, - ERROR = 2, + ERROR = 2, }; DartManagerV2() @@ -52,24 +52,25 @@ class DartManagerV2 rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , logger_(get_logger()) { - register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); + register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); - register_input("/dart/drive_belt/left/torque", left_belt_torque_); - register_input("/dart/drive_belt/right/torque", right_belt_torque_); - register_input("/dart/drive_belt/left/angle", left_belt_angle_); - register_input("/dart/drive_belt/right/angle", right_belt_angle_); - register_input("/dart/lifting_left/velocity", lifting_left_vel_fb_); - register_input("/dart/lifting_right/velocity", lifting_right_vel_fb_); + register_input("/dart/drive_belt/left/torque", left_belt_torque_); + register_input("/dart/drive_belt/right/torque", right_belt_torque_); + register_input("/dart/drive_belt/left/angle", left_belt_angle_); + register_input("/dart/drive_belt/right/angle", right_belt_angle_); + register_input("/dart/lifting_left/velocity", lifting_left_vel_fb_); + register_input("/dart/lifting_right/velocity", lifting_right_vel_fb_); - register_input("/dart/manager/command", remote_command_input_, false); - register_input("/dart/manager/web_command", web_command_input_, false); + register_input("/dart/manager/command", remote_command_input_, false); + register_input("/dart/manager/web_command", web_command_input_, false); - register_input("/remote/joystick/left", joystick_left_, false); + register_input("/remote/joystick/left", joystick_left_, false); register_input("/remote/joystick/right", joystick_right_, false); register_input("/dart_guidance/camera/target_position", target_position_input_, false); register_input("/dart_guidance/tracker/tracking", target_tracking_input_, false); - register_output("/dart/manager/belt/command", belt_command_, rmcs_msgs::DartSliderStatus::WAIT); + register_output( + "/dart/manager/belt/command", belt_command_, rmcs_msgs::DartSliderStatus::WAIT); register_output("/dart/manager/belt/target_velocity", belt_target_velocity_, 0.0); register_output("/dart/manager/belt/torque_limit", belt_torque_limit_, 0.0); register_output("/dart/manager/belt/hold_torque", belt_hold_torque_, 0.0); @@ -77,7 +78,8 @@ class DartManagerV2 register_output("/dart/manager/belt/zero_calibration", belt_zero_calibration_, false); register_output("/dart/manager/trigger/lock_enable", trigger_lock_enable_, false); - register_output("/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); + register_output( + "/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); register_output("/force/control/velocity", force_control_velocity_, 0.0); register_output("/dart/manager/aim/ready", aim_ready_, false); register_output( @@ -91,8 +93,7 @@ class DartManagerV2 // 升降指令总线 register_output( - "/dart/manager/lifting/command", lifting_command_, - rmcs_msgs::DartSliderStatus::WAIT); + "/dart/manager/lifting/command", lifting_command_, rmcs_msgs::DartSliderStatus::WAIT); register_output( "/dart/manager/limiting/command", limiting_command_, rmcs_msgs::DartLimitingServoStatus::LOCK); @@ -115,10 +116,10 @@ class DartManagerV2 load_auto_aim_configuration(); } - limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); + limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); - belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m - belt_pulley_radius_ = get_parameter("belt_pulley_radius").as_double(); // m + belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m + belt_pulley_radius_ = get_parameter("belt_pulley_radius").as_double(); // m // 传送带速度和扭矩限制参数（集中管理） try { @@ -168,7 +169,7 @@ class DartManagerV2 belt_prepare_down_ramp_timeout_ticks_ = 2000; } - lifting_stall_threshold_ = get_parameter("lifting_stall_threshold").as_double(); + lifting_stall_threshold_ = get_parameter("lifting_stall_threshold").as_double(); lifting_stall_confirm_ticks_ = (uint64_t)get_parameter("lifting_stall_confirm_ticks").as_int(); lifting_stall_min_run_ticks_ = @@ -189,9 +190,9 @@ class DartManagerV2 poll_command(); switch (state_) { - case State::IDLE: dispatch_next_task(); break; - case State::RUNNING: tick_current_task(); break; - case State::ERROR: break; + case State::IDLE: dispatch_next_task(); break; + case State::RUNNING: tick_current_task(); break; + case State::ERROR: break; } } @@ -303,8 +304,7 @@ class DartManagerV2 if (status == ActionStatus::SUCCESS) { const std::string completed_task_name = current_task_->name(); current_task_->on_exit(); - RCLCPP_INFO( - logger_, "[DartManagerV2] task '%s' SUCCESS", completed_task_name.c_str()); + RCLCPP_INFO(logger_, "[DartManagerV2] task '%s' SUCCESS", completed_task_name.c_str()); // 处理 fire 任务完成 if (completed_task_name == "fire") { @@ -318,7 +318,8 @@ class DartManagerV2 // 处理 cancel_launch 任务完成 - 重置 fire_count if (completed_task_name == "cancel_launch") { fire_count_ = 0; - RCLCPP_INFO(logger_, "[DartManagerV2] cancel_launch completed, fire_count reset to 0"); + RCLCPP_INFO( + logger_, "[DartManagerV2] cancel_launch completed, fire_count reset to 0"); } current_task_.reset(); @@ -355,7 +356,7 @@ class DartManagerV2 } void transition_to(State new_state) { - state_ = new_state; + state_ = new_state; first_tick_of_task_ = true; if (state_pub_) { @@ -372,8 +373,7 @@ class DartManagerV2 current_target_position_ = cv::Point2i(-1, -1); } - current_target_tracking_ = - target_tracking_input_.ready() ? *target_tracking_input_ : false; + current_target_tracking_ = target_tracking_input_.ready() ? *target_tracking_input_ : false; } double get_numeric_parameter_or_throw(const std::string& name) const { @@ -410,7 +410,7 @@ class DartManagerV2 switch (parameter.get_type()) { case rclcpp::PARAMETER_DOUBLE_ARRAY: return parameter.as_double_array(); case rclcpp::PARAMETER_INTEGER_ARRAY: { - const auto values = parameter.as_integer_array(); + const auto& values = parameter.as_integer_array(); std::vector result; result.reserve(values.size()); for (const auto value : values) { @@ -443,10 +443,9 @@ class DartManagerV2 has_parameter("aim_ready_exit_deadband_px") ? get_vector2_parameter_or_throw("aim_ready_exit_deadband_px") : Eigen::Vector2d::Constant(5.0); - aim_accept_deadband_px_ = - has_parameter("aim_accept_deadband_px") - ? get_vector2_parameter_or_throw("aim_accept_deadband_px") - : aim_ready_exit_deadband_px_; + aim_accept_deadband_px_ = has_parameter("aim_accept_deadband_px") + ? get_vector2_parameter_or_throw("aim_accept_deadband_px") + : aim_ready_exit_deadband_px_; aim_yaw_gain_ = get_numeric_parameter_or_throw("aim_yaw_gain"); aim_pitch_gain_ = get_numeric_parameter_or_throw("aim_pitch_gain"); aim_ready_confirm_ticks_ = has_parameter("aim_ready_confirm_ticks") @@ -500,16 +499,16 @@ class DartManagerV2 } void sync_current_dart_outputs() { - *aim_current_dart_index_ = - launch_prepare_enable_visual_assist_ ? dart_launch_sequence_.current_dart_index_u8() - : static_cast(0); + *aim_current_dart_index_ = launch_prepare_enable_visual_assist_ + ? dart_launch_sequence_.current_dart_index_u8() + : static_cast(0); auto_aim_feedback_.set_desired_target_px(current_desired_target_px()); } void clear_auto_aim_feedback() { - *aim_current_dart_index_ = - launch_prepare_enable_visual_assist_ ? dart_launch_sequence_.current_dart_index_u8() - : static_cast(0); + *aim_current_dart_index_ = launch_prepare_enable_visual_assist_ + ? dart_launch_sequence_.current_dart_index_u8() + : static_cast(0); auto_aim_feedback_.reset(current_desired_target_px()); } @@ -529,8 +528,7 @@ class DartManagerV2 } void prepare_outputs_for_task(const Task& task) { - if ( - task.name() == "launch_preparation" || task.name() == "fire" + if (task.name() == "launch_preparation" || task.name() == "fire" || task.name() == "cancel_launch" || task.name() == "slider_init" || task.name() == "manual_angle" || task.name() == "manual_force") { clear_auto_aim_feedback(); @@ -539,12 +537,9 @@ class DartManagerV2 std::shared_ptr make_slider_init_task() { return std::make_shared( - *belt_command_, - *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, - *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, - *belt_zero_calibration_); + *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, + *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, + *left_belt_torque_, *right_belt_torque_, *belt_zero_calibration_); } // 任务工厂 @@ -556,15 +551,20 @@ class DartManagerV2 bool require_lifting_down = (fire_count_ > 0); // 打印角度反馈状态 - RCLCPP_INFO(logger_, "[DartManager] Creating launch_prepare task, fire_count=%u", fire_count_); + RCLCPP_INFO( + logger_, "[DartManager] Creating launch_prepare task, fire_count=%u", fire_count_); if (left_belt_angle_.ready() && right_belt_angle_.ready()) { - RCLCPP_INFO(logger_, "[DartManager] Belt angles: left=%.4f, right=%.4f", - *left_belt_angle_, *right_belt_angle_); + RCLCPP_INFO( + logger_, "[DartManager] Belt angles: left=%.4f, right=%.4f", *left_belt_angle_, + *right_belt_angle_); } else { RCLCPP_WARN(logger_, "[DartManager] Belt angle feedback NOT READY!"); } - RCLCPP_INFO(logger_, "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m, down_velocity=%.2f rad/s", - belt_down_distance_, belt_pulley_radius_, down_velocity); + RCLCPP_INFO( + logger_, + "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m, " + "down_velocity=%.2f rad/s", + belt_down_distance_, belt_pulley_radius_, down_velocity); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, @@ -595,30 +595,28 @@ class DartManagerV2 if (cmd == "fire") { return std::make_shared( - *trigger_lock_enable_, - *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, - lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, - *limiting_command_, limiting_fill_ticks_); + *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, + *lifting_right_vel_fb_, lifting_stall_threshold_, lifting_stall_confirm_ticks_, + lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, *limiting_command_, + limiting_fill_ticks_); } if (cmd == "manual_angle") { auto task = std::make_shared("manual_angle", "手动 yaw/pitch 调整"); - task->then(std::make_shared( - auto_aim_feedback_.yaw_pitch_control_velocity()[0], - auto_aim_feedback_.yaw_pitch_control_velocity()[1], *joystick_left_, - *joystick_right_, max_transform_rate_)); + task->then( + std::make_shared( + auto_aim_feedback_.yaw_pitch_control_velocity()[0], + auto_aim_feedback_.yaw_pitch_control_velocity()[1], *joystick_left_, + *joystick_right_, max_transform_rate_)); return task; } if (cmd == "manual_force") { auto task = std::make_shared("manual_force", "手动力丝杆速度调整"); - task->then(std::make_shared( - *force_control_velocity_, - *joystick_right_, - max_transform_rate_, - manual_force_scale_)); + task->then( + std::make_shared( + *force_control_velocity_, *joystick_right_, max_transform_rate_, + manual_force_scale_)); return task; } return nullptr; @@ -635,8 +633,8 @@ class DartManagerV2 InputInterface joystick_left_; InputInterface joystick_right_; - InputInterface target_position_input_; - InputInterface target_tracking_input_; + InputInterface target_position_input_; + InputInterface target_tracking_input_; // 升降速度反馈（FillingLiftAction 堵转检测用） InputInterface lifting_left_vel_fb_; @@ -646,27 +644,27 @@ class DartManagerV2 OutputInterface belt_target_velocity_; OutputInterface belt_torque_limit_; OutputInterface belt_hold_torque_; - OutputInterface belt_wait_zero_velocity_; - OutputInterface belt_zero_calibration_; - OutputInterface trigger_lock_enable_; + OutputInterface belt_wait_zero_velocity_; + OutputInterface belt_zero_calibration_; + OutputInterface trigger_lock_enable_; OutputInterface yaw_pitch_control_velocity_; - OutputInterface force_control_velocity_; - OutputInterface aim_ready_; - OutputInterface aim_current_dart_index_; + OutputInterface force_control_velocity_; + OutputInterface aim_ready_; + OutputInterface aim_current_dart_index_; OutputInterface aim_error_px_; OutputInterface aim_desired_target_px_; OutputInterface lifting_command_; OutputInterface limiting_command_; - double max_transform_rate_{500.0}; - double manual_force_scale_{5.0}; - double auto_aim_max_transform_rate_{500.0}; + double max_transform_rate_{500.0}; + double manual_force_scale_{5.0}; + double auto_aim_max_transform_rate_{500.0}; uint64_t limiting_fill_ticks_{500}; - double belt_down_distance_{0.0}; // m - double belt_pulley_radius_{0.0}; // m + double belt_down_distance_{0.0}; // m + double belt_pulley_radius_{0.0}; // m double belt_prepare_down_velocity_first_{5.0}; // rad/s double belt_prepare_down_velocity_{10.0}; // rad/s @@ -677,7 +675,7 @@ class DartManagerV2 uint64_t belt_prepare_down_zero_confirm_ticks_{60}; // ticks uint64_t belt_prepare_down_ramp_timeout_ticks_{2000}; // ticks - double lifting_stall_threshold_{0.5}; + double lifting_stall_threshold_{0.5}; uint64_t lifting_stall_confirm_ticks_{100}; uint64_t lifting_stall_min_run_ticks_{500}; uint64_t lifting_stall_timeout_ticks_{5000}; @@ -697,16 +695,16 @@ class DartManagerV2 InputInterface remote_command_input_; InputInterface web_command_input_; - std::string last_command_; + std::string last_command_; rclcpp::Publisher::SharedPtr state_pub_; State state_{State::IDLE}; - std::shared_ptr current_task_; + std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; - uint32_t fire_count_{0}; // 当前轮次已完成发射数 + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; }; diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index a9f0151..a17c82f 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -113,7 +113,8 @@ class CancelLaunchTask : public Task { 0.5, // 堵转力矩阈值 100, // 堵转确认帧数 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); // 退出模式 + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 + false)); // 超时返回失败 // 步骤5：填装升降上行回到初始位 then( diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index ccf955f..b6623f4 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -125,27 +125,27 @@ class LaunchPreparationTask : public Task { // 步骤4：传送带上行到机械限位（速度控制 + 堵转检测） then( std::make_shared( - "belt_reset_up", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 15, // 设定速度（rad/s） - 2.5, // 设定力矩限制（N⋅m） - 0.5, // 设定保持力矩（N⋅m） - 5000, // 超时帧数 - 0.5, // 堵转速度阈值（rad/s） - 0.8, // 堵转力矩阈值（N⋅m） - 100, // 堵转确认帧数 - 50, // 最短运行帧数 + "belt_reset_up", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 15, // 设定速度（rad/s） + 2.5, // 设定力矩限制（N⋅m） + 0.5, // 设定保持力矩（N⋅m） + 5000, // 超时帧数 + 0.5, // 堵转速度阈值（rad/s） + 0.8, // 堵转力矩阈值（N⋅m） + 100, // 堵转确认帧数 + 50, // 最短运行帧数 BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 - true)); // 超时也返回 SUCCESS + false)); // 超时返回失败 then(std::make_shared("stabilize_wait", 50)); From fcbea72c196a0226ba4da11f0282e64754d3e9c8 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Wed, 1 Apr 2026 03:04:03 +0800 Subject: [PATCH 06/19] done down position control --- plugins.xml | 3 - src/manager/action/auto_aim_action.hpp | 28 +- .../action/belt_hold_torque_action.hpp | 20 +- src/manager/action/belt_move_action.hpp | 16 +- .../action/belt_pid_deceleration_action.hpp | 78 ++++++ .../action/belt_velocity_ramp_action.hpp | 73 +++-- src/manager/dart_manager.cpp | 43 ++- src/manager/task/cancel_launch_task.hpp | 134 ++++----- src/manager/task/fire_and_preload_task.hpp | 30 +- src/manager/task/launch_preparation_task.hpp | 153 ++++++----- src/manager/task/silder_init_task.hpp | 2 +- ...ision_assisted_launch_preparation_task.hpp | 44 +-- tests/launch_preparation_task_test.cpp | 160 +++++------ ..._assisted_launch_preparation_task_test.cpp | 256 +++++++++--------- 14 files changed, 583 insertions(+), 457 deletions(-) create mode 100644 src/manager/action/belt_pid_deceleration_action.hpp diff --git a/plugins.xml b/plugins.xml index 4ed63cb..ceaa893 100644 --- a/plugins.xml +++ b/plugins.xml @@ -20,9 +20,6 @@ Handles WebUI ROS communication and heartbeat - - Dart task manager V2: 4-shot group counting with lifting and limiting servo control. - Dart task manager ForFillingTest: 4-shot group counting with lifting and limiting servo control. diff --git a/src/manager/action/auto_aim_action.hpp b/src/manager/action/auto_aim_action.hpp index fc9418f..ea7ae59 100644 --- a/src/manager/action/auto_aim_action.hpp +++ b/src/manager/action/auto_aim_action.hpp @@ -38,7 +38,7 @@ class AutoAimAction : public IAction { AutoAimAction( Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, Eigen::Vector2d& desired_target_px_output, const cv::Point2i& current_target_position, - const bool& tracking, const rclcpp::Logger& logger, AutoAimParams params) + const bool& tracking, rclcpp::Logger logger, AutoAimParams params) : IAction("auto_aim") , yaw_pitch_control_velocity_(yaw_pitch_control_velocity) , aim_ready_(aim_ready) @@ -46,7 +46,7 @@ class AutoAimAction : public IAction { , desired_target_px_output_(desired_target_px_output) , current_target_position_(current_target_position) , tracking_(tracking) - , logger_(logger) + , logger_(std::move(logger)) , params_(std::move(params)) {} void on_enter() override { @@ -101,15 +101,13 @@ class AutoAimAction : public IAction { const Eigen::Vector2d accept_deadband_px = params_.accept_deadband_px.cwiseMax(ready_exit_deadband_px); - const bool within_deadband = - std::abs(error_px.x()) <= params_.deadband_px.x() - && std::abs(error_px.y()) <= params_.deadband_px.y(); + const bool within_deadband = std::abs(error_px.x()) <= params_.deadband_px.x() + && std::abs(error_px.y()) <= params_.deadband_px.y(); const bool within_ready_hold_deadband = std::abs(error_px.x()) <= ready_exit_deadband_px.x() && std::abs(error_px.y()) <= ready_exit_deadband_px.y(); - const bool within_accept_deadband = - std::abs(error_px.x()) <= accept_deadband_px.x() - && std::abs(error_px.y()) <= accept_deadband_px.y(); + const bool within_accept_deadband = std::abs(error_px.x()) <= accept_deadband_px.x() + && std::abs(error_px.y()) <= accept_deadband_px.y(); if (aim_ready_ && within_ready_hold_deadband) { yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); @@ -172,16 +170,14 @@ class AutoAimAction : public IAction { last_log_time_ = now; RCLCPP_INFO( - logger_, - "[AutoAimAction] desired=(%.1f, %.1f) current=(%d, %d) error=(%.1f, %.1f)", + logger_, "[AutoAimAction] desired=(%.1f, %.1f) current=(%d, %d) error=(%.1f, %.1f)", params_.desired_target_px.x(), params_.desired_target_px.y(), current_target_position.x, current_target_position.y, error_px.x(), error_px.y()); } void log_timeout() const { RCLCPP_WARN( - logger_, - "[AutoAimAction] timeout: tracking=%s last_error=(%.1f, %.1f)", + logger_, "[AutoAimAction] timeout: tracking=%s last_error=(%.1f, %.1f)", last_tracking_valid_ ? "true" : "false", aim_error_px_.x(), aim_error_px_.y()); } @@ -191,16 +187,16 @@ class AutoAimAction : public IAction { } const double velocity = error_px * gain; - const double bounded_velocity = std::clamp( - velocity, -params_.max_transform_rate, params_.max_transform_rate); + const double bounded_velocity = + std::clamp(velocity, -params_.max_transform_rate, params_.max_transform_rate); const double bounded_magnitude = std::abs(bounded_velocity); if (bounded_magnitude <= 0.0) { return 0.0; } const double final_magnitude = std::clamp( - std::max(bounded_magnitude, params_.min_transform_rate), - 0.0, params_.max_transform_rate); + std::max(bounded_magnitude, params_.min_transform_rate), 0.0, + params_.max_transform_rate); return std::copysign(final_magnitude, bounded_velocity); } diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index 5be9c37..e38b903 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -10,27 +10,31 @@ namespace rmcs_dart_guidance::manager { // BeltHoldTorqueAction - 保持传送带张力 -// 持续输出 WAIT 命令 + hold_torque，用于在扳机锁定期间保持传送带张力 +// 使用零速度闭环 + 常态扭矩偏移，用于在扳机锁定期间保持传送带张力 +// 与减速阶段一致，使用PID控制速度为0，同时叠加常态扭矩偏移补偿负载 class BeltHoldTorqueAction : public IAction { public: BeltHoldTorqueAction( std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, - double& belt_hold_torque, bool& belt_wait_zero_velocity, double hold_torque_value, - uint64_t hold_duration_ticks) + double& belt_hold_torque, bool& belt_wait_zero_velocity, double& belt_torque_offset, + double hold_torque_value, double torque_offset_value, uint64_t hold_duration_ticks) : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) , belt_hold_torque_(belt_hold_torque) , belt_wait_zero_velocity_(belt_wait_zero_velocity) + , belt_torque_offset_(belt_torque_offset) , hold_torque_value_(hold_torque_value) + , torque_offset_value_(torque_offset_value) , hold_duration_ticks_(hold_duration_ticks) {} void on_enter() override { - // 立即设置WAIT命令和保持扭矩，确保无缝过渡 + // 设置WAIT命令和零速度闭环模式 belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; belt_target_velocity_ = 0.0; belt_hold_torque_ = hold_torque_value_; - belt_wait_zero_velocity_ = false; // 使用 HOLD_TORQUE 模式 + belt_wait_zero_velocity_ = true; // 使用零速度闭环 + belt_torque_offset_ = torque_offset_value_; // 叠加常态扭矩偏移补偿负载 } ActionStatus update() override { @@ -41,14 +45,18 @@ class BeltHoldTorqueAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override {} + void on_exit() override { + belt_torque_offset_ = 0.0; // 退出时清除扭矩偏移 + } private: rmcs_msgs::DartSliderStatus& belt_command_; double& belt_target_velocity_; double& belt_hold_torque_; bool& belt_wait_zero_velocity_; + double& belt_torque_offset_; double hold_torque_value_; + double torque_offset_value_; uint64_t hold_duration_ticks_; }; diff --git a/src/manager/action/belt_move_action.hpp b/src/manager/action/belt_move_action.hpp index 008cfff..d05dbf9 100644 --- a/src/manager/action/belt_move_action.hpp +++ b/src/manager/action/belt_move_action.hpp @@ -6,7 +6,7 @@ #include #include -#include +#include "rmcs_msgs/dart_slider_status.hpp" namespace rmcs_dart_guidance::manager { @@ -18,16 +18,14 @@ class BeltMoveAction : public IAction { }; BeltMoveAction( - std::string name, rmcs_msgs::DartSliderStatus& belt_command, - double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, - bool& belt_wait_zero_velocity, + std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, - rmcs_msgs::DartSliderStatus command, double velocity, double torque_limit, double hold_torque, - uint64_t timeout_ticks, double stall_velocity_threshold = 1.0, - double stall_torque_threshold = 0.5, - uint64_t stall_confirm_ticks = 20, uint64_t min_running_ticks = 50, - ExitMode exit_mode = ExitMode::WAIT_HOLD_TORQUE, + rmcs_msgs::DartSliderStatus command, double velocity, double torque_limit, + double hold_torque, uint64_t timeout_ticks, double stall_velocity_threshold = 1.0, + double stall_torque_threshold = 0.5, uint64_t stall_confirm_ticks = 20, + uint64_t min_running_ticks = 50, ExitMode exit_mode = ExitMode::WAIT_HOLD_TORQUE, bool timeout_returns_success = false) : IAction(std::move(name)) , belt_command_(belt_command) diff --git a/src/manager/action/belt_pid_deceleration_action.hpp b/src/manager/action/belt_pid_deceleration_action.hpp new file mode 100644 index 0000000..a93af24 --- /dev/null +++ b/src/manager/action/belt_pid_deceleration_action.hpp @@ -0,0 +1,78 @@ +#pragma once + +#include "action.hpp" +#include +#include +#include + +namespace rmcs_dart_guidance::manager { + +class BeltPIDDecelerationAction : public IAction { +public: + BeltPIDDecelerationAction( + std::string name, + double& belt_target_velocity, + double& belt_torque_offset, + const double& left_belt_velocity, + const double& right_belt_velocity, + double torque_offset_value, + double zero_velocity_threshold, + uint64_t zero_confirm_ticks, + uint64_t timeout_ticks + ) + : IAction(std::move(name)) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_offset_(belt_torque_offset) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , torque_offset_value_(torque_offset_value) + , zero_velocity_threshold_(zero_velocity_threshold) + , zero_confirm_ticks_(zero_confirm_ticks) + , timeout_ticks_(timeout_ticks) + {} + + void on_enter() override { + belt_target_velocity_ = 0.0; + belt_torque_offset_ = torque_offset_value_; + zero_counter_ = 0; + } + + ActionStatus update() override { + // 超时检测 + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + // 计算平均速度 + double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + + // 零速检测 + if (avg_velocity < zero_velocity_threshold_) { + ++zero_counter_; + if (zero_counter_ >= zero_confirm_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + zero_counter_ = 0; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + belt_torque_offset_ = 0.0; + } + +private: + double& belt_target_velocity_; + double& belt_torque_offset_; + const double& left_belt_velocity_; + const double& right_belt_velocity_; + double torque_offset_value_; + double zero_velocity_threshold_; + uint64_t zero_confirm_ticks_; + uint64_t timeout_ticks_; + uint64_t zero_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_velocity_ramp_action.hpp b/src/manager/action/belt_velocity_ramp_action.hpp index 11028d4..6871f19 100644 --- a/src/manager/action/belt_velocity_ramp_action.hpp +++ b/src/manager/action/belt_velocity_ramp_action.hpp @@ -14,12 +14,12 @@ namespace rmcs_dart_guidance::manager { class BeltModerateAction : public IAction { public: BeltModerateAction( - std::string name, rmcs_msgs::DartSliderStatus& belt_command, - double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, - bool& belt_wait_zero_velocity, const double& left_belt_velocity, - const double& right_belt_velocity, rmcs_msgs::DartSliderStatus move_command, - double start_velocity, double ramp_step_per_tick, double torque_limit, double hold_torque, - double zero_velocity_threshold, uint64_t zero_confirm_ticks, uint64_t timeout_ticks) + std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, + const double& left_belt_velocity, const double& right_belt_velocity, + uint64_t target_ramp_ticks, double torque_limit, double hold_torque, + double zero_velocity_threshold, uint64_t zero_confirm_ticks, uint64_t timeout_ticks, + double initial_velocity) : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -28,61 +28,56 @@ class BeltModerateAction : public IAction { , belt_wait_zero_velocity_(belt_wait_zero_velocity) , left_belt_velocity_(left_belt_velocity) , right_belt_velocity_(right_belt_velocity) - , move_command_(move_command) - , start_velocity_(std::max(0.0, std::abs(start_velocity))) - , ramp_step_per_tick_(std::max(0.0, std::abs(ramp_step_per_tick))) + , target_ramp_ticks_(target_ramp_ticks) , torque_limit_(std::max(0.0, std::abs(torque_limit))) , hold_torque_(hold_torque) , zero_velocity_threshold_(std::max(0.0, std::abs(zero_velocity_threshold))) , zero_confirm_ticks_(zero_confirm_ticks) - , timeout_ticks_(timeout_ticks) {} + , timeout_ticks_(timeout_ticks) + , initial_velocity_(std::abs(initial_velocity)) {} void on_enter() override { - // 保持当前运动方向，不改变 belt_command_（避免方向突变） belt_torque_limit_ = torque_limit_; belt_hold_torque_ = hold_torque_; - // 读取当前实际速度作为起始速度（避免速度突变） - const double avg_vel = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - - // 使用实际速度和设定速度中的较大值，确保平滑过渡 - current_velocity_ = std::max(avg_vel, start_velocity_); + // 使用传入的初始速度，而不是读取可能错误的速度反馈 + // 但如果上一个action已经设置了belt_target_velocity_，优先使用它以保证连续性 + current_velocity_ = + (belt_target_velocity_ > 1e-6) ? belt_target_velocity_ : initial_velocity_; belt_target_velocity_ = current_velocity_; + // 基于初始速度和目标减速时长，动态计算减速步长 + // 这样无论初始速度是多少，减速时间都是 target_ramp_ticks + ramp_step_per_tick_ = target_ramp_ticks_ > 0 + ? (current_velocity_ / static_cast(target_ramp_ticks_)) + : current_velocity_; + zero_counter_ = 0; - printf("[%s] on_enter: start_velocity=%.4f, actual_velocity=%.4f, using=%.4f\n", - name().c_str(), start_velocity_, avg_vel, current_velocity_); + // 调试日志：打印关键参数 + static int enter_count = 0; + ++enter_count; + printf( + "[BeltModerateAction::on_enter #%d] initial_velocity_=%.4f, current_velocity_=%.4f, " + "belt_target_velocity_=%.4f, ramp_step=%.6f\n", + enter_count, initial_velocity_, current_velocity_, belt_target_velocity_, + ramp_step_per_tick_); } ActionStatus update() override { if (elapsed_ticks() >= timeout_ticks_) { - printf("[%s] TIMEOUT after %lu ticks\n", name().c_str(), elapsed_ticks()); return ActionStatus::FAILURE; } - // 平滑减速：每个 tick 减少固定步长 current_velocity_ = std::max(0.0, current_velocity_ - ramp_step_per_tick_); belt_target_velocity_ = current_velocity_; - // 每100帧打印一次调试信息 - if (elapsed_ticks() % 100 == 0) { - const double avg_vel = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - printf("[%s] tick=%lu, target_vel=%.4f, actual_vel=%.4f\n", - name().c_str(), elapsed_ticks(), current_velocity_, avg_vel); - } - - // 当目标速度接近0时，检查实际速度是否也接近0 if (current_velocity_ <= 1e-6) { const double avg_vel = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; if (avg_vel < zero_velocity_threshold_) { ++zero_counter_; if (zero_counter_ >= zero_confirm_ticks_) { - printf("[%s] SUCCESS: velocity reached zero (actual_vel=%.4f < threshold=%.4f)\n", - name().c_str(), avg_vel, zero_velocity_threshold_); return ActionStatus::SUCCESS; } } else { @@ -94,11 +89,11 @@ class BeltModerateAction : public IAction { } void on_exit() override { - // 减速完成，进入 WAIT 模式 + // 退出时设置为WAIT模式，但保持扭矩和参数设置 + // 这样下一个action可以无缝接管 belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; - belt_target_velocity_ = 0.0; // 目标速度为 0 - belt_torque_limit_ = torque_limit_; - belt_hold_torque_ = hold_torque_; + belt_target_velocity_ = 0.0; + // belt_torque_limit_和belt_hold_torque_保持不变，由下一个action接管 belt_wait_zero_velocity_ = false; } @@ -111,14 +106,14 @@ class BeltModerateAction : public IAction { const double& left_belt_velocity_; const double& right_belt_velocity_; - rmcs_msgs::DartSliderStatus move_command_; - double start_velocity_; - double ramp_step_per_tick_; + uint64_t target_ramp_ticks_; + double ramp_step_per_tick_{0.0}; double torque_limit_; double hold_torque_; double zero_velocity_threshold_; uint64_t zero_confirm_ticks_; uint64_t timeout_ticks_; + double initial_velocity_; // 减速起点速度 double current_velocity_{0.0}; uint64_t zero_counter_{0}; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 17ab83a..2ac9c12 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -8,9 +8,6 @@ #include "manager/task/launch_preparation_task.hpp" #include "manager/task/silder_init_task.hpp" #include "manager/task/task.hpp" -#include "manager/task/vision_assisted_launch_preparation_task.hpp" - -#include #include #include #include @@ -33,10 +30,10 @@ namespace rmcs_dart_guidance::manager { -// DartManagerV2 +// DartManager // · /dart/manager/lifting/command、/dart/manager/limiting/command 输出给下层执行组件 // · 升降堵转检测仍在 FillingLiftAction 内完成（直接读速度反馈，无循环依赖） -class DartManagerV2 +class DartManager : public rmcs_executor::Component , public rclcpp::Node { public: @@ -46,7 +43,7 @@ class DartManagerV2 ERROR = 2, }; - DartManagerV2() + DartManager() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) @@ -74,6 +71,7 @@ class DartManagerV2 register_output("/dart/manager/belt/target_velocity", belt_target_velocity_, 0.0); register_output("/dart/manager/belt/torque_limit", belt_torque_limit_, 0.0); register_output("/dart/manager/belt/hold_torque", belt_hold_torque_, 0.0); + register_output("/dart/manager/belt/torque_offset", belt_torque_offset_, 0.0); register_output("/dart/manager/belt/wait_zero_velocity", belt_wait_zero_velocity_, false); register_output("/dart/manager/belt/zero_calibration", belt_zero_calibration_, false); register_output("/dart/manager/trigger/lock_enable", trigger_lock_enable_, false); @@ -138,12 +136,24 @@ class DartManagerV2 } catch (...) { belt_prepare_torque_limit_ = 5.0; } + try { + belt_prepare_up_torque_limit_ = + get_parameter("belt_prepare_up_torque_limit").as_double(); + } catch (...) { + belt_prepare_up_torque_limit_ = 1.5; + } try { belt_prepare_down_ramp_ticks_ = (uint64_t)get_parameter("belt_prepare_down_ramp_ticks").as_int(); } catch (...) { belt_prepare_down_ramp_ticks_ = 400; } + try { + belt_prepare_down_torque_offset_ = + get_parameter("belt_prepare_down_torque_offset").as_double(); + } catch (...) { + belt_prepare_down_torque_offset_ = 2.0; + } try { belt_prepare_down_hold_torque_ = get_parameter("belt_prepare_down_hold_torque").as_double(); @@ -568,11 +578,12 @@ class DartManagerV2 return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *left_belt_angle_, *right_belt_angle_, - *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, + *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, belt_down_distance_, belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, - belt_prepare_down_ramp_ticks_, belt_prepare_down_hold_torque_, + belt_prepare_up_torque_limit_, belt_prepare_down_ramp_ticks_, + belt_prepare_down_torque_offset_, belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, require_lifting_down, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_); @@ -583,14 +594,15 @@ class DartManagerV2 double down_velocity = belt_prepare_down_velocity_; return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *left_belt_angle_, *right_belt_angle_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, - belt_prepare_down_ramp_ticks_, belt_prepare_down_hold_torque_, - belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, - belt_prepare_down_ramp_timeout_ticks_, *belt_zero_calibration_); + belt_prepare_up_torque_limit_, belt_prepare_down_ramp_ticks_, + belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, + belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, + *belt_zero_calibration_); } if (cmd == "fire") { @@ -644,6 +656,7 @@ class DartManagerV2 OutputInterface belt_target_velocity_; OutputInterface belt_torque_limit_; OutputInterface belt_hold_torque_; + OutputInterface belt_torque_offset_; OutputInterface belt_wait_zero_velocity_; OutputInterface belt_zero_calibration_; OutputInterface trigger_lock_enable_; @@ -669,7 +682,9 @@ class DartManagerV2 double belt_prepare_down_velocity_first_{5.0}; // rad/s double belt_prepare_down_velocity_{10.0}; // rad/s double belt_prepare_torque_limit_{5.0}; // N⋅m + double belt_prepare_up_torque_limit_{1.5}; // N⋅m uint64_t belt_prepare_down_ramp_ticks_{400}; // ticks + double belt_prepare_down_torque_offset_{2.0}; // N⋅m double belt_prepare_down_hold_torque_{5.0}; // N⋅m double belt_prepare_down_zero_velocity_threshold_{0.15}; // rad/s uint64_t belt_prepare_down_zero_confirm_ticks_{60}; // ticks @@ -711,4 +726,4 @@ class DartManagerV2 } // namespace rmcs_dart_guidance::manager #include -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::manager::DartManagerV2, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::manager::DartManager, rmcs_executor::Component) diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index a17c82f..e9f23f0 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -1,6 +1,8 @@ #pragma once +#include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" +#include "manager/action/belt_hold_torque_action.hpp" #include "manager/action/belt_move_action.hpp" #include "manager/action/belt_velocity_ramp_action.hpp" #include "manager/action/belt_zero_calibration.hpp" @@ -28,12 +30,12 @@ class CancelLaunchTask : public Task { CancelLaunchTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, double belt_down_distance, double belt_pulley_radius, - double down_velocity, double torque_limit, uint64_t down_ramp_ticks, + double down_velocity, double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, double down_hold_torque, double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, bool& belt_zero_calibration) @@ -62,72 +64,82 @@ class CancelLaunchTask : public Task { 0.20)); // 下行最大距离限制（m，正值） // 步骤2：下行速度斜坡减速到 0，成功后自动进入 HOLD_TORQUE 保持张力 - const double ramp_step_per_tick = - down_ramp_ticks > 0 ? (std::abs(down_velocity) / static_cast(down_ramp_ticks)) - : std::abs(down_velocity); then( std::make_shared( - "belt_down_ramp_to_zero", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_limit, // 力矩限幅（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT 模式选择（输出） - left_belt_velocity, // 左皮带速度反馈（输入） - right_belt_velocity, // 右皮带速度反馈（输入） - rmcs_msgs::DartSliderStatus::DOWN, // 下行方向 - down_velocity, // 斜坡初速度 - ramp_step_per_tick, // 每 tick 减速步长 - torque_limit, // 力矩限制 - down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 - down_zero_velocity_threshold, // 实测零速阈值 - down_zero_confirm_ticks, // 零速确认帧数 - down_ramp_timeout_ticks)); // 斜坡阶段超时 + "belt_down_ramp_to_zero", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 力矩限幅（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT 模式选择（输出） + left_belt_velocity, // 左皮带速度反馈（输入） + right_belt_velocity, // 右皮带速度反馈（输入） + down_ramp_ticks, // 目标减速时长（ticks） + torque_limit, // 力矩限制 + down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 + down_zero_velocity_threshold, // 实测零速阈值 + down_zero_confirm_ticks, // 零速确认帧数 + down_ramp_timeout_ticks, // 斜坡阶段超时 + down_velocity)); // 初始速度 - // 步骤3：解锁扳机 - then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000)); // 等待释放完成帧数 + // 步骤3：传送带保持高扭矩 + 解锁扳机 + 升降上行并行 + auto parallel_hold_unlock_and_lift = + std::make_shared("hold_unlock_and_lift", ActionSet::Policy::ALL_SUCCESS); + + parallel_hold_unlock_and_lift + ->also( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + 2.5, // 力矩偏移值（N⋅m）- 取消发射不需要偏移 + 2000)) // 保持时长（ticks） + .also( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 1000)) // 等待释放完成帧数 + .also( + std::make_shared( + "filling_lift_up", // 动作名称 + lifting_command, // 升降指令（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + 0.1, // 堵转速度阈值 + 100, // 堵转确认帧数 + 500, // 最短运行帧数 + 2000)); // 超时帧数 + then(parallel_hold_unlock_and_lift); // 步骤4：同步带上行复位到机械限位 then( std::make_shared( - "belt_reset", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 10, // 设定速度 - 1.0, // 设定力矩限制 - 0.5, // 设定保持力矩 - 10000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 - false)); // 超时返回失败 - - // 步骤5：填装升降上行回到初始位 - then( - std::make_shared( - "filling_lift_up", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - 0.1, // 堵转速度阈值 - 100, // 堵转确认帧数 - 500, // 最短运行帧数 - 2000)); // 超时帧数 + "belt_reset", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输入） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 10, // 设定速度 + up_torque_limit, // 设定力矩限制 + 0.5, // 设定保持力矩 + 10000, // 超时帧数 + 1.0, // 堵转速度阈值 + 0.5, // 堵转力矩阈值 + 100, // 堵转确认帧数 + 50, // 最短运行帧数 + BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 + false)); // 超时返回失败 then(std::make_shared("stabilize_wait", 50)); diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index 1f45137..b6ee3e0 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -1,7 +1,7 @@ #pragma once -#include "manager/action/filling_limit_servo_action.hpp" #include "manager/action/filling_lift_action.hpp" +#include "manager/action/filling_limit_servo_action.hpp" #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" @@ -29,30 +29,30 @@ class FireAndPreloadTask : public Task { then( std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000 // 等待释放完成帧数 + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 1000 // 等待释放完成帧数 )); then( std::make_shared( - "filling_lift_up", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 + "filling_lift_up", // 动作名称 + lifting_command, // 升降指令（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + lifting_stall_threshold, // 堵转速度阈值 + lifting_stall_confirm_ticks, // 堵转确认帧数 + lifting_stall_min_run_ticks, // 最短运行帧数 + lifting_stall_timeout_ticks // 超时帧数 )); then( std::make_shared( - limiting_command, // 限位舵机状态（输出） + limiting_command, // 限位舵机状态（输出） rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 rmcs_msgs::DartLimitingServoStatus::LOCK, // 再锁回 - preload_fill_ticks // 预装填持续帧数 + preload_fill_ticks // 预装填持续帧数 )); } }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index b6623f4..03f70c9 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -2,8 +2,9 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" +#include "manager/action/belt_hold_torque_action.hpp" #include "manager/action/belt_move_action.hpp" -#include "manager/action/belt_velocity_ramp_action.hpp" +#include "manager/action/belt_pid_deceleration_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -17,7 +18,6 @@ #include namespace rmcs_dart_guidance::manager { - // LaunchPreparationTask — 每次发射前的准备动作（传送带下行+扳机锁定） // 速度和扭矩限制由 manager 传入，不再硬编码 class LaunchPreparationTask : public Task { @@ -25,26 +25,18 @@ class LaunchPreparationTask : public Task { LaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, double belt_down_distance, double belt_pulley_radius, double down_velocity, - double torque_limit, uint64_t down_ramp_ticks, double down_hold_torque, - double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, - uint64_t down_ramp_timeout_ticks, bool require_lifting_down, - rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, bool& belt_zero_calibration) + double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, + double down_torque_offset, double down_hold_torque, double down_zero_velocity_threshold, + uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, + bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, + const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, + bool& belt_zero_calibration) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { - // 距离转角度：distance (m) = angle (rad) × radius (m) - // angle (rad) = distance (m) / radius (m) - // 约定：根据实际测量，下行=角度增大=正方向，上行=角度减小=负方向 - // 目标位置基于初始化后的实际角度计算，而非概念上的"零点" - - printf( - "[LaunchPreparationTask] belt_down_distance=%.4f m, belt_pulley_radius=%.4f m\n", - belt_down_distance, belt_pulley_radius); - // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） // 注意：target_distance为正值表示下行（角度增大方向） then( @@ -65,39 +57,41 @@ class LaunchPreparationTask : public Task { 100, // 最小运行帧数 0.5, // 位置到达容差（rad）- 增大容差 0.3, // 堵转速度阈值（rad/s） - 200, // 堵转确认帧数 - 0.20)); // 下行最大距离限制（m，正值） + 100, // 堵转确认帧数 + 0.60)); // 下行最大距离限制（m，正值） - // 步骤2：下行速度斜坡减速到 0，成功后自动进入 HOLD_TORQUE 保持张力 - const double ramp_step_per_tick = - down_ramp_ticks > 0 ? (std::abs(down_velocity) / static_cast(down_ramp_ticks)) - : std::abs(down_velocity); + // 步骤2：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） + // 必须等待速度真正降到零，否则保持阶段的常态力矩会导致疯转 then( - std::make_shared( - "belt_down_ramp_to_zero", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_limit, // 力矩限幅（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT 模式选择（输出） - left_belt_velocity, // 左皮带速度反馈（输入） - right_belt_velocity, // 右皮带速度反馈（输入） - rmcs_msgs::DartSliderStatus::DOWN, // 下行方向 - down_velocity, // 斜坡初速度 - ramp_step_per_tick, // 每 tick 减速步长 - torque_limit, // 力矩限制 - down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 - down_zero_velocity_threshold, // 实测零速阈值 - down_zero_confirm_ticks, // 零速确认帧数 - down_ramp_timeout_ticks)); // 斜坡阶段超时 + std::make_shared( + "belt_pid_deceleration", // 动作名称 + belt_target_velocity, // 目标速度（输出，设为0） + belt_torque_offset, // 力矩偏移（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + down_torque_offset, // 力矩偏移值（N⋅m） + down_zero_velocity_threshold, // 零速阈值（rad/s） + down_zero_confirm_ticks, // 零速确认帧数 + down_ramp_timeout_ticks)); // 超时帧数 if (require_lifting_down) { - // 步骤3（2-4发）：升降平台下行与扳机锁定并行 - auto parallel_lock_and_lift_down = - std::make_shared("lock_and_lift_down", ActionSet::Policy::ALL_SUCCESS); + // 步骤3（2-4发）：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 + auto parallel_hold_lock_and_lift = + std::make_shared("hold_lock_and_lift", ActionSet::Policy::ALL_SUCCESS); - parallel_lock_and_lift_down + parallel_hold_lock_and_lift ->also( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + down_torque_offset, // 力矩偏移值（N⋅m） + 1000)) // 保持时长（ticks） + .also( std::make_shared( trigger_lock_enable, // 扳机锁定使能（输出） true, // 锁定（true） @@ -112,40 +106,57 @@ class LaunchPreparationTask : public Task { 0.1, // 堵转速度阈值（rad/s） 100, // 堵转确认帧数 500, // 最短运行帧数 - 2000)); // 超时帧数 - then(parallel_lock_and_lift_down); + 1000)); // 超时帧数 + then(parallel_hold_lock_and_lift); } else { - then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 1000)); // 等待锁定完成帧数 + // 步骤3（首发）：传送带保持高扭矩 + 扳机锁定并行 + auto parallel_hold_and_lock = + std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); + + parallel_hold_and_lock + ->also( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + down_torque_offset, // 力矩偏移值（N⋅m） + 500)) // 保持时长（ticks） + .also( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 500)); // 等待锁定完成帧数 + then(parallel_hold_and_lock); } // 步骤4：传送带上行到机械限位（速度控制 + 堵转检测） then( std::make_shared( - "belt_reset_up", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 15, // 设定速度（rad/s） - 2.5, // 设定力矩限制（N⋅m） - 0.5, // 设定保持力矩（N⋅m） - 5000, // 超时帧数 - 0.5, // 堵转速度阈值（rad/s） - 0.8, // 堵转力矩阈值（N⋅m） - 100, // 堵转确认帧数 - 50, // 最短运行帧数 + "belt_reset_up", // 动作名称 + belt_command, // 同步带目标状态（输出） + belt_target_velocity, // 同步带目标速度（输出） + belt_torque_limit, // 同步带力矩限制（输出） + belt_hold_torque, // 同步带保持力矩（输出） + belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + left_belt_velocity, // 左同步带反馈（输入） + right_belt_velocity, // 右同步带反馈（输入） + left_belt_torque, // 左同步带力矩（输入） + right_belt_torque, // 右同步带力矩（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + 15, // 设定速度（rad/s） + up_torque_limit, // 设定力矩限制（N⋅m） + 0.5, // 设定保持力矩（N⋅m） + 5000, // 超时帧数 + 0.5, // 堵转速度阈值（rad/s） + 0.8, // 堵转力矩阈值（N⋅m） + 100, // 堵转确认帧数 + 50, // 最短运行帧数 BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 - false)); // 超时返回失败 + false)); // 超时返回失败 then(std::make_shared("stabilize_wait", 50)); diff --git a/src/manager/task/silder_init_task.hpp b/src/manager/task/silder_init_task.hpp index 3788189..b04fd2a 100644 --- a/src/manager/task/silder_init_task.hpp +++ b/src/manager/task/silder_init_task.hpp @@ -41,7 +41,7 @@ class SliderInitTask : public Task { 10, // 设定速度 1.0, // 设定力矩限制 0.5, // 设定保持力矩 - 5000, // 超时帧数（可配置） + 3000, // 超时帧数（可配置） 1.0, // 堵转速度阈值（rad/s） 0.5, // 堵转力矩阈值（N⋅m） 100, // 堵转确认帧数 diff --git a/src/manager/task/vision_assisted_launch_preparation_task.hpp b/src/manager/task/vision_assisted_launch_preparation_task.hpp index 8a8a391..15ff40e 100644 --- a/src/manager/task/vision_assisted_launch_preparation_task.hpp +++ b/src/manager/task/vision_assisted_launch_preparation_task.hpp @@ -22,27 +22,37 @@ class VisionAssistedLaunchPreparationTask : public Task { VisionAssistedLaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, + double& belt_torque_offset, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, - rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, double lifting_stall_threshold, - uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, - uint64_t lifting_stall_timeout_ticks, LaunchPreparationTask::Mode mode, - Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, - Eigen::Vector2d& desired_target_px_output, const cv::Point2i& target_position, - const bool& target_tracking, const rclcpp::Logger& logger, AutoAimParams auto_aim_params) + double belt_down_distance, double belt_pulley_radius, double down_velocity, + double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, + double down_torque_offset, double down_hold_torque, double down_zero_velocity_threshold, + uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, + bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, + const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, + bool& belt_zero_calibration, Eigen::Vector2d& yaw_pitch_control_velocity, + bool& aim_ready, Eigen::Vector2d& aim_error_px, Eigen::Vector2d& desired_target_px_output, + const cv::Point2i& target_position, const bool& target_tracking, + const rclcpp::Logger& logger, AutoAimParams auto_aim_params) : Task("launch_preparation", "视觉辅助滑块发射准备") { - then(std::make_shared( - yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, - target_position, target_tracking, logger, std::move(auto_aim_params))); - - then(std::make_shared( - belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, - belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, left_belt_torque, - right_belt_torque, trigger_lock_enable, lifting_command, lifting_left_vel_fb, - lifting_right_vel_fb, lifting_stall_threshold, lifting_stall_confirm_ticks, - lifting_stall_min_run_ticks, lifting_stall_timeout_ticks, mode)); + then( + std::make_shared( + yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, + target_position, target_tracking, logger, std::move(auto_aim_params))); + + then( + std::make_shared( + belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, + belt_wait_zero_velocity, belt_torque_offset, left_belt_angle, right_belt_angle, + left_belt_velocity, right_belt_velocity, left_belt_torque, right_belt_torque, + trigger_lock_enable, belt_down_distance, belt_pulley_radius, down_velocity, + torque_limit, up_torque_limit, down_ramp_ticks, down_torque_offset, + down_hold_torque, down_zero_velocity_threshold, down_zero_confirm_ticks, + down_ramp_timeout_ticks, require_lifting_down, lifting_command, + lifting_left_vel_fb, lifting_right_vel_fb, belt_zero_calibration)); } }; diff --git a/tests/launch_preparation_task_test.cpp b/tests/launch_preparation_task_test.cpp index bbb18b2..3713169 100644 --- a/tests/launch_preparation_task_test.cpp +++ b/tests/launch_preparation_task_test.cpp @@ -1,80 +1,80 @@ -#include "manager/task/launch_preparation_task.hpp" - -#include - -#include - -namespace rmcs_dart_guidance::manager::test { - -class LaunchPreparationTaskTest : public ::testing::Test { -protected: - struct Harness { - explicit Harness(LaunchPreparationTask::Mode mode) - : task( - belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, - belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, - left_belt_torque, right_belt_torque, trigger_lock_enable, lifting_command, - lifting_left_vel_fb, lifting_right_vel_fb, 1.0, 1, 0, 2000, mode) {} - - rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; - double belt_target_velocity{0.0}; - double belt_torque_limit{0.0}; - double belt_hold_torque{0.0}; - bool belt_wait_zero_velocity{false}; - double left_belt_velocity{0.0}; - double right_belt_velocity{0.0}; - double left_belt_torque{0.0}; - double right_belt_torque{0.0}; - bool trigger_lock_enable{false}; - - rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; - double lifting_left_vel_fb{0.0}; - double lifting_right_vel_fb{0.0}; - - LaunchPreparationTask task; - }; -}; - -TEST_F(LaunchPreparationTaskTest, FirstFillStartsWithLiftReset) { - Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); - harness.lifting_left_vel_fb = 2.0; - harness.lifting_right_vel_fb = 2.0; - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 0.0); - EXPECT_FALSE(harness.trigger_lock_enable); -} - -TEST_F(LaunchPreparationTaskTest, FirstFillMovesBeltDownAfterLiftResetCompletes) { - Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); - harness.lifting_left_vel_fb = 2.0; - harness.lifting_right_vel_fb = 2.0; - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); - - harness.lifting_left_vel_fb = 0.0; - harness.lifting_right_vel_fb = 0.0; - EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - - EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); - EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); -} - -TEST_F(LaunchPreparationTaskTest, NormalModeStartsWithBeltMove) { - Harness harness(LaunchPreparationTask::Mode::NORMAL); - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); - EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); - EXPECT_FALSE(harness.trigger_lock_enable); -} - -} // namespace rmcs_dart_guidance::manager::test +// #include "manager/task/launch_preparation_task.hpp" + +// #include + +// #include + +// namespace rmcs_dart_guidance::manager::test { + +// class LaunchPreparationTaskTest : public ::testing::Test { +// protected: +// struct Harness { +// explicit Harness(LaunchPreparationTask::Mode mode) +// : task( +// belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, +// belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, +// left_belt_torque, right_belt_torque, trigger_lock_enable, lifting_command, +// lifting_left_vel_fb, lifting_right_vel_fb, 1.0, 1, 0, 2000, mode) {} + +// rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; +// double belt_target_velocity{0.0}; +// double belt_torque_limit{0.0}; +// double belt_hold_torque{0.0}; +// bool belt_wait_zero_velocity{false}; +// double left_belt_velocity{0.0}; +// double right_belt_velocity{0.0}; +// double left_belt_torque{0.0}; +// double right_belt_torque{0.0}; +// bool trigger_lock_enable{false}; + +// rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; +// double lifting_left_vel_fb{0.0}; +// double lifting_right_vel_fb{0.0}; + +// LaunchPreparationTask task; +// }; +// }; + +// TEST_F(LaunchPreparationTaskTest, FirstFillStartsWithLiftReset) { +// Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); +// harness.lifting_left_vel_fb = 2.0; +// harness.lifting_right_vel_fb = 2.0; + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); +// EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 0.0); +// EXPECT_FALSE(harness.trigger_lock_enable); +// } + +// TEST_F(LaunchPreparationTaskTest, FirstFillMovesBeltDownAfterLiftResetCompletes) { +// Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); +// harness.lifting_left_vel_fb = 2.0; +// harness.lifting_right_vel_fb = 2.0; + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); + +// harness.lifting_left_vel_fb = 0.0; +// harness.lifting_right_vel_fb = 0.0; +// EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + +// EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); +// EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); +// } + +// TEST_F(LaunchPreparationTaskTest, NormalModeStartsWithBeltMove) { +// Harness harness(LaunchPreparationTask::Mode::NORMAL); + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); +// EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); +// EXPECT_FALSE(harness.trigger_lock_enable); +// } + +// } // namespace rmcs_dart_guidance::manager::test diff --git a/tests/vision_assisted_launch_preparation_task_test.cpp b/tests/vision_assisted_launch_preparation_task_test.cpp index 0d2506b..516540b 100644 --- a/tests/vision_assisted_launch_preparation_task_test.cpp +++ b/tests/vision_assisted_launch_preparation_task_test.cpp @@ -1,125 +1,131 @@ -#include "manager/task/vision_assisted_launch_preparation_task.hpp" - -#include - -#include - -#include -#include -#include -#include - -namespace rmcs_dart_guidance::manager::test { - -class VisionAssistedLaunchPreparationTaskTest : public ::testing::Test { -protected: - static void SetUpTestSuite() { - if (!rclcpp::ok()) { - ::setenv("ROS_LOG_DIR", "/tmp/rmcs_dart_guidance_test_logs", 1); - int argc = 0; - char** argv = nullptr; - rclcpp::init(argc, argv); - } - } - - static void TearDownTestSuite() { - if (rclcpp::ok()) { - rclcpp::shutdown(); - } - } - - static AutoAimParams make_auto_aim_params() { - AutoAimParams params; - params.desired_target_px = Eigen::Vector2d(100.0, 100.0); - params.deadband_px = Eigen::Vector2d::Constant(2.0); - params.ready_exit_deadband_px = Eigen::Vector2d::Constant(3.0); - params.accept_deadband_px = Eigen::Vector2d::Constant(6.0); - params.ready_confirm_ticks = 1; - params.timeout_ticks = 2; - params.yaw_gain = 0.1; - params.pitch_gain = 0.1; - params.max_transform_rate = 10.0; - return params; - } - - struct Harness { - explicit Harness(LaunchPreparationTask::Mode mode) - : task( - belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, - belt_wait_zero_velocity, left_belt_velocity, right_belt_velocity, - left_belt_torque, right_belt_torque, trigger_lock_enable, lifting_command, - lifting_left_vel_fb, lifting_right_vel_fb, 1.0, 1, 0, 2000, mode, - yaw_pitch_control_velocity, aim_ready, aim_error_px, desired_target_px_output, - target_position, target_tracking, - rclcpp::get_logger("vision_assisted_launch_preparation_task_test"), - make_auto_aim_params()) {} - - rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; - double belt_target_velocity{0.0}; - double belt_torque_limit{0.0}; - double belt_hold_torque{0.0}; - bool belt_wait_zero_velocity{false}; - double left_belt_velocity{0.0}; - double right_belt_velocity{0.0}; - double left_belt_torque{0.0}; - double right_belt_torque{0.0}; - bool trigger_lock_enable{false}; - - rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; - double lifting_left_vel_fb{0.0}; - double lifting_right_vel_fb{0.0}; - - Eigen::Vector2d yaw_pitch_control_velocity{Eigen::Vector2d::Zero()}; - bool aim_ready{false}; - Eigen::Vector2d aim_error_px{Eigen::Vector2d::Zero()}; - Eigen::Vector2d desired_target_px_output{Eigen::Vector2d::Zero()}; - cv::Point2i target_position{-1, -1}; - bool target_tracking{false}; - - VisionAssistedLaunchPreparationTask task; - }; -}; - -TEST_F(VisionAssistedLaunchPreparationTaskTest, StartsWithAutoAimBeforeMechanicalSequence) { - Harness harness(LaunchPreparationTask::Mode::NORMAL); - harness.target_tracking = true; - harness.target_position = cv::Point2i(90, 100); - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); - EXPECT_GT(harness.yaw_pitch_control_velocity.x(), 0.0); - EXPECT_FALSE(harness.aim_ready); -} - -TEST_F(VisionAssistedLaunchPreparationTaskTest, TimeoutContinuesIntoMechanicalSequence) { - Harness harness(LaunchPreparationTask::Mode::NORMAL); - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - - EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - - EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); - EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); -} - -TEST_F(VisionAssistedLaunchPreparationTaskTest, FirstFillModePreservesLiftResetAfterAim) { - Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); - harness.target_tracking = true; - harness.target_position = cv::Point2i(100, 100); - - EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); - EXPECT_TRUE(harness.aim_ready); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); - - harness.lifting_left_vel_fb = 2.0; - harness.lifting_right_vel_fb = 2.0; - EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); - EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); - EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); -} - -} // namespace rmcs_dart_guidance::manager::test +// #include "manager/task/vision_assisted_launch_preparation_task.hpp" + +// #include + +// #include + +// #include +// #include +// #include +// #include + +// namespace rmcs_dart_guidance::manager::test { + +// class VisionAssistedLaunchPreparationTaskTest : public ::testing::Test { +// protected: +// static void SetUpTestSuite() { +// if (!rclcpp::ok()) { +// ::setenv("ROS_LOG_DIR", "/tmp/rmcs_dart_guidance_test_logs", 1); +// int argc = 0; +// char** argv = nullptr; +// rclcpp::init(argc, argv); +// } +// } + +// static void TearDownTestSuite() { +// if (rclcpp::ok()) { +// rclcpp::shutdown(); +// } +// } + +// static AutoAimParams make_auto_aim_params() { +// AutoAimParams params; +// params.desired_target_px = Eigen::Vector2d(100.0, 100.0); +// params.deadband_px = Eigen::Vector2d::Constant(2.0); +// params.ready_exit_deadband_px = Eigen::Vector2d::Constant(3.0); +// params.accept_deadband_px = Eigen::Vector2d::Constant(6.0); +// params.ready_confirm_ticks = 1; +// params.timeout_ticks = 2; +// params.yaw_gain = 0.1; +// params.pitch_gain = 0.1; +// params.max_transform_rate = 10.0; +// return params; +// } + +// struct Harness { +// explicit Harness(bool require_lifting_down) +// : task( +// belt_command, belt_target_velocity, belt_torque_limit, belt_hold_torque, +// belt_wait_zero_velocity, belt_torque_offset, left_belt_angle, right_belt_angle, +// left_belt_velocity, right_belt_velocity, left_belt_torque, right_belt_torque, +// trigger_lock_enable, 0.15, 0.0195, 10.0, 5.0, 1.5, 400, 2.0, 5.0, 0.15, 60, +// 5000, require_lifting_down, lifting_command, lifting_left_vel_fb, +// lifting_right_vel_fb, belt_zero_calibration, yaw_pitch_control_velocity, +// aim_ready, aim_error_px, desired_target_px_output, target_position, +// target_tracking, +// rclcpp::get_logger("vision_assisted_launch_preparation_task_test"), +// make_auto_aim_params()) {} + +// rmcs_msgs::DartSliderStatus belt_command{rmcs_msgs::DartSliderStatus::WAIT}; +// double belt_target_velocity{0.0}; +// double belt_torque_limit{0.0}; +// double belt_hold_torque{0.0}; +// bool belt_wait_zero_velocity{false}; +// double belt_torque_offset{0.0}; +// double left_belt_angle{0.0}; +// double right_belt_angle{0.0}; +// double left_belt_velocity{0.0}; +// double right_belt_velocity{0.0}; +// double left_belt_torque{0.0}; +// double right_belt_torque{0.0}; +// bool trigger_lock_enable{false}; +// bool belt_zero_calibration{false}; + +// rmcs_msgs::DartSliderStatus lifting_command{rmcs_msgs::DartSliderStatus::WAIT}; +// double lifting_left_vel_fb{0.0}; +// double lifting_right_vel_fb{0.0}; + +// Eigen::Vector2d yaw_pitch_control_velocity{Eigen::Vector2d::Zero()}; +// bool aim_ready{false}; +// Eigen::Vector2d aim_error_px{Eigen::Vector2d::Zero()}; +// Eigen::Vector2d desired_target_px_output{Eigen::Vector2d::Zero()}; +// cv::Point2i target_position{-1, -1}; +// bool target_tracking{false}; + +// VisionAssistedLaunchPreparationTask task; +// }; +// }; + +// TEST_F(VisionAssistedLaunchPreparationTaskTest, StartsWithAutoAimBeforeMechanicalSequence) { +// Harness harness(false); +// harness.target_tracking = true; +// harness.target_position = cv::Point2i(90, 100); + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::WAIT); +// EXPECT_GT(harness.yaw_pitch_control_velocity.x(), 0.0); +// EXPECT_FALSE(harness.aim_ready); +// } + +// TEST_F(VisionAssistedLaunchPreparationTaskTest, TimeoutContinuesIntoMechanicalSequence) { +// Harness harness(false); + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + +// EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + +// EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::DOWN); +// EXPECT_DOUBLE_EQ(harness.belt_target_velocity, 10.0); +// } + +// TEST_F(VisionAssistedLaunchPreparationTaskTest, FirstFillModePreservesLiftResetAfterAim) { +// Harness harness(LaunchPreparationTask::Mode::FIRST_FILL); +// harness.target_tracking = true; +// harness.target_position = cv::Point2i(100, 100); + +// EXPECT_EQ(harness.task.tick_first(), ActionStatus::RUNNING); +// EXPECT_TRUE(harness.aim_ready); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); + +// harness.lifting_left_vel_fb = 2.0; +// harness.lifting_right_vel_fb = 2.0; +// EXPECT_EQ(harness.task.tick(), ActionStatus::RUNNING); +// EXPECT_EQ(harness.lifting_command, rmcs_msgs::DartSliderStatus::UP); +// EXPECT_EQ(harness.belt_command, rmcs_msgs::DartSliderStatus::WAIT); +// } + +// } // namespace rmcs_dart_guidance::manager::test From 2949f127406d3162841795f452cafaac332a1359 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Wed, 1 Apr 2026 18:30:23 +0800 Subject: [PATCH 07/19] done all contorl of the first fire --- .../belt_constant_velocity_move_action.hpp | 81 +++++--------- .../belt_deceleration_with_stall_action.hpp | 98 +++++++++++++++++ .../action/belt_pid_deceleration_action.hpp | 25 ++--- src/manager/action/manual_angle_control.hpp | 9 +- src/manager/action/manual_force_control.hpp | 10 +- src/manager/command.cpp | 4 - src/manager/dart_manager.cpp | 104 ++++++++++++++---- src/manager/task/fire_and_preload_task.hpp | 68 +++++++----- src/manager/task/launch_preparation_task.hpp | 98 ++++++++++++----- ...ision_assisted_launch_preparation_task.hpp | 22 ++-- 10 files changed, 349 insertions(+), 170 deletions(-) create mode 100644 src/manager/action/belt_deceleration_with_stall_action.hpp diff --git a/src/manager/action/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp index 5b2486c..b9200d3 100644 --- a/src/manager/action/belt_constant_velocity_move_action.hpp +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -21,24 +21,17 @@ namespace rmcs_dart_guidance::manager { class BeltConstantVelocityMoveAction : public IAction { public: BeltConstantVelocityMoveAction( - std::string name, - rmcs_msgs::DartSliderStatus& belt_command, - double& belt_target_velocity, - double& belt_torque_limit, - const double& left_belt_angle, - const double& right_belt_angle, - const double& left_belt_velocity, - const double& right_belt_velocity, + std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_torque_limit, const double& left_belt_angle, const double& right_belt_angle, + const double& left_belt_velocity, const double& right_belt_velocity, double target_distance, // 目标距离（m，正=上行，负=下行） double pulley_radius, // 滑轮半径（m） double velocity, // 运动速度（rad/s） double torque_limit, // 扭矩限制（N⋅m） - uint64_t timeout_ticks, - uint64_t min_running_ticks = 50, + uint64_t timeout_ticks, uint64_t min_running_ticks = 50, double position_tolerance = 0.01, // 位置到达容差（rad） - double stall_velocity_threshold = 0.3, - uint64_t stall_confirm_ticks = 200, - double max_down_distance = 0.20) // 下行最大距离限制（m，正值，防止过度下行） + double stall_velocity_threshold = 0.3, uint64_t stall_confirm_ticks = 200, + double max_down_distance = 0.60) // 下行最大距离限制（m，正值，防止过度下行） : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -73,16 +66,10 @@ class BeltConstantVelocityMoveAction : public IAction { // 下行最大距离限制检查（防止过度下行） // 将最大下行距离（m）换算为角度偏移（rad） - double max_down_angle_offset = -max_down_distance_ / pulley_radius_; // 负值表示下行 + double max_down_angle_offset = -max_down_distance_ / pulley_radius_; // 负值表示下行 double max_down_position = initial_angle_ + max_down_angle_offset; if (target_position_ < max_down_position) { - printf("[%s] ERROR: target_position=%.4f exceeds max_down_position=%.4f\n", - name().c_str(), target_position_, max_down_position); - printf("[%s] target_distance=%.4f m, max_down_distance=%.4f m\n", - name().c_str(), target_distance_, max_down_distance_); - printf("[%s] Limiting target to max_down_position=%.4f\n", - name().c_str(), max_down_position); target_position_ = max_down_position; } @@ -100,31 +87,26 @@ class BeltConstantVelocityMoveAction : public IAction { } belt_torque_limit_ = torque_limit_; - - printf("[%s] on_enter: initial_angle=%.4f, target_distance=%.4f m, target_angle_offset=%.4f rad\n", - name().c_str(), initial_angle_, target_distance_, target_angle_offset); - printf("[%s] target_position=%.4f, velocity=%.4f, direction=%s\n", - name().c_str(), target_position_, belt_target_velocity_, - (distance_to_target > 0) ? "DOWN" : "UP"); } ActionStatus update() override { if (elapsed_ticks() >= timeout_ticks_) { - printf("[%s] TIMEOUT after %lu ticks\n", name().c_str(), elapsed_ticks()); return ActionStatus::FAILURE; } double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; - double position_error = target_position_ - avg_angle; // 保留符号，用于判断方向 - double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - - // 每100帧打印一次调试信息 - if (elapsed_ticks() % 100 == 0) { - printf("[%s] tick=%lu, current=%.4f, target=%.4f, error=%.4f, vel=%.4f\n", - name().c_str(), elapsed_ticks(), avg_angle, target_position_, - position_error, avg_velocity); - printf("[%s] left_angle=%.4f, right_angle=%.4f, initial=%.4f\n", - name().c_str(), left_belt_angle_, right_belt_angle_, initial_angle_); + double position_error = target_position_ - avg_angle; // 保留符号，用于判断方向 + double avg_velocity = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + + // 上行速度切换逻辑：在0.2m处从5.0切换到15.0 + if (target_distance_ < 0) { // 上行（负距离） + double distance_traveled = std::abs(avg_angle - initial_angle_) * pulley_radius_; + if (distance_traveled < 0.2) { + belt_target_velocity_ = 5.0; // 前0.2m慢速 + } else { + belt_target_velocity_ = 15.0; // 0.2m后快速 + } } // 位置到达判断（优先级最高） @@ -148,8 +130,6 @@ class BeltConstantVelocityMoveAction : public IAction { } if (within_tolerance || overshot) { - printf("[%s] SUCCESS: position reached (error=%.4f, tolerance=%.4f, overshot=%d)\n", - name().c_str(), position_error, position_tolerance_, overshot); return ActionStatus::SUCCESS; } } @@ -159,8 +139,6 @@ class BeltConstantVelocityMoveAction : public IAction { if (avg_velocity < stall_velocity_threshold_) { ++stall_counter_; if (stall_counter_ >= stall_confirm_ticks_) { - printf("[%s] FAILURE: stalled (vel=%.4f < threshold=%.4f for %lu ticks)\n", - name().c_str(), avg_velocity, stall_velocity_threshold_, stall_counter_); return ActionStatus::FAILURE; } } else { @@ -171,10 +149,7 @@ class BeltConstantVelocityMoveAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override { - // 不在这里停止运动，让下一个 action 接管控制 - // 这样可以避免速度突变 - } + void on_exit() override {} private: rmcs_msgs::DartSliderStatus& belt_command_; @@ -185,19 +160,19 @@ class BeltConstantVelocityMoveAction : public IAction { const double& left_belt_velocity_; const double& right_belt_velocity_; - double target_distance_; // 目标距离（m） - double pulley_radius_; // 滑轮半径（m） - double velocity_; // 运动速度（rad/s） - double torque_limit_; // 扭矩限制（N⋅m） + double target_distance_; // 目标距离（m） + double pulley_radius_; // 滑轮半径（m） + double velocity_; // 运动速度（rad/s） + double torque_limit_; // 扭矩限制（N⋅m） uint64_t timeout_ticks_; uint64_t min_running_ticks_; - double position_tolerance_; // 位置到达容差（rad） + double position_tolerance_; // 位置到达容差（rad） double stall_velocity_threshold_; uint64_t stall_confirm_ticks_; - double max_down_distance_; // 下行最大距离限制（m，正值） + double max_down_distance_; // 下行最大距离限制（m，正值） - double initial_angle_{0.0}; // 初始角度（rad，在on_enter中读取） - double target_position_{0.0}; // 实际目标位置（rad，在on_enter中计算） + double initial_angle_{0.0}; // 初始角度（rad，在on_enter中读取） + double target_position_{0.0}; // 实际目标位置（rad，在on_enter中计算） uint64_t stall_counter_{0}; }; diff --git a/src/manager/action/belt_deceleration_with_stall_action.hpp b/src/manager/action/belt_deceleration_with_stall_action.hpp new file mode 100644 index 0000000..fc544e7 --- /dev/null +++ b/src/manager/action/belt_deceleration_with_stall_action.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include "action.hpp" +#include +#include + +namespace rmcs_dart_guidance::manager { + +// BeltDecelerationWithStallAction - 减速并监测堵转 +// 用于上行最后阶段：减速到目标速度，监测堵转作为成功标志 +// 与 BeltPIDDecelerationAction 的区别： +// - 目标速度可配置（不一定是0） +// - 堵转检测返回 SUCCESS（而不是 FAILURE） +class BeltDecelerationWithStallAction : public IAction { +public: + BeltDecelerationWithStallAction( + std::string name, double& belt_target_velocity, double& belt_torque_offset, + const double& left_belt_velocity, const double& right_belt_velocity, + const double& left_belt_torque, const double& right_belt_torque, + double target_velocity, // 目标速度（rad/s） + double torque_offset_value, // 力矩偏移（N⋅m） + double stall_velocity_threshold, // 堵转速度阈值（rad/s） + double stall_torque_threshold, // 堵转扭矩阈值（N⋅m） + uint64_t stall_confirm_ticks, // 堵转确认帧数 + uint64_t min_running_ticks, // 最小运行帧数 + uint64_t timeout_ticks // 超时帧数 + ) + : IAction(std::move(name)) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_offset_(belt_torque_offset) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , left_belt_torque_(left_belt_torque) + , right_belt_torque_(right_belt_torque) + , target_velocity_(target_velocity) + , torque_offset_value_(torque_offset_value) + , stall_velocity_threshold_(stall_velocity_threshold) + , stall_torque_threshold_(stall_torque_threshold) + , stall_confirm_ticks_(stall_confirm_ticks) + , min_running_ticks_(min_running_ticks) + , timeout_ticks_(timeout_ticks) {} + + void on_enter() override { + belt_target_velocity_ = target_velocity_; + belt_torque_offset_ = torque_offset_value_; + stall_counter_ = 0; + printf( + "[%s] on_enter: target_velocity=%.4f, torque_offset=%.4f\n", name().c_str(), + target_velocity_, torque_offset_value_); + } + + ActionStatus update() override { + // 超时检测 + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::FAILURE; + } + + // 计算平均速度和扭矩 + double avg_velocity = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + bool torque_active = std::abs(left_belt_torque_) > stall_torque_threshold_ + || std::abs(right_belt_torque_) > stall_torque_threshold_; + + // 堵转检测（最小运行帧数后才开始检测）：速度低且扭矩高 + if (elapsed_ticks() > min_running_ticks_) { + if (avg_velocity < stall_velocity_threshold_ && torque_active) { + ++stall_counter_; + if (stall_counter_ >= stall_confirm_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { belt_torque_offset_ = 0.0; } + +private: + double& belt_target_velocity_; + double& belt_torque_offset_; + const double& left_belt_velocity_; + const double& right_belt_velocity_; + const double& left_belt_torque_; + const double& right_belt_torque_; + double target_velocity_; + double torque_offset_value_; + double stall_velocity_threshold_; + double stall_torque_threshold_; + uint64_t stall_confirm_ticks_; + uint64_t min_running_ticks_; + uint64_t timeout_ticks_; + uint64_t stall_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_pid_deceleration_action.hpp b/src/manager/action/belt_pid_deceleration_action.hpp index a93af24..b52bcef 100644 --- a/src/manager/action/belt_pid_deceleration_action.hpp +++ b/src/manager/action/belt_pid_deceleration_action.hpp @@ -1,7 +1,6 @@ #pragma once #include "action.hpp" -#include #include #include @@ -10,16 +9,10 @@ namespace rmcs_dart_guidance::manager { class BeltPIDDecelerationAction : public IAction { public: BeltPIDDecelerationAction( - std::string name, - double& belt_target_velocity, - double& belt_torque_offset, - const double& left_belt_velocity, - const double& right_belt_velocity, - double torque_offset_value, - double zero_velocity_threshold, - uint64_t zero_confirm_ticks, - uint64_t timeout_ticks - ) + std::string name, double& belt_target_velocity, double& belt_torque_offset, + const double& left_belt_velocity, const double& right_belt_velocity, + double torque_offset_value, double zero_velocity_threshold, uint64_t zero_confirm_ticks, + uint64_t timeout_ticks) : IAction(std::move(name)) , belt_target_velocity_(belt_target_velocity) , belt_torque_offset_(belt_torque_offset) @@ -28,8 +21,7 @@ class BeltPIDDecelerationAction : public IAction { , torque_offset_value_(torque_offset_value) , zero_velocity_threshold_(zero_velocity_threshold) , zero_confirm_ticks_(zero_confirm_ticks) - , timeout_ticks_(timeout_ticks) - {} + , timeout_ticks_(timeout_ticks) {} void on_enter() override { belt_target_velocity_ = 0.0; @@ -44,7 +36,8 @@ class BeltPIDDecelerationAction : public IAction { } // 计算平均速度 - double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + double avg_velocity = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; // 零速检测 if (avg_velocity < zero_velocity_threshold_) { @@ -59,9 +52,7 @@ class BeltPIDDecelerationAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override { - belt_torque_offset_ = 0.0; - } + void on_exit() override { belt_torque_offset_ = 0.0; } private: double& belt_target_velocity_; diff --git a/src/manager/action/manual_angle_control.hpp b/src/manager/action/manual_angle_control.hpp index d4b6959..e4a6117 100644 --- a/src/manager/action/manual_angle_control.hpp +++ b/src/manager/action/manual_angle_control.hpp @@ -2,7 +2,6 @@ #include "action.hpp" -#include #include namespace rmcs_dart_guidance::manager { @@ -12,10 +11,8 @@ namespace rmcs_dart_guidance::manager { class DartManualAngleControlAction : public IAction { public: DartManualAngleControlAction( - double& yaw_control_velocity, - double& pitch_control_velocity, - const Eigen::Vector2d& joystick_left, - const Eigen::Vector2d& joystick_right, + double& yaw_control_velocity, double& pitch_control_velocity, + const Eigen::Vector2d& joystick_left, const Eigen::Vector2d& joystick_right, double max_transform_rate) : IAction("dart_manual_angle_control") , yaw_control_velocity_(yaw_control_velocity) @@ -27,7 +24,7 @@ class DartManualAngleControlAction : public IAction { void on_enter() override {} ActionStatus update() override { - yaw_control_velocity_ = joystick_left_.y() * max_transform_rate_; + yaw_control_velocity_ = joystick_left_.y() * max_transform_rate_; pitch_control_velocity_ = joystick_right_.x() * max_transform_rate_; return ActionStatus::RUNNING; diff --git a/src/manager/action/manual_force_control.hpp b/src/manager/action/manual_force_control.hpp index dfb6322..e7679fa 100644 --- a/src/manager/action/manual_force_control.hpp +++ b/src/manager/action/manual_force_control.hpp @@ -2,7 +2,6 @@ #include "action.hpp" -#include #include namespace rmcs_dart_guidance::manager { @@ -12,10 +11,8 @@ namespace rmcs_dart_guidance::manager { class DartManualForceControlAction : public IAction { public: DartManualForceControlAction( - double& force_control_velocity, - const Eigen::Vector2d& joystick_right, - double max_transform_rate, - double manual_force_scale = 5.0) + double& force_control_velocity, const Eigen::Vector2d& joystick_right, + double max_transform_rate, double manual_force_scale = 5.0) : IAction("dart_manual_force_control") , force_control_velocity_(force_control_velocity) , joystick_right_(joystick_right) @@ -25,8 +22,7 @@ class DartManualForceControlAction : public IAction { void on_enter() override {} ActionStatus update() override { - force_control_velocity_ = - joystick_right_.x() * max_transform_rate_ * manual_force_scale_; + force_control_velocity_ = joystick_right_.x() * max_transform_rate_ * manual_force_scale_; return ActionStatus::RUNNING; } diff --git a/src/manager/command.cpp b/src/manager/command.cpp index 3d4744d..d0f07e5 100644 --- a/src/manager/command.cpp +++ b/src/manager/command.cpp @@ -1,4 +1,3 @@ -#include #include #include @@ -87,9 +86,6 @@ class RemoteCommandBridge RCLCPP_INFO(logger_, "[RemoteCommandBridge] toggle -> unload"); } else { emit_command("launch_prepare"); - - // RCLCPP_INFO(logger_,"cmd ： launch_prepare"); - chambered_ = true; RCLCPP_INFO(logger_, "[RemoteCommandBridge] toggle -> launch_prepare"); } diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 2ac9c12..66d80d4 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -179,6 +179,53 @@ class DartManager belt_prepare_down_ramp_timeout_ticks_ = 2000; } + // 加载上行控制参数 + try { + belt_up_distance_ = get_parameter("belt_up_distance").as_double(); + } catch (...) { + belt_up_distance_ = 0.65; + } + try { + belt_prepare_up_velocity_ = get_parameter("belt_prepare_up_velocity").as_double(); + } catch (...) { + belt_prepare_up_velocity_ = 15.0; + } + try { + belt_up_decel_target_velocity_ = + get_parameter("belt_up_decel_target_velocity").as_double(); + } catch (...) { + belt_up_decel_target_velocity_ = 1.0; + } + try { + belt_up_decel_torque_offset_ = get_parameter("belt_up_decel_torque_offset").as_double(); + } catch (...) { + belt_up_decel_torque_offset_ = 0.0; + } + try { + belt_up_stall_velocity_threshold_ = + get_parameter("belt_up_stall_velocity_threshold").as_double(); + } catch (...) { + belt_up_stall_velocity_threshold_ = 0.15; + } + try { + belt_up_stall_confirm_ticks_ = + (uint64_t)get_parameter("belt_up_stall_confirm_ticks").as_int(); + } catch (...) { + belt_up_stall_confirm_ticks_ = 100; + } + try { + belt_up_stall_min_run_ticks_ = + (uint64_t)get_parameter("belt_up_stall_min_run_ticks").as_int(); + } catch (...) { + belt_up_stall_min_run_ticks_ = 300; + } + try { + belt_up_decel_timeout_ticks_ = + (uint64_t)get_parameter("belt_up_decel_timeout_ticks").as_int(); + } catch (...) { + belt_up_decel_timeout_ticks_ = 3000; + } + lifting_stall_threshold_ = get_parameter("lifting_stall_threshold").as_double(); lifting_stall_confirm_ticks_ = (uint64_t)get_parameter("lifting_stall_confirm_ticks").as_int(); @@ -192,7 +239,7 @@ class DartManager sync_current_dart_outputs(); clear_auto_aim_feedback(); submit_task(make_slider_init_task()); - RCLCPP_INFO(logger_, "[DartManagerV2] initialized, queued SliderInitTask on startup"); + RCLCPP_INFO(logger_, "[DartManager] initialized, queued SliderInitTask on startup"); } void update() override { @@ -232,11 +279,11 @@ class DartManager recover(); } else { auto task = make_task(cmd); - RCLCPP_INFO(logger_, "[DartManagerV2] received command: '%s'", cmd.c_str()); + RCLCPP_INFO(logger_, "[DartManager] received command: '%s'", cmd.c_str()); if (task) { submit_task(std::move(task)); } else { - RCLCPP_WARN(logger_, "[DartManagerV2] unknown command: '%s'", cmd.c_str()); + RCLCPP_WARN(logger_, "[DartManager] unknown command: '%s'", cmd.c_str()); } } } @@ -244,7 +291,7 @@ class DartManager void submit_task(std::shared_ptr task) { task_queue_.push_back(std::move(task)); RCLCPP_INFO( - logger_, "[DartManagerV2] task queued: %s (queue size=%zu)", + logger_, "[DartManager] task queued: %s (queue size=%zu)", task_queue_.back()->name().c_str(), task_queue_.size()); } @@ -253,7 +300,7 @@ class DartManager if (current_task_) { current_task_->cancel(); current_task_.reset(); - RCLCPP_WARN(logger_, "[DartManagerV2] all tasks cancelled"); + RCLCPP_WARN(logger_, "[DartManager] all tasks cancelled"); } first_fill_pending_ = true; @@ -272,7 +319,7 @@ class DartManager if (state_ == State::ERROR) { current_task_.reset(); task_queue_.clear(); - RCLCPP_INFO(logger_, "[DartManagerV2] recovered from ERROR, state=IDLE"); + RCLCPP_INFO(logger_, "[DartManager] recovered from ERROR, state=IDLE"); transition_to(State::IDLE); } @@ -285,7 +332,7 @@ class DartManager // 无论 ERROR 还是 IDLE，都重新排队传送带复位 submit_task(make_slider_init_task()); - RCLCPP_INFO(logger_, "[DartManagerV2] queued SliderInitTask for recovery"); + RCLCPP_INFO(logger_, "[DartManager] queued SliderInitTask for recovery"); } void dispatch_next_task() { @@ -296,8 +343,7 @@ class DartManager task_queue_.pop_front(); prepare_outputs_for_task(*current_task_); - RCLCPP_INFO( - logger_, "[DartManagerV2] dispatching task: '%s'", current_task_->name().c_str()); + RCLCPP_INFO(logger_, "[DartManager] dispatching task: '%s'", current_task_->name().c_str()); transition_to(State::RUNNING); tick_current_task(); @@ -314,12 +360,12 @@ class DartManager if (status == ActionStatus::SUCCESS) { const std::string completed_task_name = current_task_->name(); current_task_->on_exit(); - RCLCPP_INFO(logger_, "[DartManagerV2] task '%s' SUCCESS", completed_task_name.c_str()); + RCLCPP_INFO(logger_, "[DartManager] task '%s' SUCCESS", completed_task_name.c_str()); // 处理 fire 任务完成 if (completed_task_name == "fire") { fire_count_++; - RCLCPP_INFO(logger_, "[DartManagerV2] fire completed, fire_count=%u", fire_count_); + RCLCPP_INFO(logger_, "[DartManager] fire completed, fire_count=%u", fire_count_); if (launch_prepare_enable_visual_assist_) { advance_dart_sequence_after_fire(); } @@ -329,7 +375,7 @@ class DartManager if (completed_task_name == "cancel_launch") { fire_count_ = 0; RCLCPP_INFO( - logger_, "[DartManagerV2] cancel_launch completed, fire_count reset to 0"); + logger_, "[DartManager] cancel_launch completed, fire_count reset to 0"); } current_task_.reset(); @@ -337,7 +383,7 @@ class DartManager } else if (status == ActionStatus::FAILURE) { RCLCPP_ERROR( - logger_, "[DartManagerV2] task '%s' FAILURE → state=ERROR", + logger_, "[DartManager] task '%s' FAILURE → state=ERROR", current_task_->name().c_str()); on_task_failure(); } @@ -531,7 +577,7 @@ class DartManager void advance_dart_sequence_after_fire() { if (!dart_launch_sequence_.advance_after_fire()) { - RCLCPP_WARN(logger_, "[DartManagerV2] current dart index already at the last dart"); + RCLCPP_WARN(logger_, "[DartManager] current dart index already at the last dart"); } sync_current_dart_outputs(); clear_auto_aim_feedback(); @@ -559,10 +605,13 @@ class DartManager double down_velocity = (fire_count_ == 0) ? belt_prepare_down_velocity_first_ : belt_prepare_down_velocity_; bool require_lifting_down = (fire_count_ > 0); + bool is_first_shot = (fire_count_ == 0); // 打印角度反馈状态 RCLCPP_INFO( - logger_, "[DartManager] Creating launch_prepare task, fire_count=%u", fire_count_); + logger_, + "[DartManager] Creating launch_prepare task, fire_count=%u, is_first_shot=%d", + fire_count_, is_first_shot); if (left_belt_angle_.ready() && right_belt_angle_.ready()) { RCLCPP_INFO( logger_, "[DartManager] Belt angles: left=%.4f, right=%.4f", *left_belt_angle_, @@ -586,7 +635,11 @@ class DartManager belt_prepare_down_torque_offset_, belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, require_lifting_down, *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_); + *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_, + belt_up_distance_, belt_prepare_up_velocity_, belt_up_decel_target_velocity_, + belt_up_decel_torque_offset_, belt_up_stall_velocity_threshold_, + belt_up_stall_confirm_ticks_, belt_up_stall_min_run_ticks_, + belt_up_decel_timeout_ticks_, is_first_shot); } if (cmd == "unload" || cmd == "cancel_launch") { @@ -594,8 +647,8 @@ class DartManager double down_velocity = belt_prepare_down_velocity_; return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, *right_belt_angle_, - *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, + *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, @@ -606,11 +659,12 @@ class DartManager } if (cmd == "fire") { + bool is_first_shot = (fire_count_ == 0); return std::make_shared( *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, lifting_stall_threshold_, lifting_stall_confirm_ticks_, lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, *limiting_command_, - limiting_fill_ticks_); + limiting_fill_ticks_, is_first_shot); } if (cmd == "manual_angle") { @@ -690,6 +744,16 @@ class DartManager uint64_t belt_prepare_down_zero_confirm_ticks_{60}; // ticks uint64_t belt_prepare_down_ramp_timeout_ticks_{2000}; // ticks + // 传送带上行控制参数 + double belt_up_distance_{0.65}; // m - 上行目标距离（略低于下行起点） + double belt_prepare_up_velocity_{15.0}; // rad/s - 上行速度 + double belt_up_decel_target_velocity_{1.0}; // rad/s - 上行减速阶段目标速度 + double belt_up_decel_torque_offset_{0.0}; // N⋅m - 上行减速阶段力矩偏移 + double belt_up_stall_velocity_threshold_{0.15}; // rad/s - 上行堵转速度阈值 + uint64_t belt_up_stall_confirm_ticks_{100}; // ticks - 上行堵转确认帧数 + uint64_t belt_up_stall_min_run_ticks_{300}; // ticks - 上行堵转最小运行帧数 + uint64_t belt_up_decel_timeout_ticks_{3000}; // ticks - 上行减速超时帧数 + double lifting_stall_threshold_{0.5}; uint64_t lifting_stall_confirm_ticks_{100}; uint64_t lifting_stall_min_run_ticks_{500}; @@ -719,7 +783,7 @@ class DartManager std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; - uint32_t fire_count_{0}; // 当前轮次已完成发射数 + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; }; diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index b6ee3e0..12f4fcb 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -24,36 +24,48 @@ class FireAndPreloadTask : public Task { const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, double lifting_stall_threshold, uint64_t lifting_stall_confirm_ticks, uint64_t lifting_stall_min_run_ticks, uint64_t lifting_stall_timeout_ticks, - rmcs_msgs::DartLimitingServoStatus& limiting_command, uint64_t preload_fill_ticks) + rmcs_msgs::DartLimitingServoStatus& limiting_command, uint64_t preload_fill_ticks, + bool is_first_shot = false) : Task("fire", "发射并预装填") { - then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000 // 等待释放完成帧数 - )); - - then( - std::make_shared( - "filling_lift_up", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 - )); - - then( - std::make_shared( - limiting_command, // 限位舵机状态（输出） - rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 - rmcs_msgs::DartLimitingServoStatus::LOCK, // 再锁回 - preload_fill_ticks // 预装填持续帧数 - )); + // 第一发特殊处理：只解锁扳机，无需升降和预装填 + if (is_first_shot) { + then( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 500 // 等待释放完成帧数 + )); + return; + } else { + then( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 500 // 等待释放完成帧数 + )); + + then( + std::make_shared( + "filling_lift_up", // 动作名称 + lifting_command, // 升降指令（输出） + rmcs_msgs::DartSliderStatus::UP, // 指令状态 + lifting_left_vel_fb, // 左升降电机速度反馈（输入） + lifting_right_vel_fb, // 右升降电机速度反馈（输入） + lifting_stall_threshold, // 堵转速度阈值 + lifting_stall_confirm_ticks, // 堵转确认帧数 + lifting_stall_min_run_ticks, // 最短运行帧数 + lifting_stall_timeout_ticks // 超时帧数 + )); + + then( + std::make_shared( + limiting_command, // 限位舵机状态（输出） + rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 + rmcs_msgs::DartLimitingServoStatus::LOCK, // 再锁回 + preload_fill_ticks // 预装填持续帧数 + )); + } } }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 03f70c9..db735ee 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -2,6 +2,7 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" +#include "manager/action/belt_deceleration_with_stall_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" #include "manager/action/belt_move_action.hpp" #include "manager/action/belt_pid_deceleration_action.hpp" @@ -34,7 +35,11 @@ class LaunchPreparationTask : public Task { uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - bool& belt_zero_calibration) + bool& belt_zero_calibration, double belt_up_distance, double up_velocity, + double up_decel_target_velocity, double up_decel_torque_offset, + double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, + uint64_t up_stall_min_run_ticks, uint64_t up_decel_timeout_ticks, + bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -61,7 +66,6 @@ class LaunchPreparationTask : public Task { 0.60)); // 下行最大距离限制（m，正值） // 步骤2：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） - // 必须等待速度真正降到零，否则保持阶段的常态力矩会导致疯转 then( std::make_shared( "belt_pid_deceleration", // 动作名称 @@ -74,8 +78,32 @@ class LaunchPreparationTask : public Task { down_zero_confirm_ticks, // 零速确认帧数 down_ramp_timeout_ticks)); // 超时帧数 - if (require_lifting_down) { - // 步骤3（2-4发）：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 + // 步骤3：根据是否第一发选择不同的并行动作 + if (is_first_shot) { + // 第一发：传送带保持高扭矩 + 扳机锁定并行（无升降下行） + auto parallel_hold_and_lock = + std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); + + parallel_hold_and_lock + ->also( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + down_torque_offset, // 力矩偏移值（N⋅m） + 500)) // 保持时长（ticks） + .also( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 500)); // 等待锁定完成帧数 + then(parallel_hold_and_lock); + } else if (require_lifting_down) { + // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 auto parallel_hold_lock_and_lift = std::make_shared("hold_lock_and_lift", ActionSet::Policy::ALL_SUCCESS); @@ -133,30 +161,46 @@ class LaunchPreparationTask : public Task { then(parallel_hold_and_lock); } - // 步骤4：传送带上行到机械限位（速度控制 + 堵转检测） + // 步骤4：传送带上行到目标位置（位置控制） + // 目标位置略低于下行起点（比如 -0.65m，而下行是 0.70m） + then( + std::make_shared( + "belt_reset_up_position", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_limit, // 扭矩限幅（输出） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -belt_up_distance, // 目标距离（m，负值=上行） + belt_pulley_radius, // 滑轮半径（m） + up_velocity, // 运动速度（rad/s） + up_torque_limit, // 扭矩限制（N⋅m） + 5000, // 超时帧数 + 50, // 最小运行帧数 + 0.5, // 位置到达容差（rad） + 0.3, // 堵转速度阈值（rad/s） + 100, // 堵转确认帧数 + 0.80)); // 下行最大距离限制（m，正值） + + // 步骤5：减速并监测堵转（堵转标志成功，作为下次下行起点） then( - std::make_shared( - "belt_reset_up", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 15, // 设定速度（rad/s） - up_torque_limit, // 设定力矩限制（N⋅m） - 0.5, // 设定保持力矩（N⋅m） - 5000, // 超时帧数 - 0.5, // 堵转速度阈值（rad/s） - 0.8, // 堵转力矩阈值（N⋅m） - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 - false)); // 超时返回失败 + std::make_shared( + "belt_decel_and_stall", // 动作名称 + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + 5.0, // 目标速度（rad/s） + up_decel_torque_offset, // 力矩偏移（N⋅m） + 0.1, // 堵转速度阈值（rad/s） + 0.1, // 堵转扭矩阈值（N⋅m） + 100, // 堵转确认帧数 + 10, // 最小运行帧数 + 2000)); // 超时帧数 then(std::make_shared("stabilize_wait", 50)); diff --git a/src/manager/task/vision_assisted_launch_preparation_task.hpp b/src/manager/task/vision_assisted_launch_preparation_task.hpp index 15ff40e..3b5ace9 100644 --- a/src/manager/task/vision_assisted_launch_preparation_task.hpp +++ b/src/manager/task/vision_assisted_launch_preparation_task.hpp @@ -22,8 +22,7 @@ class VisionAssistedLaunchPreparationTask : public Task { VisionAssistedLaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, - const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, double belt_down_distance, double belt_pulley_radius, double down_velocity, @@ -32,10 +31,14 @@ class VisionAssistedLaunchPreparationTask : public Task { uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - bool& belt_zero_calibration, Eigen::Vector2d& yaw_pitch_control_velocity, - bool& aim_ready, Eigen::Vector2d& aim_error_px, Eigen::Vector2d& desired_target_px_output, - const cv::Point2i& target_position, const bool& target_tracking, - const rclcpp::Logger& logger, AutoAimParams auto_aim_params) + bool& belt_zero_calibration, double belt_up_distance, double up_velocity, + double up_decel_target_velocity, double up_decel_torque_offset, + double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, + uint64_t up_stall_min_run_ticks, uint64_t up_decel_timeout_ticks, + Eigen::Vector2d& yaw_pitch_control_velocity, bool& aim_ready, Eigen::Vector2d& aim_error_px, + Eigen::Vector2d& desired_target_px_output, const cv::Point2i& target_position, + const bool& target_tracking, const rclcpp::Logger& logger, AutoAimParams auto_aim_params, + bool is_first_shot = false) : Task("launch_preparation", "视觉辅助滑块发射准备") { then( @@ -51,8 +54,11 @@ class VisionAssistedLaunchPreparationTask : public Task { trigger_lock_enable, belt_down_distance, belt_pulley_radius, down_velocity, torque_limit, up_torque_limit, down_ramp_ticks, down_torque_offset, down_hold_torque, down_zero_velocity_threshold, down_zero_confirm_ticks, - down_ramp_timeout_ticks, require_lifting_down, lifting_command, - lifting_left_vel_fb, lifting_right_vel_fb, belt_zero_calibration)); + down_ramp_timeout_ticks, require_lifting_down, lifting_command, lifting_left_vel_fb, + lifting_right_vel_fb, belt_zero_calibration, belt_up_distance, up_velocity, + up_decel_target_velocity, up_decel_torque_offset, up_stall_velocity_threshold, + up_stall_confirm_ticks, up_stall_min_run_ticks, up_decel_timeout_ticks, + is_first_shot)); } }; From cc55f8f224243d359620903baed2204f272af5e8 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Fri, 3 Apr 2026 23:17:47 +0800 Subject: [PATCH 08/19] develping position control to make slider hold still at certain point --- .../belt_constant_velocity_move_action.hpp | 21 +-- .../action/belt_pid_deceleration_action.hpp | 3 - .../action/belt_velocity_ramp_action.hpp | 122 ------------------ .../action/force_screw_calibration_action.hpp | 91 +++++++++++++ src/manager/dart_manager.cpp | 86 +++++++++++- src/manager/task/cancel_launch_task.hpp | 63 ++++----- src/manager/task/launch_preparation_task.hpp | 93 +++++++++---- 7 files changed, 284 insertions(+), 195 deletions(-) delete mode 100644 src/manager/action/belt_velocity_ramp_action.hpp create mode 100644 src/manager/action/force_screw_calibration_action.hpp diff --git a/src/manager/action/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp index b9200d3..32636f7 100644 --- a/src/manager/action/belt_constant_velocity_move_action.hpp +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -22,7 +22,8 @@ class BeltConstantVelocityMoveAction : public IAction { public: BeltConstantVelocityMoveAction( std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, - double& belt_torque_limit, const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, double& belt_torque_limit, double torque_offset_value, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, double target_distance, // 目标距离（m，正=上行，负=下行） double pulley_radius, // 滑轮半径（m） @@ -35,7 +36,9 @@ class BeltConstantVelocityMoveAction : public IAction { : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) + , belt_torque_offset_(belt_torque_offset) , belt_torque_limit_(belt_torque_limit) + , torque_offset_value_(torque_offset_value) , left_belt_angle_(left_belt_angle) , right_belt_angle_(right_belt_angle) , left_belt_velocity_(left_belt_velocity) @@ -86,12 +89,13 @@ class BeltConstantVelocityMoveAction : public IAction { belt_target_velocity_ = std::abs(velocity_); } + belt_torque_offset_ = torque_offset_value_; belt_torque_limit_ = torque_limit_; } ActionStatus update() override { if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::FAILURE; + return ActionStatus::SUCCESS; } double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; @@ -99,13 +103,12 @@ class BeltConstantVelocityMoveAction : public IAction { double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - // 上行速度切换逻辑：在0.2m处从5.0切换到15.0 - if (target_distance_ < 0) { // 上行（负距离） + if (target_distance_ < 0) { // 上行（负距离） double distance_traveled = std::abs(avg_angle - initial_angle_) * pulley_radius_; if (distance_traveled < 0.2) { - belt_target_velocity_ = 5.0; // 前0.2m慢速 + belt_target_velocity_ = 5.0; // 前0.2m慢速 } else { - belt_target_velocity_ = 15.0; // 0.2m后快速 + belt_target_velocity_ = 15.0; // 0.2m后快速 } } @@ -139,7 +142,7 @@ class BeltConstantVelocityMoveAction : public IAction { if (avg_velocity < stall_velocity_threshold_) { ++stall_counter_; if (stall_counter_ >= stall_confirm_ticks_) { - return ActionStatus::FAILURE; + return ActionStatus::SUCCESS; } } else { stall_counter_ = 0; @@ -149,12 +152,14 @@ class BeltConstantVelocityMoveAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override {} + void on_exit() override { belt_torque_offset_ = 0.0; } private: rmcs_msgs::DartSliderStatus& belt_command_; double& belt_target_velocity_; + double& belt_torque_offset_; double& belt_torque_limit_; + double torque_offset_value_; const double& left_belt_angle_; const double& right_belt_angle_; const double& left_belt_velocity_; diff --git a/src/manager/action/belt_pid_deceleration_action.hpp b/src/manager/action/belt_pid_deceleration_action.hpp index b52bcef..142e6e9 100644 --- a/src/manager/action/belt_pid_deceleration_action.hpp +++ b/src/manager/action/belt_pid_deceleration_action.hpp @@ -30,16 +30,13 @@ class BeltPIDDecelerationAction : public IAction { } ActionStatus update() override { - // 超时检测 if (elapsed_ticks() >= timeout_ticks_) { return ActionStatus::SUCCESS; } - // 计算平均速度 double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - // 零速检测 if (avg_velocity < zero_velocity_threshold_) { ++zero_counter_; if (zero_counter_ >= zero_confirm_ticks_) { diff --git a/src/manager/action/belt_velocity_ramp_action.hpp b/src/manager/action/belt_velocity_ramp_action.hpp deleted file mode 100644 index 6871f19..0000000 --- a/src/manager/action/belt_velocity_ramp_action.hpp +++ /dev/null @@ -1,122 +0,0 @@ -#pragma once - -#include "action.hpp" - -#include -#include -#include -#include - -#include - -namespace rmcs_dart_guidance::manager { - -class BeltModerateAction : public IAction { -public: - BeltModerateAction( - std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, - double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - const double& left_belt_velocity, const double& right_belt_velocity, - uint64_t target_ramp_ticks, double torque_limit, double hold_torque, - double zero_velocity_threshold, uint64_t zero_confirm_ticks, uint64_t timeout_ticks, - double initial_velocity) - : IAction(std::move(name)) - , belt_command_(belt_command) - , belt_target_velocity_(belt_target_velocity) - , belt_torque_limit_(belt_torque_limit) - , belt_hold_torque_(belt_hold_torque) - , belt_wait_zero_velocity_(belt_wait_zero_velocity) - , left_belt_velocity_(left_belt_velocity) - , right_belt_velocity_(right_belt_velocity) - , target_ramp_ticks_(target_ramp_ticks) - , torque_limit_(std::max(0.0, std::abs(torque_limit))) - , hold_torque_(hold_torque) - , zero_velocity_threshold_(std::max(0.0, std::abs(zero_velocity_threshold))) - , zero_confirm_ticks_(zero_confirm_ticks) - , timeout_ticks_(timeout_ticks) - , initial_velocity_(std::abs(initial_velocity)) {} - - void on_enter() override { - belt_torque_limit_ = torque_limit_; - belt_hold_torque_ = hold_torque_; - - // 使用传入的初始速度，而不是读取可能错误的速度反馈 - // 但如果上一个action已经设置了belt_target_velocity_，优先使用它以保证连续性 - current_velocity_ = - (belt_target_velocity_ > 1e-6) ? belt_target_velocity_ : initial_velocity_; - belt_target_velocity_ = current_velocity_; - - // 基于初始速度和目标减速时长，动态计算减速步长 - // 这样无论初始速度是多少，减速时间都是 target_ramp_ticks - ramp_step_per_tick_ = target_ramp_ticks_ > 0 - ? (current_velocity_ / static_cast(target_ramp_ticks_)) - : current_velocity_; - - zero_counter_ = 0; - - // 调试日志：打印关键参数 - static int enter_count = 0; - ++enter_count; - printf( - "[BeltModerateAction::on_enter #%d] initial_velocity_=%.4f, current_velocity_=%.4f, " - "belt_target_velocity_=%.4f, ramp_step=%.6f\n", - enter_count, initial_velocity_, current_velocity_, belt_target_velocity_, - ramp_step_per_tick_); - } - - ActionStatus update() override { - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::FAILURE; - } - - current_velocity_ = std::max(0.0, current_velocity_ - ramp_step_per_tick_); - belt_target_velocity_ = current_velocity_; - - if (current_velocity_ <= 1e-6) { - const double avg_vel = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - if (avg_vel < zero_velocity_threshold_) { - ++zero_counter_; - if (zero_counter_ >= zero_confirm_ticks_) { - return ActionStatus::SUCCESS; - } - } else { - zero_counter_ = 0; - } - } - - return ActionStatus::RUNNING; - } - - void on_exit() override { - // 退出时设置为WAIT模式，但保持扭矩和参数设置 - // 这样下一个action可以无缝接管 - belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; - belt_target_velocity_ = 0.0; - // belt_torque_limit_和belt_hold_torque_保持不变，由下一个action接管 - belt_wait_zero_velocity_ = false; - } - -private: - rmcs_msgs::DartSliderStatus& belt_command_; - double& belt_target_velocity_; - double& belt_torque_limit_; - double& belt_hold_torque_; - bool& belt_wait_zero_velocity_; - const double& left_belt_velocity_; - const double& right_belt_velocity_; - - uint64_t target_ramp_ticks_; - double ramp_step_per_tick_{0.0}; - double torque_limit_; - double hold_torque_; - double zero_velocity_threshold_; - uint64_t zero_confirm_ticks_; - uint64_t timeout_ticks_; - double initial_velocity_; // 减速起点速度 - - double current_velocity_{0.0}; - uint64_t zero_counter_{0}; -}; - -} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/force_screw_calibration_action.hpp b/src/manager/action/force_screw_calibration_action.hpp new file mode 100644 index 0000000..35bb3ac --- /dev/null +++ b/src/manager/action/force_screw_calibration_action.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include "action.hpp" +#include +#include + +namespace rmcs_dart_guidance::manager { + +// ForceScrewCalibrationAction — 力矩闭环控制动作 +// 通过PID控制force_screw_motor，使当前力闭环到目标力值 +// 电机正转力增大，反转力减小 +class ForceScrewCalibrationAction : public IAction { +public: + ForceScrewCalibrationAction( + std::string name, double& force_screw_velocity, const int& current_force, + double target_force, double force_tolerance, uint64_t settle_ticks, uint64_t timeout_ticks, + double kp, double ki, double kd, double max_velocity) + : IAction(std::move(name)) + , force_screw_velocity_(force_screw_velocity) + , current_force_(current_force) + , target_force_(target_force) + , force_tolerance_(force_tolerance) + , settle_ticks_(settle_ticks) + , timeout_ticks_(timeout_ticks) + , kp_(kp) + , ki_(ki) + , kd_(kd) + , max_velocity_(max_velocity) {} + + void on_enter() override { + integral_ = 0.0; + last_error_ = 0.0; + settle_counter_ = 0; + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + double error = (target_force_ - static_cast(current_force_)) * 5; // 增加响应速度 + + integral_ += error; + double derivative = error - last_error_; + last_error_ = error; + + // PID输出 + double output = kp_ * error + ki_ * integral_ + kd_ * derivative; + + // 限幅 + if (output > max_velocity_) { + output = max_velocity_; + } else if (output < -max_velocity_) { + output = -max_velocity_; + } + + // 输出到电机 + force_screw_velocity_ = output; + + // 检查是否在容差范围内 + if (std::abs(error) <= force_tolerance_) { + ++settle_counter_; + if (settle_counter_ >= settle_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + settle_counter_ = 0; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { force_screw_velocity_ = 0.0; } + +private: + double& force_screw_velocity_; + const int& current_force_; + double target_force_; + double force_tolerance_; + uint64_t settle_ticks_; + uint64_t timeout_ticks_; + double kp_; + double ki_; + double kd_; + double max_velocity_; + double integral_{0.0}; + double last_error_{0.0}; + uint64_t settle_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 66d80d4..db39980 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -57,6 +57,8 @@ class DartManager register_input("/dart/drive_belt/right/angle", right_belt_angle_); register_input("/dart/lifting_left/velocity", lifting_left_vel_fb_); register_input("/dart/lifting_right/velocity", lifting_right_vel_fb_); + register_input("/force_sensor/channel_1/weight", current_force_ch1_); + register_input("/force_sensor/channel_2/weight", current_force_ch2_); register_input("/dart/manager/command", remote_command_input_, false); register_input("/dart/manager/web_command", web_command_input_, false); @@ -234,6 +236,48 @@ class DartManager lifting_stall_timeout_ticks_ = (uint64_t)get_parameter("lifting_stall_timeout_ticks").as_int(); + // 力矩闭环参数 + try { + enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); + } catch (...) { + enable_force_calibration_ = false; + } + try { + force_tolerance_ = get_parameter("force_tolerance").as_double(); + } catch (...) { + force_tolerance_ = 5.0; // N + } + try { + force_settle_ticks_ = (uint64_t)get_parameter("force_settle_ticks").as_int(); + } catch (...) { + force_settle_ticks_ = 50; + } + try { + force_timeout_ticks_ = (uint64_t)get_parameter("force_timeout_ticks").as_int(); + } catch (...) { + force_timeout_ticks_ = 2000; + } + try { + force_kp_ = get_parameter("force_kp").as_double(); + } catch (...) { + force_kp_ = 0.1; + } + try { + force_ki_ = get_parameter("force_ki").as_double(); + } catch (...) { + force_ki_ = 0.0; + } + try { + force_kd_ = get_parameter("force_kd").as_double(); + } catch (...) { + force_kd_ = 0.01; + } + try { + force_max_velocity_ = get_parameter("force_max_velocity").as_double(); + } catch (...) { + force_max_velocity_ = 5.0; // rad/s + } + state_pub_ = create_publisher("/dart/manager/state", 10); sync_current_dart_outputs(); @@ -304,12 +348,13 @@ class DartManager } first_fill_pending_ = true; + fire_count_ = 0; // 重置开火次数 enter_belt_wait_zero_velocity_mode(); *lifting_command_ = rmcs_msgs::DartSliderStatus::WAIT; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); - *force_control_velocity_ = 0.0; + *force_control_velocity_ = 0.0; // 停止丝杆电机（已包含） clear_auto_aim_feedback(); transition_to(State::IDLE); @@ -398,7 +443,7 @@ class DartManager *lifting_command_ = rmcs_msgs::DartSliderStatus::WAIT; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); - *force_control_velocity_ = 0.0; + *force_control_velocity_ = 0.0; // 停止丝杆电机（已包含） clear_auto_aim_feedback(); transition_to(State::ERROR); @@ -625,6 +670,16 @@ class DartManager "down_velocity=%.2f rad/s", belt_down_distance_, belt_pulley_radius_, down_velocity); + // 记录当前力值（用于下次fire前的力矩闭环） + if (current_force_ch1_.ready()) { + last_fire_force_ = static_cast(*current_force_ch1_); + RCLCPP_INFO( + logger_, "[DartManager] Recorded force before fire: %.2f (ch1=%d)", + last_fire_force_, *current_force_ch1_); + } else { + RCLCPP_WARN(logger_, "[DartManager] Force sensor NOT READY, using last value"); + } + return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, @@ -639,12 +694,17 @@ class DartManager belt_up_distance_, belt_prepare_up_velocity_, belt_up_decel_target_velocity_, belt_up_decel_torque_offset_, belt_up_stall_velocity_threshold_, belt_up_stall_confirm_ticks_, belt_up_stall_min_run_ticks_, - belt_up_decel_timeout_ticks_, is_first_shot); + belt_up_decel_timeout_ticks_, *force_control_velocity_, *current_force_ch1_, + last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, + force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, + is_first_shot); } if (cmd == "unload" || cmd == "cancel_launch") { // 取消发射使用与准备发射相同的下行参数 double down_velocity = belt_prepare_down_velocity_; + // 第一发时不需要升降上行（因为还没有下行过） + bool require_lifting_up = (fire_count_ > 0); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, @@ -655,7 +715,7 @@ class DartManager belt_prepare_up_torque_limit_, belt_prepare_down_ramp_ticks_, belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, - *belt_zero_calibration_); + *belt_zero_calibration_, require_lifting_up); } if (cmd == "fire") { @@ -706,6 +766,10 @@ class DartManager InputInterface lifting_left_vel_fb_; InputInterface lifting_right_vel_fb_; + // 力传感器反馈（两个通道） + InputInterface current_force_ch1_; + InputInterface current_force_ch2_; + OutputInterface belt_command_; OutputInterface belt_target_velocity_; OutputInterface belt_torque_limit_; @@ -758,6 +822,17 @@ class DartManager uint64_t lifting_stall_confirm_ticks_{100}; uint64_t lifting_stall_min_run_ticks_{500}; uint64_t lifting_stall_timeout_ticks_{5000}; + + // 力矩闭环参数 + bool enable_force_calibration_{false}; + double force_tolerance_{5.0}; // N + uint64_t force_settle_ticks_{50}; + uint64_t force_timeout_ticks_{2000}; + double force_kp_{0.1}; + double force_ki_{0.0}; + double force_kd_{0.01}; + double force_max_velocity_{5.0}; // rad/s + bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; DartLaunchSequence dart_launch_sequence_; @@ -783,8 +858,9 @@ class DartManager std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; - uint32_t fire_count_{0}; // 当前轮次已完成发射数 + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; + double last_fire_force_{0.0}; // 上次fire前记录的力值 }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index e9f23f0..e1477de 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -4,7 +4,7 @@ #include "manager/action/belt_constant_velocity_move_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" #include "manager/action/belt_move_action.hpp" -#include "manager/action/belt_velocity_ramp_action.hpp" +#include "manager/action/belt_pid_deceleration_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -38,7 +38,7 @@ class CancelLaunchTask : public Task { double down_velocity, double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, double down_hold_torque, double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, - bool& belt_zero_calibration) + bool& belt_zero_calibration, bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） @@ -47,7 +47,9 @@ class CancelLaunchTask : public Task { "belt_move_down_constant_velocity", // 动作名称 belt_command, // 速度模式方向命令（输出） belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） belt_torque_limit, // 扭矩限幅（输出） + 0.0, left_belt_angle, // 左电机角度反馈（输入） right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） @@ -63,47 +65,44 @@ class CancelLaunchTask : public Task { 200, // 堵转确认帧数 0.20)); // 下行最大距离限制（m，正值） - // 步骤2：下行速度斜坡减速到 0，成功后自动进入 HOLD_TORQUE 保持张力 + // 步骤2：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） then( - std::make_shared( + std::make_shared( "belt_down_ramp_to_zero", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_limit, // 力矩限幅（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT 模式选择（输出） - left_belt_velocity, // 左皮带速度反馈（输入） - right_belt_velocity, // 右皮带速度反馈（输入） - down_ramp_ticks, // 目标减速时长（ticks） - torque_limit, // 力矩限制 - down_hold_torque, // 进入 HOLD_TORQUE 的保持力矩 - down_zero_velocity_threshold, // 实测零速阈值 + belt_target_velocity, // 目标速度（输出，设为0） + belt_torque_offset, // 力矩偏移（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + down_hold_torque, // 力矩偏移值（N⋅m） + down_zero_velocity_threshold, // 零速阈值（rad/s） down_zero_confirm_ticks, // 零速确认帧数 - down_ramp_timeout_ticks, // 斜坡阶段超时 - down_velocity)); // 初始速度 + down_ramp_timeout_ticks)); // 超时帧数 - // 步骤3：传送带保持高扭矩 + 解锁扳机 + 升降上行并行 + // 步骤3：传送带保持高扭矩 + 解锁扳机 + (可选)升降上行并行 auto parallel_hold_unlock_and_lift = std::make_shared("hold_unlock_and_lift", ActionSet::Policy::ALL_SUCCESS); parallel_hold_unlock_and_lift ->also( std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - 2.5, // 力矩偏移值（N⋅m）- 取消发射不需要偏移 - 2000)) // 保持时长（ticks） + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + 2.5, // 力矩偏移值（N⋅m）- 取消发射不需要偏移 + 500)) // 保持时长（ticks） .also( std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000)) // 等待释放完成帧数 - .also( + trigger_lock_enable, // 扳机锁定使能（输出） + false, // 解锁（false） + 500)); // 等待释放完成帧数 + + // 只有在需要时才添加升降上行动作（第二发及以后才需要） + if (require_lifting_up) { + parallel_hold_unlock_and_lift->also( std::make_shared( "filling_lift_up", // 动作名称 lifting_command, // 升降指令（输出） @@ -114,6 +113,8 @@ class CancelLaunchTask : public Task { 100, // 堵转确认帧数 500, // 最短运行帧数 2000)); // 超时帧数 + } + then(parallel_hold_unlock_and_lift); // 步骤4：同步带上行复位到机械限位 diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index db735ee..0f8747b 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -9,6 +9,7 @@ #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" +#include "manager/action/force_screw_calibration_action.hpp" #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" @@ -39,7 +40,10 @@ class LaunchPreparationTask : public Task { double up_decel_target_velocity, double up_decel_torque_offset, double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, uint64_t up_stall_min_run_ticks, uint64_t up_decel_timeout_ticks, - bool is_first_shot = false) + double& force_screw_velocity, const int& current_force, double target_force, + bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, + uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, + double force_max_velocity, bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -49,7 +53,9 @@ class LaunchPreparationTask : public Task { "belt_move_down_constant_velocity", // 动作名称 belt_command, // 速度模式方向命令（输出） belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值 left_belt_angle, // 左电机角度反馈（输入） right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） @@ -63,9 +69,9 @@ class LaunchPreparationTask : public Task { 0.5, // 位置到达容差（rad）- 增大容差 0.3, // 堵转速度阈值（rad/s） 100, // 堵转确认帧数 - 0.60)); // 下行最大距离限制（m，正值） + 0.80)); // 下行最大距离限制（m，正值） - // 步骤2：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） + // 步骤3：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） then( std::make_shared( "belt_pid_deceleration", // 动作名称 @@ -75,8 +81,8 @@ class LaunchPreparationTask : public Task { right_belt_velocity, // 右电机速度反馈（输入） down_torque_offset, // 力矩偏移值（N⋅m） down_zero_velocity_threshold, // 零速阈值（rad/s） - down_zero_confirm_ticks, // 零速确认帧数 - down_ramp_timeout_ticks)); // 超时帧数 + 100, // 零速确认帧数 + 1000)); // 超时帧数 // 步骤3：根据是否第一发选择不同的并行动作 if (is_first_shot) { @@ -86,22 +92,35 @@ class LaunchPreparationTask : public Task { parallel_hold_and_lock ->also( - std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - down_torque_offset, // 力矩偏移值（N⋅m） - 500)) // 保持时长（ticks） - .also( std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 500)); // 等待锁定完成帧数 + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 750)) + .also( + std::make_shared( + "belt_pid_deceleration", // 动作名称 + belt_target_velocity, // 目标速度（输出，设为0） + belt_torque_offset, // 力矩偏移（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + down_torque_offset, // 力矩偏移值（N⋅m） + down_zero_velocity_threshold, // 零速阈值（rad/s） + 10, // 零速确认帧数 + 3000)); // 超时帧数 then(parallel_hold_and_lock); + + then( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + down_hold_torque, // 保持力矩值（N⋅m） + down_torque_offset, // 力矩偏移值（N⋅m） + 500)); // 保持时长（ticks） + } else if (require_lifting_down) { // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 auto parallel_hold_lock_and_lift = @@ -152,12 +171,12 @@ class LaunchPreparationTask : public Task { belt_torque_offset, // 力矩偏移（输出） down_hold_torque, // 保持力矩值（N⋅m） down_torque_offset, // 力矩偏移值（N⋅m） - 500)) // 保持时长（ticks） + 1000)) // 保持时长（ticks） .also( std::make_shared( trigger_lock_enable, // 扳机锁定使能（输出） true, // 锁定（true） - 500)); // 等待锁定完成帧数 + 1000)); // 等待锁定完成帧数 then(parallel_hold_and_lock); } @@ -168,7 +187,9 @@ class LaunchPreparationTask : public Task { "belt_reset_up_position", // 动作名称 belt_command, // 速度模式方向命令（输出） belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） belt_torque_limit, // 扭矩限幅（输出） + 5.0, // 力矩偏移值 left_belt_angle, // 左电机角度反馈（输入） right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） @@ -177,7 +198,7 @@ class LaunchPreparationTask : public Task { belt_pulley_radius, // 滑轮半径（m） up_velocity, // 运动速度（rad/s） up_torque_limit, // 扭矩限制（N⋅m） - 5000, // 超时帧数 + 15000, // 超时帧数 50, // 最小运行帧数 0.5, // 位置到达容差（rad） 0.3, // 堵转速度阈值（rad/s） @@ -194,17 +215,37 @@ class LaunchPreparationTask : public Task { right_belt_velocity, // 右电机速度反馈（输入） left_belt_torque, // 左电机力矩反馈（输入） right_belt_torque, // 右电机力矩反馈（输入） - 5.0, // 目标速度（rad/s） + 2.0, // 目标速度（rad/s） up_decel_torque_offset, // 力矩偏移（N⋅m） - 0.1, // 堵转速度阈值（rad/s） - 0.1, // 堵转扭矩阈值（N⋅m） + 0.5, // 堵转速度阈值（rad/s） + 0.5, // 堵转扭矩阈值（N⋅m） 100, // 堵转确认帧数 10, // 最小运行帧数 - 2000)); // 超时帧数 + 5000)); // 超时帧数 then(std::make_shared("stabilize_wait", 50)); then(std::make_shared(belt_zero_calibration)); + + // 力矩闭环：第一发使用固定目标力9525，后续发使用上次记录的力值 + // if (enable_force_calibration) { + // double target_force_value = is_first_shot ? 11300.0 : target_force; + // double tolerance_value = is_first_shot ? 3.0 : force_tolerance; + + // then( + // std::make_shared( + // "force_screw_calibration", // 动作名称 + // force_screw_velocity, // 丝杆电机速度（输出） + // current_force, // 当前力传感器读数（输入） + // target_force_value, // 目标力值（第一发9025，后续用记录值） + // tolerance_value, // 力容差（第一发±30，后续用配置值） + // force_settle_ticks, // 稳定确认帧数 + // force_timeout_ticks, // 超时帧数 + // force_kp, // PID参数Kp + // force_ki, // PID参数Ki + // force_kd, // PID参数Kd + // force_max_velocity)); // 最大速度限制 + // } } }; From 4f2e48004bc2b9580ead02666c1a05c6ae3a3679 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 5 Apr 2026 15:02:51 +0800 Subject: [PATCH 09/19] position contorl tested ac with a unexcept problem of delay when go UP --- .../belt_constant_velocity_move_action.hpp | 10 +- .../action/belt_deceleration_action.hpp | 148 +++++++++++ .../belt_deceleration_with_stall_action.hpp | 98 -------- .../action/belt_hold_torque_action.hpp | 3 +- .../action/belt_pid_deceleration_action.hpp | 66 ----- .../action/belt_up_initial_accel_action.hpp | 87 +++++++ src/manager/dart_manager.cpp | 51 ++-- src/manager/task/cancel_launch_task.hpp | 25 +- src/manager/task/launch_preparation_task.hpp | 233 ++++++++++-------- 9 files changed, 415 insertions(+), 306 deletions(-) create mode 100644 src/manager/action/belt_deceleration_action.hpp delete mode 100644 src/manager/action/belt_deceleration_with_stall_action.hpp delete mode 100644 src/manager/action/belt_pid_deceleration_action.hpp create mode 100644 src/manager/action/belt_up_initial_accel_action.hpp diff --git a/src/manager/action/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp index 32636f7..f4177d6 100644 --- a/src/manager/action/belt_constant_velocity_move_action.hpp +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -15,7 +15,7 @@ namespace rmcs_dart_guidance::manager { // 完成条件：到达目标位置（基于多圈角度反馈） // 优点：避免PID error过大导致的问题，运动更平滑 // -// 重要：目标位置基于初始化后的实际角度计算，而非概念上的"零点" +// 重要：目标位置基于初始化后的实际角度计算 // - 输入参数是距离（m），在函数内换算为角度（rad） // - 目标位置 = 初始角度 + (目标距离 / 滑轮半径) class BeltConstantVelocityMoveAction : public IAction { @@ -32,7 +32,7 @@ class BeltConstantVelocityMoveAction : public IAction { uint64_t timeout_ticks, uint64_t min_running_ticks = 50, double position_tolerance = 0.01, // 位置到达容差（rad） double stall_velocity_threshold = 0.3, uint64_t stall_confirm_ticks = 200, - double max_down_distance = 0.60) // 下行最大距离限制（m，正值，防止过度下行） + double max_down_distance = 0.80) // 下行最大距离限制（m，正值，防止过度下行） : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -61,7 +61,6 @@ class BeltConstantVelocityMoveAction : public IAction { initial_angle_ = left_belt_angle_; // 将目标距离（m）换算为角度偏移（rad） - // angle_offset = distance / radius double target_angle_offset = target_distance_ / pulley_radius_; // 计算实际目标位置 = 初始角度 + 角度偏移 @@ -113,11 +112,6 @@ class BeltConstantVelocityMoveAction : public IAction { } // 位置到达判断（优先级最高） - // 判断条件： - // 1. 在容差范围内：|error| < tolerance - // 2. 或者已经越过目标（考虑运动方向）： - // - 下行（target > initial）：error < 0 表示已经越过（current > target） - // - 上行（target < initial）：error > 0 表示已经越过（current < target） if (elapsed_ticks() > min_running_ticks_) { bool within_tolerance = std::abs(position_error) < position_tolerance_; bool overshot = false; diff --git a/src/manager/action/belt_deceleration_action.hpp b/src/manager/action/belt_deceleration_action.hpp new file mode 100644 index 0000000..0bf13d5 --- /dev/null +++ b/src/manager/action/belt_deceleration_action.hpp @@ -0,0 +1,148 @@ +#pragma once + +#include "action.hpp" +#include +#include + +namespace rmcs_dart_guidance::manager { + +// BeltDecelerationAction - 传送带减速动作 +// 支持多种减速模式： +// 1. 零速检测模式：减速到零速，检测速度低于阈值作为成功条件 +// 2. 堵转检测模式：减速到目标速度，检测堵转（低速+高扭矩）作为成功条件 +// 3. 混合模式：同时支持零速和堵转检测 +// +// 参数说明： +// - target_velocity: 目标速度（rad/s），通常为0或小的正值 +// - torque_offset_value: 力矩偏移（N⋅m），用于补偿负载 +// - enable_stall_detection: 是否启用堵转检测 +// - enable_zero_velocity_detection: 是否启用零速检测 +// - stall_velocity_threshold: 堵转速度阈值（rad/s） +// - stall_torque_threshold: 堵转扭矩阈值（N⋅m） +// - zero_velocity_threshold: 零速阈值（rad/s） +// - stall_confirm_ticks: 堵转确认帧数 +// - zero_confirm_ticks: 零速确认帧数 +// - min_running_ticks: 最小运行帧数（避免启动瞬间误触发） +// - timeout_ticks: 超时帧数 +class BeltDecelerationAction : public IAction { +public: + BeltDecelerationAction( + std::string name, double& belt_target_velocity, double& belt_torque_offset, + double& belt_error_gain, bool& belt_use_decel_pid, + const double& left_belt_velocity, const double& right_belt_velocity, + const double& left_belt_torque, const double& right_belt_torque, + double target_velocity = 0.0, double torque_offset_value = 0.0, + double error_gain_value = 2.0, // 默认2倍error增益 + bool enable_stall_detection = false, bool enable_zero_velocity_detection = true, + double stall_velocity_threshold = 0.5, double stall_torque_threshold = 0.5, + double zero_velocity_threshold = 0.1, uint64_t stall_confirm_ticks = 100, + uint64_t zero_confirm_ticks = 100, uint64_t min_running_ticks = 50, + uint64_t timeout_ticks = 3000) + : IAction(std::move(name)) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_offset_(belt_torque_offset) + , belt_error_gain_(belt_error_gain) + , belt_use_decel_pid_(belt_use_decel_pid) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , left_belt_torque_(left_belt_torque) + , right_belt_torque_(right_belt_torque) + , target_velocity_(target_velocity) + , torque_offset_value_(torque_offset_value) + , error_gain_value_(error_gain_value) + , enable_stall_detection_(enable_stall_detection) + , enable_zero_velocity_detection_(enable_zero_velocity_detection) + , stall_velocity_threshold_(stall_velocity_threshold) + , stall_torque_threshold_(stall_torque_threshold) + , zero_velocity_threshold_(zero_velocity_threshold) + , stall_confirm_ticks_(stall_confirm_ticks) + , zero_confirm_ticks_(zero_confirm_ticks) + , min_running_ticks_(min_running_ticks) + , timeout_ticks_(timeout_ticks) {} + + void on_enter() override { + belt_target_velocity_ = target_velocity_; + belt_torque_offset_ = torque_offset_value_; + belt_error_gain_ = error_gain_value_; + belt_use_decel_pid_ = true; // 启用减速PID参数 + stall_counter_ = 0; + zero_counter_ = 0; + } + + ActionStatus update() override { + // 超时检测 + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + // 计算平均速度和扭矩 + double avg_velocity = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + bool torque_active = std::abs(left_belt_torque_) > stall_torque_threshold_ + || std::abs(right_belt_torque_) > stall_torque_threshold_; + + // 最小运行帧数后才开始检测 + if (elapsed_ticks() > min_running_ticks_) { + // 优先检测堵转：速度低且扭矩高（碰到机械限位） + if (enable_stall_detection_) { + if (avg_velocity < stall_velocity_threshold_ && torque_active) { + ++stall_counter_; + if (stall_counter_ >= stall_confirm_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + } + + // 零速检测：速度低于阈值且扭矩不高（正常减速到0） + // 只有在没有堵转的情况下才检测零速 + if (enable_zero_velocity_detection_ && !torque_active) { + if (avg_velocity < zero_velocity_threshold_) { + ++zero_counter_; + if (zero_counter_ >= zero_confirm_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + zero_counter_ = 0; + } + } + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + belt_torque_offset_ = 0.0; + belt_error_gain_ = 1.0; + belt_use_decel_pid_ = false; // 恢复正常PID参数 + } + +private: + double& belt_target_velocity_; + double& belt_torque_offset_; + double& belt_error_gain_; + bool& belt_use_decel_pid_; + const double& left_belt_velocity_; + const double& right_belt_velocity_; + const double& left_belt_torque_; + const double& right_belt_torque_; + + double target_velocity_; + double torque_offset_value_; + double error_gain_value_; + bool enable_stall_detection_; + bool enable_zero_velocity_detection_; + double stall_velocity_threshold_; + double stall_torque_threshold_; + double zero_velocity_threshold_; + uint64_t stall_confirm_ticks_; + uint64_t zero_confirm_ticks_; + uint64_t min_running_ticks_; + uint64_t timeout_ticks_; + + uint64_t stall_counter_{0}; + uint64_t zero_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_deceleration_with_stall_action.hpp b/src/manager/action/belt_deceleration_with_stall_action.hpp deleted file mode 100644 index fc544e7..0000000 --- a/src/manager/action/belt_deceleration_with_stall_action.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once - -#include "action.hpp" -#include -#include - -namespace rmcs_dart_guidance::manager { - -// BeltDecelerationWithStallAction - 减速并监测堵转 -// 用于上行最后阶段：减速到目标速度，监测堵转作为成功标志 -// 与 BeltPIDDecelerationAction 的区别： -// - 目标速度可配置（不一定是0） -// - 堵转检测返回 SUCCESS（而不是 FAILURE） -class BeltDecelerationWithStallAction : public IAction { -public: - BeltDecelerationWithStallAction( - std::string name, double& belt_target_velocity, double& belt_torque_offset, - const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque, - double target_velocity, // 目标速度（rad/s） - double torque_offset_value, // 力矩偏移（N⋅m） - double stall_velocity_threshold, // 堵转速度阈值（rad/s） - double stall_torque_threshold, // 堵转扭矩阈值（N⋅m） - uint64_t stall_confirm_ticks, // 堵转确认帧数 - uint64_t min_running_ticks, // 最小运行帧数 - uint64_t timeout_ticks // 超时帧数 - ) - : IAction(std::move(name)) - , belt_target_velocity_(belt_target_velocity) - , belt_torque_offset_(belt_torque_offset) - , left_belt_velocity_(left_belt_velocity) - , right_belt_velocity_(right_belt_velocity) - , left_belt_torque_(left_belt_torque) - , right_belt_torque_(right_belt_torque) - , target_velocity_(target_velocity) - , torque_offset_value_(torque_offset_value) - , stall_velocity_threshold_(stall_velocity_threshold) - , stall_torque_threshold_(stall_torque_threshold) - , stall_confirm_ticks_(stall_confirm_ticks) - , min_running_ticks_(min_running_ticks) - , timeout_ticks_(timeout_ticks) {} - - void on_enter() override { - belt_target_velocity_ = target_velocity_; - belt_torque_offset_ = torque_offset_value_; - stall_counter_ = 0; - printf( - "[%s] on_enter: target_velocity=%.4f, torque_offset=%.4f\n", name().c_str(), - target_velocity_, torque_offset_value_); - } - - ActionStatus update() override { - // 超时检测 - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::FAILURE; - } - - // 计算平均速度和扭矩 - double avg_velocity = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - bool torque_active = std::abs(left_belt_torque_) > stall_torque_threshold_ - || std::abs(right_belt_torque_) > stall_torque_threshold_; - - // 堵转检测（最小运行帧数后才开始检测）：速度低且扭矩高 - if (elapsed_ticks() > min_running_ticks_) { - if (avg_velocity < stall_velocity_threshold_ && torque_active) { - ++stall_counter_; - if (stall_counter_ >= stall_confirm_ticks_) { - return ActionStatus::SUCCESS; - } - } else { - stall_counter_ = 0; - } - } - - return ActionStatus::RUNNING; - } - - void on_exit() override { belt_torque_offset_ = 0.0; } - -private: - double& belt_target_velocity_; - double& belt_torque_offset_; - const double& left_belt_velocity_; - const double& right_belt_velocity_; - const double& left_belt_torque_; - const double& right_belt_torque_; - double target_velocity_; - double torque_offset_value_; - double stall_velocity_threshold_; - double stall_torque_threshold_; - uint64_t stall_confirm_ticks_; - uint64_t min_running_ticks_; - uint64_t timeout_ticks_; - uint64_t stall_counter_{0}; -}; - -} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index e38b903..703b2a7 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -46,7 +46,8 @@ class BeltHoldTorqueAction : public IAction { } void on_exit() override { - belt_torque_offset_ = 0.0; // 退出时清除扭矩偏移 + belt_torque_offset_ = 0.0; // 退出时清除扭矩偏移 + belt_wait_zero_velocity_ = false; // 退出零速度闭环模式 } private: diff --git a/src/manager/action/belt_pid_deceleration_action.hpp b/src/manager/action/belt_pid_deceleration_action.hpp deleted file mode 100644 index 142e6e9..0000000 --- a/src/manager/action/belt_pid_deceleration_action.hpp +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once - -#include "action.hpp" -#include -#include - -namespace rmcs_dart_guidance::manager { - -class BeltPIDDecelerationAction : public IAction { -public: - BeltPIDDecelerationAction( - std::string name, double& belt_target_velocity, double& belt_torque_offset, - const double& left_belt_velocity, const double& right_belt_velocity, - double torque_offset_value, double zero_velocity_threshold, uint64_t zero_confirm_ticks, - uint64_t timeout_ticks) - : IAction(std::move(name)) - , belt_target_velocity_(belt_target_velocity) - , belt_torque_offset_(belt_torque_offset) - , left_belt_velocity_(left_belt_velocity) - , right_belt_velocity_(right_belt_velocity) - , torque_offset_value_(torque_offset_value) - , zero_velocity_threshold_(zero_velocity_threshold) - , zero_confirm_ticks_(zero_confirm_ticks) - , timeout_ticks_(timeout_ticks) {} - - void on_enter() override { - belt_target_velocity_ = 0.0; - belt_torque_offset_ = torque_offset_value_; - zero_counter_ = 0; - } - - ActionStatus update() override { - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::SUCCESS; - } - - double avg_velocity = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - - if (avg_velocity < zero_velocity_threshold_) { - ++zero_counter_; - if (zero_counter_ >= zero_confirm_ticks_) { - return ActionStatus::SUCCESS; - } - } else { - zero_counter_ = 0; - } - - return ActionStatus::RUNNING; - } - - void on_exit() override { belt_torque_offset_ = 0.0; } - -private: - double& belt_target_velocity_; - double& belt_torque_offset_; - const double& left_belt_velocity_; - const double& right_belt_velocity_; - double torque_offset_value_; - double zero_velocity_threshold_; - uint64_t zero_confirm_ticks_; - uint64_t timeout_ticks_; - uint64_t zero_counter_{0}; -}; - -} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_up_initial_accel_action.hpp b/src/manager/action/belt_up_initial_accel_action.hpp new file mode 100644 index 0000000..ff24bfd --- /dev/null +++ b/src/manager/action/belt_up_initial_accel_action.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "action.hpp" + +#include +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +// BeltUpInitialAccelAction - 上行初始加速阶段 +// 前0.1m慢速（5 rad/s）+ 常态力矩偏移补偿负载 +// 完成条件：行进距离达到0.2m +class BeltUpInitialAccelAction : public IAction { +public: + BeltUpInitialAccelAction( + std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, + double& belt_torque_offset, double& belt_torque_limit, double& belt_error_gain, + bool& belt_use_decel_pid, const double& left_belt_angle, const double& right_belt_angle, + double pulley_radius, double slow_velocity, double torque_offset_value, double torque_limit, + double accel_distance = 0.1, uint64_t timeout_ticks = 5000) + : IAction(std::move(name)) + , belt_command_(belt_command) + , belt_target_velocity_(belt_target_velocity) + , belt_torque_offset_(belt_torque_offset) + , belt_torque_limit_(belt_torque_limit) + , belt_error_gain_(belt_error_gain) + , belt_use_decel_pid_(belt_use_decel_pid) + , left_belt_angle_(left_belt_angle) + , right_belt_angle_(right_belt_angle) + , pulley_radius_(pulley_radius) + , slow_velocity_(slow_velocity) + , torque_offset_value_(torque_offset_value) + , torque_limit_(torque_limit) + , accel_distance_(accel_distance) + , timeout_ticks_(timeout_ticks) {} + + void on_enter() override { + initial_angle_ = (left_belt_angle_ + right_belt_angle_) / 2.0; + belt_command_ = rmcs_msgs::DartSliderStatus::UP; + belt_target_velocity_ = slow_velocity_; + belt_torque_offset_ = torque_offset_value_; + belt_torque_limit_ = torque_limit_; + belt_error_gain_ = 1.0; // 恢复正常error增益 + belt_use_decel_pid_ = false; // 恢复正常PID参数 + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; + double distance_traveled = std::abs(avg_angle - initial_angle_) * pulley_radius_; + + if (distance_traveled >= accel_distance_) { + return ActionStatus::SUCCESS; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { belt_torque_offset_ = 0.0; } + +private: + rmcs_msgs::DartSliderStatus& belt_command_; + double& belt_target_velocity_; + double& belt_torque_offset_; + double& belt_torque_limit_; + double& belt_error_gain_; + bool& belt_use_decel_pid_; + const double& left_belt_angle_; + const double& right_belt_angle_; + + double pulley_radius_; + double slow_velocity_; + double torque_offset_value_; + double torque_limit_; + double accel_distance_; + uint64_t timeout_ticks_; + + double initial_angle_{0.0}; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index db39980..45dc871 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -76,6 +76,8 @@ class DartManager register_output("/dart/manager/belt/torque_offset", belt_torque_offset_, 0.0); register_output("/dart/manager/belt/wait_zero_velocity", belt_wait_zero_velocity_, false); register_output("/dart/manager/belt/zero_calibration", belt_zero_calibration_, false); + register_output("/dart/manager/belt/error_gain", belt_error_gain_, 1.0); + register_output("/dart/manager/belt/use_decel_pid", belt_use_decel_pid_, false); register_output("/dart/manager/trigger/lock_enable", trigger_lock_enable_, false); register_output( @@ -172,7 +174,7 @@ class DartManager belt_prepare_down_zero_confirm_ticks_ = (uint64_t)get_parameter("belt_prepare_down_zero_confirm_ticks").as_int(); } catch (...) { - belt_prepare_down_zero_confirm_ticks_ = 60; + belt_prepare_down_zero_confirm_ticks_ = 80; } try { belt_prepare_down_ramp_timeout_ticks_ = @@ -348,13 +350,20 @@ class DartManager } first_fill_pending_ = true; - fire_count_ = 0; // 重置开火次数 + fire_count_ = 0; // 重置开火次数 - enter_belt_wait_zero_velocity_mode(); + *belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; + *belt_target_velocity_ = 0.0; + *belt_torque_limit_ = 0.0; + *belt_hold_torque_ = 0.0; + *belt_torque_offset_ = 0.0; + *belt_wait_zero_velocity_ = false; + *belt_error_gain_ = 1.0; + *belt_use_decel_pid_ = false; *lifting_command_ = rmcs_msgs::DartSliderStatus::WAIT; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); - *force_control_velocity_ = 0.0; // 停止丝杆电机（已包含） + *force_control_velocity_ = 0.0; clear_auto_aim_feedback(); transition_to(State::IDLE); @@ -682,20 +691,20 @@ class DartManager return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, - *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, - *right_belt_torque_, *trigger_lock_enable_, belt_down_distance_, - belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, - belt_prepare_up_torque_limit_, belt_prepare_down_ramp_ticks_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *belt_error_gain_, + *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, + *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, + *trigger_lock_enable_, belt_down_distance_, belt_pulley_radius_, down_velocity, + belt_prepare_torque_limit_, belt_prepare_up_torque_limit_, belt_prepare_down_torque_offset_, belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, require_lifting_down, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_, - belt_up_distance_, belt_prepare_up_velocity_, belt_up_decel_target_velocity_, - belt_up_decel_torque_offset_, belt_up_stall_velocity_threshold_, - belt_up_stall_confirm_ticks_, belt_up_stall_min_run_ticks_, - belt_up_decel_timeout_ticks_, *force_control_velocity_, *current_force_ch1_, - last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, + belt_up_distance_, belt_up_decel_target_velocity_, belt_up_decel_torque_offset_, + belt_up_stall_velocity_threshold_, belt_up_stall_confirm_ticks_, + belt_up_stall_min_run_ticks_, belt_up_decel_timeout_ticks_, + *force_control_velocity_, *current_force_ch1_, last_fire_force_, + enable_force_calibration_, force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, is_first_shot); } @@ -707,12 +716,12 @@ class DartManager bool require_lifting_up = (fire_count_ > 0); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, - *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, - *right_belt_torque_, *trigger_lock_enable_, *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, - belt_pulley_radius_, down_velocity, belt_prepare_torque_limit_, - belt_prepare_up_torque_limit_, belt_prepare_down_ramp_ticks_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *belt_error_gain_, + *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, + *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, + *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, + *lifting_right_vel_fb_, belt_down_distance_, belt_pulley_radius_, down_velocity, + belt_prepare_torque_limit_, belt_prepare_up_torque_limit_, belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, *belt_zero_calibration_, require_lifting_up); @@ -777,6 +786,8 @@ class DartManager OutputInterface belt_torque_offset_; OutputInterface belt_wait_zero_velocity_; OutputInterface belt_zero_calibration_; + OutputInterface belt_error_gain_; + OutputInterface belt_use_decel_pid_; OutputInterface trigger_lock_enable_; OutputInterface yaw_pitch_control_velocity_; diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index e1477de..a0612ab 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -2,9 +2,9 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" +#include "manager/action/belt_deceleration_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" #include "manager/action/belt_move_action.hpp" -#include "manager/action/belt_pid_deceleration_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -30,12 +30,13 @@ class CancelLaunchTask : public Task { CancelLaunchTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, double belt_down_distance, double belt_pulley_radius, - double down_velocity, double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, + double down_velocity, double torque_limit, double up_torque_limit, double down_hold_torque, double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, bool& belt_zero_calibration, bool require_lifting_up = true) @@ -65,17 +66,29 @@ class CancelLaunchTask : public Task { 200, // 堵转确认帧数 0.20)); // 下行最大距离限制（m，正值） - // 步骤2：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） + // 步骤2：减速阶段（目标速度=0，加常态力矩偏移补偿负载，使用零速检测） then( - std::make_shared( + std::make_shared( "belt_down_ramp_to_zero", // 动作名称 - belt_target_velocity, // 目标速度（输出，设为0） + belt_target_velocity, // 目标速度（输出） belt_torque_offset, // 力矩偏移（输出） + belt_error_gain, // 误差增益（输出） + belt_use_decel_pid, // 使用减速PID（输出） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + 0.0, // 目标速度（rad/s） down_hold_torque, // 力矩偏移值（N⋅m） + 2.0, // 误差增益倍数 + true, // 启用堵转检测 + true, // 启用零速检测 + 0.3, // 堵转速度阈值（rad/s） + 1.0, // 堵转扭矩阈值（N⋅m） down_zero_velocity_threshold, // 零速阈值（rad/s） + 50, // 堵转确认帧数 down_zero_confirm_ticks, // 零速确认帧数 + 50, // 最小运行帧数 down_ramp_timeout_ticks)); // 超时帧数 // 步骤3：传送带保持高扭矩 + 解锁扳机 + (可选)升降上行并行 diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 0f8747b..b68a99e 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -2,10 +2,9 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" -#include "manager/action/belt_deceleration_with_stall_action.hpp" +#include "manager/action/belt_deceleration_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" -#include "manager/action/belt_move_action.hpp" -#include "manager/action/belt_pid_deceleration_action.hpp" +#include "manager/action/belt_up_initial_accel_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -27,23 +26,23 @@ class LaunchPreparationTask : public Task { LaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, + const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, double belt_down_distance, double belt_pulley_radius, double down_velocity, - double torque_limit, double up_torque_limit, uint64_t down_ramp_ticks, - double down_torque_offset, double down_hold_torque, double down_zero_velocity_threshold, + double torque_limit, double up_torque_limit, double down_torque_offset, + double down_hold_torque, double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - bool& belt_zero_calibration, double belt_up_distance, double up_velocity, - double up_decel_target_velocity, double up_decel_torque_offset, - double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, - uint64_t up_stall_min_run_ticks, uint64_t up_decel_timeout_ticks, - double& force_screw_velocity, const int& current_force, double target_force, - bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, - uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, - double force_max_velocity, bool is_first_shot = false) + bool& belt_zero_calibration, double belt_up_distance, double up_decel_target_velocity, + double up_decel_torque_offset, double up_stall_velocity_threshold, + uint64_t up_stall_confirm_ticks, uint64_t up_stall_min_run_ticks, + uint64_t up_decel_timeout_ticks, double& force_screw_velocity, const int& current_force, + double target_force, bool enable_force_calibration, double force_tolerance, + uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, + double force_kd, double force_max_velocity, bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -66,62 +65,64 @@ class LaunchPreparationTask : public Task { torque_limit, // 扭矩限制（N⋅m） 10000, // 超时帧数 100, // 最小运行帧数 - 0.5, // 位置到达容差（rad）- 增大容差 + 0.5, // 位置到达容差（rad） 0.3, // 堵转速度阈值（rad/s） 100, // 堵转确认帧数 0.80)); // 下行最大距离限制（m，正值） - // 步骤3：PID减速阶段（目标速度=0，加常态力矩偏移补偿负载） + // 步骤2：减速阶段（目标速度=0，加常态力矩偏移补偿负载，使用减速PID+error增益） + // 同时启用零速检测和堵转检测：正常减速到0或碰到机械限位都能成功 then( - std::make_shared( - "belt_pid_deceleration", // 动作名称 - belt_target_velocity, // 目标速度（输出，设为0） + std::make_shared( + "belt_deceleration", // 动作名称 + belt_target_velocity, // 目标速度（输出） belt_torque_offset, // 力矩偏移（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + 0.0, // 目标速度（rad/s） down_torque_offset, // 力矩偏移值（N⋅m） + 5.0, // error增益倍数（2倍） + true, // 启用堵转检测（碰到机械限位） + true, // 启用零速检测（正常减速到0） + 0.3, // 堵转速度阈值（rad/s） + 1.0, // 堵转扭矩阈值（N⋅m） down_zero_velocity_threshold, // 零速阈值（rad/s） - 100, // 零速确认帧数 - 1000)); // 超时帧数 + 50, // 堵转确认帧数 + down_zero_confirm_ticks, // 零速确认帧数 + 50, // 最小运行帧数 + down_ramp_timeout_ticks)); // 超时帧数 // 步骤3：根据是否第一发选择不同的并行动作 - if (is_first_shot) { - // 第一发：传送带保持高扭矩 + 扳机锁定并行（无升降下行） - auto parallel_hold_and_lock = - std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); + // if (is_first_shot) { + // // 第一发：传送带保持高扭矩 + 扳机锁定 + // auto parallel_hold_and_lock = + // std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); - parallel_hold_and_lock - ->also( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 750)) - .also( - std::make_shared( - "belt_pid_deceleration", // 动作名称 - belt_target_velocity, // 目标速度（输出，设为0） - belt_torque_offset, // 力矩偏移（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - down_torque_offset, // 力矩偏移值（N⋅m） - down_zero_velocity_threshold, // 零速阈值（rad/s） - 10, // 零速确认帧数 - 3000)); // 超时帧数 - then(parallel_hold_and_lock); + // parallel_hold_and_lock + // ->also( + // std::make_shared( + // "belt_hold_torque", // 动作名称 + // belt_command, // 传送带命令（输出） + // belt_target_velocity, // 目标速度（输出） + // belt_hold_torque, // 保持力矩（输出） + // belt_wait_zero_velocity, // WAIT模式选择（输出） + // belt_torque_offset, // 力矩偏移（输出） + // down_hold_torque, // 保持力矩值（N⋅m） + // down_torque_offset, // 力矩偏移值（N⋅m） + // 1000)) // 保持时长（ticks） + // .also( + // std::make_shared( + // trigger_lock_enable, // 扳机锁定使能（输出） + // true, // 锁定（true） + // 1000)); // 等待锁定完成帧数 + // then(parallel_hold_and_lock); - then( - std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - down_torque_offset, // 力矩偏移值（N⋅m） - 500)); // 保持时长（ticks） - - } else if (require_lifting_down) { + // } else + if (require_lifting_down) { // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 auto parallel_hold_lock_and_lift = std::make_shared("hold_lock_and_lift", ActionSet::Policy::ALL_SUCCESS); @@ -180,71 +181,89 @@ class LaunchPreparationTask : public Task { then(parallel_hold_and_lock); } - // 步骤4：传送带上行到目标位置（位置控制） - // 目标位置略低于下行起点（比如 -0.65m，而下行是 0.70m） + // 步骤4a：上行初始加速阶段（前0.2m慢速 + 常态力矩偏移） + then( + std::make_shared( + "belt_up_initial_accel", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + belt_pulley_radius, // 滑轮半径（m） + 5.0, // 慢速（rad/s） + 5.0, // 力矩偏移值（N⋅m） + up_torque_limit, // 扭矩限制（N⋅m） + 0.05, // 加速距离（m） + 5000)); // 超时帧数 + + // 步骤4b：上行匀速阶段（到达目标位置前，无力矩偏移） + // 目标位置 = 初始位置 - (belt_up_distance - 0.2) then( std::make_shared( - "belt_reset_up_position", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - 5.0, // 力矩偏移值 - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - -belt_up_distance, // 目标距离（m，负值=上行） - belt_pulley_radius, // 滑轮半径（m） - up_velocity, // 运动速度（rad/s） - up_torque_limit, // 扭矩限制（N⋅m） - 15000, // 超时帧数 - 50, // 最小运行帧数 - 0.5, // 位置到达容差（rad） - 0.3, // 堵转速度阈值（rad/s） - 100, // 堵转确认帧数 - 0.80)); // 下行最大距离限制（m，正值） - - // 步骤5：减速并监测堵转（堵转标志成功，作为下次下行起点） + "belt_up_cruise", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值（无） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.2), // 目标距离（m，负值=上行，减去已行进的0.2m） + belt_pulley_radius, // 滑轮半径（m） + 15.0, // 快速（rad/s） + up_torque_limit, // 扭矩限制（N⋅m） + 10000, // 超时帧数 + 50, // 最小运行帧数 + 0.5, // 位置到达容差（rad） + 0.3, // 堵转速度阈值（rad/s） + 100, // 堵转确认帧数 + 0.80)); // 下行最大距离限制（m，正值） + + // 步骤4c：上行减速+堵转检测（堵转标志成功，作为下次下行起点，无力矩偏移） then( - std::make_shared( - "belt_decel_and_stall", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - 2.0, // 目标速度（rad/s） - up_decel_torque_offset, // 力矩偏移（N⋅m） - 0.5, // 堵转速度阈值（rad/s） - 0.5, // 堵转扭矩阈值（N⋅m） - 100, // 堵转确认帧数 - 10, // 最小运行帧数 - 5000)); // 超时帧数 + std::make_shared( + "belt_up_decel_and_stall", // 动作名称 + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + up_decel_target_velocity, // 目标速度（rad/s） + up_decel_torque_offset, // 力矩偏移值（无） + 1.0, // error增益倍数（正常） + true, // 启用堵转检测 + false, // 不启用零速检测 + up_stall_velocity_threshold, // 堵转速度阈值（rad/s） + 0.5, // 堵转扭矩阈值（N⋅m） + 0.1, // 零速阈值（未使用） + up_stall_confirm_ticks, // 堵转确认帧数 + 100, // 零速确认帧数（未使用） + up_stall_min_run_ticks, // 最小运行帧数 + up_decel_timeout_ticks)); // 超时帧数 then(std::make_shared("stabilize_wait", 50)); then(std::make_shared(belt_zero_calibration)); - // 力矩闭环：第一发使用固定目标力9525，后续发使用上次记录的力值 + // 力矩闭环：第一发使用固定目标力，后续发使用上次记录的力值 // if (enable_force_calibration) { // double target_force_value = is_first_shot ? 11300.0 : target_force; // double tolerance_value = is_first_shot ? 3.0 : force_tolerance; // then( // std::make_shared( - // "force_screw_calibration", // 动作名称 - // force_screw_velocity, // 丝杆电机速度（输出） - // current_force, // 当前力传感器读数（输入） - // target_force_value, // 目标力值（第一发9025，后续用记录值） - // tolerance_value, // 力容差（第一发±30，后续用配置值） - // force_settle_ticks, // 稳定确认帧数 - // force_timeout_ticks, // 超时帧数 - // force_kp, // PID参数Kp - // force_ki, // PID参数Ki - // force_kd, // PID参数Kd - // force_max_velocity)); // 最大速度限制 + // "force_screw_calibration", force_screw_velocity, current_force, + // target_force_value, tolerance_value, force_settle_ticks, force_timeout_ticks, + // force_kp, force_ki, force_kd, force_max_velocity)); // } } }; From 0eaf7d546cdd22bd2a7ea493c45903840a752240 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 5 Apr 2026 22:19:15 +0800 Subject: [PATCH 10/19] delay pronlem fixed.added protection to manual force control --- CMakeLists.txt | 8 ++- src/manager/action/manual_force_control.hpp | 61 +++++++++++++++++-- src/manager/dart_manager.cpp | 48 ++++++++++++++- tests/manual_force_control_test.cpp | 65 +++++++++++++++++++++ 4 files changed, 175 insertions(+), 7 deletions(-) create mode 100644 tests/manual_force_control_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 086cf06..3c27b65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,11 +126,17 @@ if(BUILD_TESTING) tests/dart_launch_sequence_test.cpp ) + ament_add_gtest( + manual_force_control_test + tests/manual_force_control_test.cpp + ) + foreach(target auto_aim_action_test launch_preparation_task_test vision_assisted_launch_preparation_task_test - dart_launch_sequence_test) + dart_launch_sequence_test + manual_force_control_test) if(TARGET ${target}) target_include_directories(${target} PRIVATE ${PROJECT_SOURCE_DIR}/src/ diff --git a/src/manager/action/manual_force_control.hpp b/src/manager/action/manual_force_control.hpp index e7679fa..98feef2 100644 --- a/src/manager/action/manual_force_control.hpp +++ b/src/manager/action/manual_force_control.hpp @@ -2,37 +2,88 @@ #include "action.hpp" +#include +#include + #include namespace rmcs_dart_guidance::manager { // 手动力控：右摇杆控制拉力电机速度。 -// 任务将持续运行，直到被外部模式切换指令抢占取消。 +// 默认持续运行；若检测到丝杆堵转，则返回 SUCCESS 并停车。 class DartManualForceControlAction : public IAction { public: DartManualForceControlAction( double& force_control_velocity, const Eigen::Vector2d& joystick_right, - double max_transform_rate, double manual_force_scale = 5.0) + double max_transform_rate, double manual_force_scale = 5.0, + const double* force_screw_velocity_feedback = nullptr, + const double* force_screw_torque_feedback = nullptr, + double stall_velocity_threshold = 0.15, double stall_torque_threshold = 0.5, + uint64_t stall_confirm_ticks = 100, uint64_t min_running_ticks = 100) : IAction("dart_manual_force_control") , force_control_velocity_(force_control_velocity) , joystick_right_(joystick_right) , max_transform_rate_(max_transform_rate) - , manual_force_scale_(manual_force_scale) {} + , manual_force_scale_(manual_force_scale) + , force_screw_velocity_feedback_(force_screw_velocity_feedback) + , force_screw_torque_feedback_(force_screw_torque_feedback) + , stall_velocity_threshold_(stall_velocity_threshold) + , stall_torque_threshold_(stall_torque_threshold) + , stall_confirm_ticks_(stall_confirm_ticks) + , min_running_ticks_(min_running_ticks) {} - void on_enter() override {} + void on_enter() override { + force_control_velocity_ = 0.0; + stall_counter_ = 0; + } ActionStatus update() override { - force_control_velocity_ = joystick_right_.x() * max_transform_rate_ * manual_force_scale_; + const double target_velocity = + joystick_right_.x() * max_transform_rate_ * manual_force_scale_; + force_control_velocity_ = target_velocity; + + if (std::abs(target_velocity) <= 1e-6 || force_screw_velocity_feedback_ == nullptr + || force_screw_torque_feedback_ == nullptr) { + stall_counter_ = 0; + return ActionStatus::RUNNING; + } + + if (elapsed_ticks() > min_running_ticks_) { + const double measured_velocity = std::abs(*force_screw_velocity_feedback_); + const double measured_torque = std::abs(*force_screw_torque_feedback_); + + if (measured_velocity < stall_velocity_threshold_ + && measured_torque > stall_torque_threshold_) { + ++stall_counter_; + if (stall_counter_ >= stall_confirm_ticks_) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + } return ActionStatus::RUNNING; } + void on_exit() override { + force_control_velocity_ = 0.0; + stall_counter_ = 0; + } + private: double& force_control_velocity_; const Eigen::Vector2d& joystick_right_; double max_transform_rate_; double manual_force_scale_; + const double* force_screw_velocity_feedback_; + const double* force_screw_torque_feedback_; + double stall_velocity_threshold_; + double stall_torque_threshold_; + uint64_t stall_confirm_ticks_; + uint64_t min_running_ticks_; + uint64_t stall_counter_{0}; }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 45dc871..1d3dc21 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -57,6 +57,8 @@ class DartManager register_input("/dart/drive_belt/right/angle", right_belt_angle_); register_input("/dart/lifting_left/velocity", lifting_left_vel_fb_); register_input("/dart/lifting_right/velocity", lifting_right_vel_fb_); + register_input("/dart/force_screw_motor/velocity", force_screw_velocity_fb_); + register_input("/dart/force_screw_motor/torque", force_screw_torque_fb_); register_input("/force_sensor/channel_1/weight", current_force_ch1_); register_input("/force_sensor/channel_2/weight", current_force_ch2_); @@ -279,6 +281,30 @@ class DartManager } catch (...) { force_max_velocity_ = 5.0; // rad/s } + try { + force_stall_velocity_threshold_ = + get_parameter("force_stall_velocity_threshold").as_double(); + } catch (...) { + force_stall_velocity_threshold_ = 0.15; + } + try { + force_stall_torque_threshold_ = + get_parameter("force_stall_torque_threshold").as_double(); + } catch (...) { + force_stall_torque_threshold_ = 0.5; + } + try { + force_stall_confirm_ticks_ = + (uint64_t)get_parameter("force_stall_confirm_ticks").as_int(); + } catch (...) { + force_stall_confirm_ticks_ = 100; + } + try { + force_stall_min_run_ticks_ = + (uint64_t)get_parameter("force_stall_min_run_ticks").as_int(); + } catch (...) { + force_stall_min_run_ticks_ = 100; + } state_pub_ = create_publisher("/dart/manager/state", 10); @@ -747,11 +773,25 @@ class DartManager } if (cmd == "manual_force") { + const double* force_screw_velocity_feedback = + force_screw_velocity_fb_.ready() ? &*force_screw_velocity_fb_ : nullptr; + const double* force_screw_torque_feedback = + force_screw_torque_fb_.ready() ? &*force_screw_torque_fb_ : nullptr; + + if (force_screw_velocity_feedback == nullptr || force_screw_torque_feedback == nullptr) { + RCLCPP_WARN( + logger_, + "[DartManager] force screw feedback not ready, manual_force stall detection disabled"); + } + auto task = std::make_shared("manual_force", "手动力丝杆速度调整"); task->then( std::make_shared( *force_control_velocity_, *joystick_right_, max_transform_rate_, - manual_force_scale_)); + manual_force_scale_, force_screw_velocity_feedback, + force_screw_torque_feedback, force_stall_velocity_threshold_, + force_stall_torque_threshold_, force_stall_confirm_ticks_, + force_stall_min_run_ticks_)); return task; } return nullptr; @@ -774,6 +814,8 @@ class DartManager // 升降速度反馈（FillingLiftAction 堵转检测用） InputInterface lifting_left_vel_fb_; InputInterface lifting_right_vel_fb_; + InputInterface force_screw_velocity_fb_; + InputInterface force_screw_torque_fb_; // 力传感器反馈（两个通道） InputInterface current_force_ch1_; @@ -843,6 +885,10 @@ class DartManager double force_ki_{0.0}; double force_kd_{0.01}; double force_max_velocity_{5.0}; // rad/s + double force_stall_velocity_threshold_{0.15}; + double force_stall_torque_threshold_{0.5}; + uint64_t force_stall_confirm_ticks_{100}; + uint64_t force_stall_min_run_ticks_{100}; bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; diff --git a/tests/manual_force_control_test.cpp b/tests/manual_force_control_test.cpp new file mode 100644 index 0000000..e12a5d5 --- /dev/null +++ b/tests/manual_force_control_test.cpp @@ -0,0 +1,65 @@ +#include "manager/action/manual_force_control.hpp" + +#include + +#include + +namespace rmcs_dart_guidance::manager::test { + +TEST(DartManualForceControlActionTest, OutputsScaledVelocityFromJoystick) { + double force_control_velocity = 0.0; + Eigen::Vector2d joystick_right(0.4, 0.0); + + DartManualForceControlAction action(force_control_velocity, joystick_right, 5.0, 3.0); + + EXPECT_EQ(action.tick_first(), ActionStatus::RUNNING); + EXPECT_DOUBLE_EQ(force_control_velocity, 6.0); + + action.on_exit(); + EXPECT_DOUBLE_EQ(force_control_velocity, 0.0); +} + +TEST(DartManualForceControlActionTest, ReturnsSuccessAfterConfirmedStall) { + double force_control_velocity = 0.0; + Eigen::Vector2d joystick_right(1.0, 0.0); + double measured_velocity = 0.0; + double measured_torque = 0.8; + + DartManualForceControlAction action( + force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque, + 0.15, 0.5, 3, 0); + + EXPECT_EQ(action.tick_first(), ActionStatus::RUNNING); + EXPECT_DOUBLE_EQ(force_control_velocity, 15.0); + + EXPECT_EQ(action.tick(), ActionStatus::RUNNING); + EXPECT_EQ(action.tick(), ActionStatus::SUCCESS); + + action.on_exit(); + EXPECT_DOUBLE_EQ(force_control_velocity, 0.0); +} + +TEST(DartManualForceControlActionTest, ResetsStallCounterWhenCommandDropsToZero) { + double force_control_velocity = 0.0; + Eigen::Vector2d joystick_right(1.0, 0.0); + double measured_velocity = 0.0; + double measured_torque = 0.8; + + DartManualForceControlAction action( + force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque, + 0.15, 0.5, 3, 0); + + EXPECT_EQ(action.tick_first(), ActionStatus::RUNNING); + EXPECT_EQ(action.tick(), ActionStatus::RUNNING); + + joystick_right.x() = 0.0; + EXPECT_EQ(action.tick(), ActionStatus::RUNNING); + EXPECT_DOUBLE_EQ(force_control_velocity, 0.0); + + joystick_right.x() = 1.0; + EXPECT_EQ(action.tick(), ActionStatus::RUNNING); + EXPECT_EQ(action.tick(), ActionStatus::RUNNING); + EXPECT_EQ(action.tick(), ActionStatus::SUCCESS); +} + +} // namespace rmcs_dart_guidance::manager::test From e56a84b808fefc0f9c88e36c55f3286085d30e37 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Mon, 6 Apr 2026 15:52:36 +0800 Subject: [PATCH 11/19] added intergral limit --- src/manager/action/action_sequence.hpp | 46 +++++++++---------- .../action/belt_deceleration_action.hpp | 22 ++++++++- .../action/belt_hold_torque_action.hpp | 5 +- src/manager/dart_manager.cpp | 3 ++ src/manager/task/launch_preparation_task.hpp | 13 ++++-- 5 files changed, 54 insertions(+), 35 deletions(-) diff --git a/src/manager/action/action_sequence.hpp b/src/manager/action/action_sequence.hpp index 9db5c83..b533dd4 100644 --- a/src/manager/action/action_sequence.hpp +++ b/src/manager/action/action_sequence.hpp @@ -46,36 +46,34 @@ class ActionSequence : public IAction { if (cursor_ >= actions_.size()) return ActionStatus::SUCCESS; - auto& action_ptr = actions_[cursor_]; - if (!action_ptr) { - // Defensive check: if somehow a null action got in, skip it - ++cursor_; - first_tick_of_current_ = true; - return ActionStatus::RUNNING; - } - auto& current = *action_ptr; + while (cursor_ < actions_.size()) { + auto& action_ptr = actions_[cursor_]; + if (!action_ptr) { + ++cursor_; + first_tick_of_current_ = true; + continue; + } - // 首帧调用 tick_first，后续调用 tick - ActionStatus status = first_tick_of_current_ ? current.tick_first() : current.tick(); - first_tick_of_current_ = false; + auto& current = *action_ptr; + ActionStatus status = first_tick_of_current_ ? current.tick_first() : current.tick(); + first_tick_of_current_ = false; - if (status == ActionStatus::SUCCESS) { - current.on_exit(); - ++cursor_; - if (cursor_ >= actions_.size()) { - return ActionStatus::SUCCESS; + if (status == ActionStatus::SUCCESS) { + current.on_exit(); + ++cursor_; + first_tick_of_current_ = true; + continue; } - // 下一个动作首帧标记 - first_tick_of_current_ = true; - return ActionStatus::RUNNING; - } - if (status == ActionStatus::FAILURE) { - current.cancel(); - return ActionStatus::FAILURE; + if (status == ActionStatus::FAILURE) { + current.cancel(); + return ActionStatus::FAILURE; + } + + return ActionStatus::RUNNING; } - return ActionStatus::RUNNING; + return ActionStatus::SUCCESS; } void on_exit() override { diff --git a/src/manager/action/belt_deceleration_action.hpp b/src/manager/action/belt_deceleration_action.hpp index 0bf13d5..cdc8d17 100644 --- a/src/manager/action/belt_deceleration_action.hpp +++ b/src/manager/action/belt_deceleration_action.hpp @@ -1,6 +1,9 @@ #pragma once #include "action.hpp" + +#include + #include #include @@ -37,7 +40,9 @@ class BeltDecelerationAction : public IAction { double stall_velocity_threshold = 0.5, double stall_torque_threshold = 0.5, double zero_velocity_threshold = 0.1, uint64_t stall_confirm_ticks = 100, uint64_t zero_confirm_ticks = 100, uint64_t min_running_ticks = 50, - uint64_t timeout_ticks = 3000) + uint64_t timeout_ticks = 3000, + rmcs_msgs::DartSliderStatus* belt_command = nullptr, + bool* belt_wait_zero_velocity = nullptr, bool send_wait_zero_on_exit = false) : IAction(std::move(name)) , belt_target_velocity_(belt_target_velocity) , belt_torque_offset_(belt_torque_offset) @@ -58,7 +63,10 @@ class BeltDecelerationAction : public IAction { , stall_confirm_ticks_(stall_confirm_ticks) , zero_confirm_ticks_(zero_confirm_ticks) , min_running_ticks_(min_running_ticks) - , timeout_ticks_(timeout_ticks) {} + , timeout_ticks_(timeout_ticks) + , belt_command_(belt_command) + , belt_wait_zero_velocity_(belt_wait_zero_velocity) + , send_wait_zero_on_exit_(send_wait_zero_on_exit) {} void on_enter() override { belt_target_velocity_ = target_velocity_; @@ -113,6 +121,13 @@ class BeltDecelerationAction : public IAction { } void on_exit() override { + if (send_wait_zero_on_exit_ && belt_command_ != nullptr) { + *belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; + belt_target_velocity_ = 0.0; + if (belt_wait_zero_velocity_ != nullptr) { + *belt_wait_zero_velocity_ = true; + } + } belt_torque_offset_ = 0.0; belt_error_gain_ = 1.0; belt_use_decel_pid_ = false; // 恢复正常PID参数 @@ -140,6 +155,9 @@ class BeltDecelerationAction : public IAction { uint64_t zero_confirm_ticks_; uint64_t min_running_ticks_; uint64_t timeout_ticks_; + rmcs_msgs::DartSliderStatus* belt_command_; + bool* belt_wait_zero_velocity_; + bool send_wait_zero_on_exit_; uint64_t stall_counter_{0}; uint64_t zero_counter_{0}; diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index 703b2a7..f3c902f 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -45,10 +45,7 @@ class BeltHoldTorqueAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override { - belt_torque_offset_ = 0.0; // 退出时清除扭矩偏移 - belt_wait_zero_velocity_ = false; // 退出零速度闭环模式 - } + void on_exit() override {} private: rmcs_msgs::DartSliderStatus& belt_command_; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 1d3dc21..2e764b5 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -488,7 +488,10 @@ class DartManager *belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; *belt_target_velocity_ = 0.0; *belt_hold_torque_ = 0.0; + *belt_torque_offset_ = 0.0; *belt_wait_zero_velocity_ = true; + *belt_error_gain_ = 1.0; + *belt_use_decel_pid_ = false; } void transition_to(State new_state) { diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index b68a99e..23da1b6 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -94,7 +94,7 @@ class LaunchPreparationTask : public Task { 50, // 堵转确认帧数 down_zero_confirm_ticks, // 零速确认帧数 50, // 最小运行帧数 - down_ramp_timeout_ticks)); // 超时帧数 + 800)); // 超时帧数 // 步骤3：根据是否第一发选择不同的并行动作 // if (is_first_shot) { @@ -195,7 +195,7 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） belt_pulley_radius, // 滑轮半径（m） 5.0, // 慢速（rad/s） - 5.0, // 力矩偏移值（N⋅m） + 3.0, // 力矩偏移值（N⋅m） up_torque_limit, // 扭矩限制（N⋅m） 0.05, // 加速距离（m） 5000)); // 超时帧数 @@ -223,7 +223,7 @@ class LaunchPreparationTask : public Task { 0.5, // 位置到达容差（rad） 0.3, // 堵转速度阈值（rad/s） 100, // 堵转确认帧数 - 0.80)); // 下行最大距离限制（m，正值） + 0.90)); // 下行最大距离限制（m，正值） // 步骤4c：上行减速+堵转检测（堵转标志成功，作为下次下行起点，无力矩偏移） then( @@ -238,7 +238,7 @@ class LaunchPreparationTask : public Task { left_belt_torque, // 左电机力矩反馈（输入） right_belt_torque, // 右电机力矩反馈（输入） up_decel_target_velocity, // 目标速度（rad/s） - up_decel_torque_offset, // 力矩偏移值（无） + 0.0, // 力矩偏移值（无） 1.0, // error增益倍数（正常） true, // 启用堵转检测 false, // 不启用零速检测 @@ -248,7 +248,10 @@ class LaunchPreparationTask : public Task { up_stall_confirm_ticks, // 堵转确认帧数 100, // 零速确认帧数（未使用） up_stall_min_run_ticks, // 最小运行帧数 - up_decel_timeout_ticks)); // 超时帧数 + 5000, // 超时帧数 + &belt_command, // 仅上行减速结束时显式下发 WAIT + &belt_wait_zero_velocity, // 上行减速结束后显式进入 WAIT_ZERO + true)); // 仅上行减速结束时显式下发 WAIT/0 then(std::make_shared("stabilize_wait", 50)); From a8d6775b92acc02ebe982b8a87f9da69042ad906 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sat, 11 Apr 2026 23:23:02 +0800 Subject: [PATCH 12/19] optimalized force calibration.can choose ch1 or 2, need to be tested~ --- .../action/belt_hold_torque_action.hpp | 3 +- .../belt_launch_prepare_program_action.hpp | 214 ++++++++++++++++++ .../action/force_screw_calibration_action.hpp | 20 +- src/manager/dart_manager.cpp | 29 ++- src/manager/task/launch_preparation_task.hpp | 58 ++--- 5 files changed, 268 insertions(+), 56 deletions(-) create mode 100644 src/manager/action/belt_launch_prepare_program_action.hpp diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index f3c902f..e3fd433 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -29,11 +29,10 @@ class BeltHoldTorqueAction : public IAction { , hold_duration_ticks_(hold_duration_ticks) {} void on_enter() override { - // 设置WAIT命令和零速度闭环模式 belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; belt_target_velocity_ = 0.0; belt_hold_torque_ = hold_torque_value_; - belt_wait_zero_velocity_ = true; // 使用零速度闭环 + belt_wait_zero_velocity_ = false; // 使用 HOLD_TORQUE 模式（常数力矩） belt_torque_offset_ = torque_offset_value_; // 叠加常态扭矩偏移补偿负载 } diff --git a/src/manager/action/belt_launch_prepare_program_action.hpp b/src/manager/action/belt_launch_prepare_program_action.hpp new file mode 100644 index 0000000..5aca12a --- /dev/null +++ b/src/manager/action/belt_launch_prepare_program_action.hpp @@ -0,0 +1,214 @@ +#pragma once + +#include "action.hpp" + +#include +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +// BeltLaunchPrepareProgramAction +// 触发 controller 内部的传送带阶段程序（LaunchPreparationTask 专用），并在 manager 侧基于反馈等待其完成。 +// +// 说明： +// - 为避免 rmcs_executor 的循环依赖（manager <-> controller），该 Action 不读取 controller 的 program_done。 +// - 完成条件严格复刻此前 manager 内 BeltDecelerationAction 的检测逻辑（零速/堵转）。 +// - 速度/距离/切换点等阶段字面量由 controller 内部实现；manager 仅注入力矩补偿与力矩限幅。 +class BeltLaunchPrepareProgramAction : public IAction { +public: + enum class Program : uint8_t { + LAUNCH_PREP_DOWN = 1, + LAUNCH_PREP_UP = 2, + }; + + BeltLaunchPrepareProgramAction( + std::string name, uint8_t& belt_program_cmd, bool& belt_program_first_shot, + double& belt_program_down_comp_torque, double& belt_program_up_comp_torque, + double& belt_torque_limit, rmcs_msgs::DartSliderStatus& belt_command, + double& belt_target_velocity, double& belt_hold_torque, bool& belt_wait_zero_velocity, + double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, + const double& left_belt_velocity, const double& right_belt_velocity, + const double& left_belt_torque, const double& right_belt_torque, Program program, + bool first_shot, double down_comp_torque_value, double up_comp_torque_value, + double torque_limit_value, uint64_t timeout_ticks) + : IAction(std::move(name)) + , belt_program_cmd_(belt_program_cmd) + , belt_program_first_shot_(belt_program_first_shot) + , belt_program_down_comp_torque_(belt_program_down_comp_torque) + , belt_program_up_comp_torque_(belt_program_up_comp_torque) + , belt_torque_limit_(belt_torque_limit) + , belt_command_(belt_command) + , belt_target_velocity_(belt_target_velocity) + , belt_hold_torque_(belt_hold_torque) + , belt_wait_zero_velocity_(belt_wait_zero_velocity) + , belt_torque_offset_(belt_torque_offset) + , belt_error_gain_(belt_error_gain) + , belt_use_decel_pid_(belt_use_decel_pid) + , left_belt_velocity_(left_belt_velocity) + , right_belt_velocity_(right_belt_velocity) + , left_belt_torque_(left_belt_torque) + , right_belt_torque_(right_belt_torque) + , program_(program) + , first_shot_(first_shot) + , down_comp_torque_value_(down_comp_torque_value) + , up_comp_torque_value_(up_comp_torque_value) + , torque_limit_value_(torque_limit_value) + , timeout_ticks_(timeout_ticks) {} + + void on_enter() override { + belt_program_cmd_ = static_cast(program_); + belt_program_first_shot_ = first_shot_; + belt_program_down_comp_torque_ = down_comp_torque_value_; + belt_program_up_comp_torque_ = up_comp_torque_value_; + belt_torque_limit_ = torque_limit_value_; + + stall_counter_ = 0; + zero_counter_ = 0; + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + const double avg_velocity = + (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; + + if (program_ == Program::LAUNCH_PREP_DOWN) { + return update_down(avg_velocity); + } + + return update_up(avg_velocity); + } + + void on_exit() override { + belt_program_cmd_ = 0; + + if (program_ == Program::LAUNCH_PREP_UP) { + // 复刻原 BeltDecelerationAction(send_wait_zero_on_exit=true) 的退出效果： + // 1) 显式下发 WAIT/0 + // 2) 切换到 WAIT_ZERO（零速闭环） + // 3) 清理 torque_offset/error_gain/use_decel_pid + belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; + belt_target_velocity_ = 0.0; + belt_hold_torque_ = 0.0; + belt_wait_zero_velocity_ = true; + belt_torque_offset_ = 0.0; + belt_error_gain_ = 1.0; + belt_use_decel_pid_ = false; + return; + } + + // 下行程序结束后下一步立刻进入 hold/lock；这里仅做最小清理，避免残留。 + belt_torque_offset_ = 0.0; + belt_error_gain_ = 1.0; + belt_use_decel_pid_ = false; + belt_wait_zero_velocity_ = false; + } + +private: + ActionStatus update_down(double avg_velocity) { + // 复刻 LaunchPreparationTask 内 BeltDecelerationAction 的检测参数（下行减速）： + // - stall: vel < 0.3 & torque_active, confirm 50 + // - zero : vel < 0.05 & !torque_active, confirm 60 + // - min_running_ticks: 50 + static constexpr uint64_t kMinRunningTicks = 50; + static constexpr double kStallVelocityThreshold = 0.3; + static constexpr double kStallTorqueThreshold = 1.0; + static constexpr uint64_t kStallConfirmTicks = 50; + static constexpr double kZeroVelocityThreshold = 0.05; + static constexpr uint64_t kZeroConfirmTicks = 60; + + const bool torque_active = + std::abs(left_belt_torque_) > kStallTorqueThreshold + || std::abs(right_belt_torque_) > kStallTorqueThreshold; + + if (elapsed_ticks() > kMinRunningTicks) { + if (avg_velocity < kStallVelocityThreshold && torque_active) { + ++stall_counter_; + if (stall_counter_ >= kStallConfirmTicks) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + + if (!torque_active) { + if (avg_velocity < kZeroVelocityThreshold) { + ++zero_counter_; + if (zero_counter_ >= kZeroConfirmTicks) { + return ActionStatus::SUCCESS; + } + } else { + zero_counter_ = 0; + } + } else { + zero_counter_ = 0; + } + } + + return ActionStatus::RUNNING; + } + + ActionStatus update_up(double avg_velocity) { + // 复刻 LaunchPreparationTask 内 BeltDecelerationAction 的检测参数（上行减速+堵转）： + // - stall: vel < 0.15 & torque_active, confirm 100 + // - min_running_ticks: 300 + static constexpr uint64_t kMinRunningTicks = 300; + static constexpr double kStallVelocityThreshold = 0.15; + static constexpr double kStallTorqueThreshold = 0.5; + static constexpr uint64_t kStallConfirmTicks = 100; + + const bool torque_active = + std::abs(left_belt_torque_) > kStallTorqueThreshold + || std::abs(right_belt_torque_) > kStallTorqueThreshold; + + if (elapsed_ticks() > kMinRunningTicks) { + if (avg_velocity < kStallVelocityThreshold && torque_active) { + ++stall_counter_; + if (stall_counter_ >= kStallConfirmTicks) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + } + + return ActionStatus::RUNNING; + } + + uint8_t& belt_program_cmd_; + bool& belt_program_first_shot_; + double& belt_program_down_comp_torque_; + double& belt_program_up_comp_torque_; + double& belt_torque_limit_; + + rmcs_msgs::DartSliderStatus& belt_command_; + double& belt_target_velocity_; + double& belt_hold_torque_; + bool& belt_wait_zero_velocity_; + double& belt_torque_offset_; + double& belt_error_gain_; + bool& belt_use_decel_pid_; + + const double& left_belt_velocity_; + const double& right_belt_velocity_; + const double& left_belt_torque_; + const double& right_belt_torque_; + + Program program_; + bool first_shot_; + double down_comp_torque_value_; + double up_comp_torque_value_; + double torque_limit_value_; + uint64_t timeout_ticks_; + + uint64_t stall_counter_{0}; + uint64_t zero_counter_{0}; +}; + +} // namespace rmcs_dart_guidance::manager + diff --git a/src/manager/action/force_screw_calibration_action.hpp b/src/manager/action/force_screw_calibration_action.hpp index 35bb3ac..441e6da 100644 --- a/src/manager/action/force_screw_calibration_action.hpp +++ b/src/manager/action/force_screw_calibration_action.hpp @@ -11,13 +11,17 @@ namespace rmcs_dart_guidance::manager { // 电机正转力增大，反转力减小 class ForceScrewCalibrationAction : public IAction { public: + // force_channel: 1 = ch1, 2 = ch2 ForceScrewCalibrationAction( - std::string name, double& force_screw_velocity, const int& current_force, - double target_force, double force_tolerance, uint64_t settle_ticks, uint64_t timeout_ticks, - double kp, double ki, double kd, double max_velocity) + std::string name, double& force_screw_velocity, const int& current_force_ch1, + const int& current_force_ch2, int force_channel, double target_force, + double force_tolerance, uint64_t settle_ticks, uint64_t timeout_ticks, double kp, + double ki, double kd, double max_velocity) : IAction(std::move(name)) , force_screw_velocity_(force_screw_velocity) - , current_force_(current_force) + , current_force_ch1_(current_force_ch1) + , current_force_ch2_(current_force_ch2) + , force_channel_(force_channel) , target_force_(target_force) , force_tolerance_(force_tolerance) , settle_ticks_(settle_ticks) @@ -38,7 +42,9 @@ class ForceScrewCalibrationAction : public IAction { return ActionStatus::SUCCESS; } - double error = (target_force_ - static_cast(current_force_)) * 5; // 增加响应速度 + double current_force = + static_cast(force_channel_ == 2 ? current_force_ch2_ : current_force_ch1_); + double error = (target_force_ - current_force) * 5; // 增加响应速度 integral_ += error; double derivative = error - last_error_; @@ -74,7 +80,9 @@ class ForceScrewCalibrationAction : public IAction { private: double& force_screw_velocity_; - const int& current_force_; + const int& current_force_ch1_; + const int& current_force_ch2_; + int force_channel_; // 1 = ch1, 2 = ch2 double target_force_; double force_tolerance_; uint64_t settle_ticks_; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 2e764b5..c947bf3 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -305,6 +305,17 @@ class DartManager } catch (...) { force_stall_min_run_ticks_ = 100; } + try { + force_channel_ = get_parameter("force_channel").as_int(); + if (force_channel_ != 1 && force_channel_ != 2) { + RCLCPP_WARN( + get_logger(), "[DartManager] force_channel=%d invalid, defaulting to 1", + force_channel_); + force_channel_ = 1; + } + } catch (...) { + force_channel_ = 1; + } state_pub_ = create_publisher("/dart/manager/state", 10); @@ -708,12 +719,15 @@ class DartManager "down_velocity=%.2f rad/s", belt_down_distance_, belt_pulley_radius_, down_velocity); - // 记录当前力值（用于下次fire前的力矩闭环） - if (current_force_ch1_.ready()) { - last_fire_force_ = static_cast(*current_force_ch1_); + // 记录当前力值（用于下次fire前的力矩闭环，根据 force_channel_ 选择通道） + bool force_ready = (force_channel_ == 2) ? current_force_ch2_.ready() + : current_force_ch1_.ready(); + if (force_ready) { + last_fire_force_ = static_cast( + force_channel_ == 2 ? *current_force_ch2_ : *current_force_ch1_); RCLCPP_INFO( - logger_, "[DartManager] Recorded force before fire: %.2f (ch1=%d)", - last_fire_force_, *current_force_ch1_); + logger_, "[DartManager] Recorded force before fire: %.2f (ch%d)", + last_fire_force_, force_channel_); } else { RCLCPP_WARN(logger_, "[DartManager] Force sensor NOT READY, using last value"); } @@ -732,8 +746,8 @@ class DartManager belt_up_distance_, belt_up_decel_target_velocity_, belt_up_decel_torque_offset_, belt_up_stall_velocity_threshold_, belt_up_stall_confirm_ticks_, belt_up_stall_min_run_ticks_, belt_up_decel_timeout_ticks_, - *force_control_velocity_, *current_force_ch1_, last_fire_force_, - enable_force_calibration_, force_tolerance_, force_settle_ticks_, + *force_control_velocity_, *current_force_ch1_, *current_force_ch2_, force_channel_, + last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, is_first_shot); } @@ -892,6 +906,7 @@ class DartManager double force_stall_torque_threshold_{0.5}; uint64_t force_stall_confirm_ticks_{100}; uint64_t force_stall_min_run_ticks_{100}; + int force_channel_{1}; // 1 = ch1, 2 = ch2 bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 23da1b6..3abf48a 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -39,10 +39,11 @@ class LaunchPreparationTask : public Task { bool& belt_zero_calibration, double belt_up_distance, double up_decel_target_velocity, double up_decel_torque_offset, double up_stall_velocity_threshold, uint64_t up_stall_confirm_ticks, uint64_t up_stall_min_run_ticks, - uint64_t up_decel_timeout_ticks, double& force_screw_velocity, const int& current_force, - double target_force, bool enable_force_calibration, double force_tolerance, - uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, - double force_kd, double force_max_velocity, bool is_first_shot = false) + uint64_t up_decel_timeout_ticks, double& force_screw_velocity, const int& current_force_ch1, + const int& current_force_ch2, int force_channel, double target_force, + bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, + uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, + double force_max_velocity, bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -96,32 +97,6 @@ class LaunchPreparationTask : public Task { 50, // 最小运行帧数 800)); // 超时帧数 - // 步骤3：根据是否第一发选择不同的并行动作 - // if (is_first_shot) { - // // 第一发：传送带保持高扭矩 + 扳机锁定 - // auto parallel_hold_and_lock = - // std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); - - // parallel_hold_and_lock - // ->also( - // std::make_shared( - // "belt_hold_torque", // 动作名称 - // belt_command, // 传送带命令（输出） - // belt_target_velocity, // 目标速度（输出） - // belt_hold_torque, // 保持力矩（输出） - // belt_wait_zero_velocity, // WAIT模式选择（输出） - // belt_torque_offset, // 力矩偏移（输出） - // down_hold_torque, // 保持力矩值（N⋅m） - // down_torque_offset, // 力矩偏移值（N⋅m） - // 1000)) // 保持时长（ticks） - // .also( - // std::make_shared( - // trigger_lock_enable, // 扳机锁定使能（输出） - // true, // 锁定（true） - // 1000)); // 等待锁定完成帧数 - // then(parallel_hold_and_lock); - - // } else if (require_lifting_down) { // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 auto parallel_hold_lock_and_lift = @@ -216,7 +191,7 @@ class LaunchPreparationTask : public Task { right_belt_velocity, // 右电机速度反馈（输入） -(belt_up_distance - 0.2), // 目标距离（m，负值=上行，减去已行进的0.2m） belt_pulley_radius, // 滑轮半径（m） - 15.0, // 快速（rad/s） + 12.0, // 快速（rad/s） up_torque_limit, // 扭矩限制（N⋅m） 10000, // 超时帧数 50, // 最小运行帧数 @@ -258,16 +233,17 @@ class LaunchPreparationTask : public Task { then(std::make_shared(belt_zero_calibration)); // 力矩闭环：第一发使用固定目标力，后续发使用上次记录的力值 - // if (enable_force_calibration) { - // double target_force_value = is_first_shot ? 11300.0 : target_force; - // double tolerance_value = is_first_shot ? 3.0 : force_tolerance; - - // then( - // std::make_shared( - // "force_screw_calibration", force_screw_velocity, current_force, - // target_force_value, tolerance_value, force_settle_ticks, force_timeout_ticks, - // force_kp, force_ki, force_kd, force_max_velocity)); - // } + if (enable_force_calibration) { + double target_force_value = is_first_shot ? 11300.0 : target_force; + double tolerance_value = is_first_shot ? 1.0 : force_tolerance; + + then( + std::make_shared( + "force_screw_calibration", force_screw_velocity, current_force_ch1, + current_force_ch2, force_channel, target_force_value, tolerance_value, + force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, + force_max_velocity)); + } } }; From d0634bbb4a415e417bf9b3913feabd422088b7f8 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 12 Apr 2026 00:17:17 +0800 Subject: [PATCH 13/19] Implement adaptive normal torque compensation --- .../action/belt_deceleration_action.hpp | 16 +++-- .../action/belt_hold_torque_action.hpp | 9 ++- .../action/belt_up_initial_accel_action.hpp | 4 +- src/manager/dart_manager.cpp | 8 ++- src/manager/task/launch_preparation_task.hpp | 63 ++++++++++++------- 5 files changed, 68 insertions(+), 32 deletions(-) diff --git a/src/manager/action/belt_deceleration_action.hpp b/src/manager/action/belt_deceleration_action.hpp index cdc8d17..097ba61 100644 --- a/src/manager/action/belt_deceleration_action.hpp +++ b/src/manager/action/belt_deceleration_action.hpp @@ -2,6 +2,7 @@ #include "action.hpp" +#include #include #include @@ -34,7 +35,7 @@ class BeltDecelerationAction : public IAction { double& belt_error_gain, bool& belt_use_decel_pid, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, - double target_velocity = 0.0, double torque_offset_value = 0.0, + double target_velocity = 0.0, const double& torque_offset_value = kZero_, double error_gain_value = 2.0, // 默认2倍error增益 bool enable_stall_detection = false, bool enable_zero_velocity_detection = true, double stall_velocity_threshold = 0.5, double stall_torque_threshold = 0.5, @@ -42,7 +43,8 @@ class BeltDecelerationAction : public IAction { uint64_t zero_confirm_ticks = 100, uint64_t min_running_ticks = 50, uint64_t timeout_ticks = 3000, rmcs_msgs::DartSliderStatus* belt_command = nullptr, - bool* belt_wait_zero_velocity = nullptr, bool send_wait_zero_on_exit = false) + bool* belt_wait_zero_velocity = nullptr, bool send_wait_zero_on_exit = false, + std::function on_enter_hook = nullptr) : IAction(std::move(name)) , belt_target_velocity_(belt_target_velocity) , belt_torque_offset_(belt_torque_offset) @@ -66,9 +68,12 @@ class BeltDecelerationAction : public IAction { , timeout_ticks_(timeout_ticks) , belt_command_(belt_command) , belt_wait_zero_velocity_(belt_wait_zero_velocity) - , send_wait_zero_on_exit_(send_wait_zero_on_exit) {} + , send_wait_zero_on_exit_(send_wait_zero_on_exit) + , on_enter_hook_(std::move(on_enter_hook)) {} void on_enter() override { + if (on_enter_hook_) + on_enter_hook_(); belt_target_velocity_ = target_velocity_; belt_torque_offset_ = torque_offset_value_; belt_error_gain_ = error_gain_value_; @@ -144,7 +149,7 @@ class BeltDecelerationAction : public IAction { const double& right_belt_torque_; double target_velocity_; - double torque_offset_value_; + const double& torque_offset_value_; double error_gain_value_; bool enable_stall_detection_; bool enable_zero_velocity_detection_; @@ -158,9 +163,12 @@ class BeltDecelerationAction : public IAction { rmcs_msgs::DartSliderStatus* belt_command_; bool* belt_wait_zero_velocity_; bool send_wait_zero_on_exit_; + std::function on_enter_hook_; uint64_t stall_counter_{0}; uint64_t zero_counter_{0}; + + static constexpr double kZero_ = 0.0; }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index e3fd433..c2418f6 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -17,7 +17,7 @@ class BeltHoldTorqueAction : public IAction { BeltHoldTorqueAction( std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_hold_torque, bool& belt_wait_zero_velocity, double& belt_torque_offset, - double hold_torque_value, double torque_offset_value, uint64_t hold_duration_ticks) + double hold_torque_value, const double& torque_offset_value, uint64_t hold_duration_ticks) : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -44,7 +44,10 @@ class BeltHoldTorqueAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override {} + void on_exit() override { + belt_hold_torque_ = 0.0; + belt_torque_offset_ = 0.0; + } private: rmcs_msgs::DartSliderStatus& belt_command_; @@ -53,7 +56,7 @@ class BeltHoldTorqueAction : public IAction { bool& belt_wait_zero_velocity_; double& belt_torque_offset_; double hold_torque_value_; - double torque_offset_value_; + const double& torque_offset_value_; uint64_t hold_duration_ticks_; }; diff --git a/src/manager/action/belt_up_initial_accel_action.hpp b/src/manager/action/belt_up_initial_accel_action.hpp index ff24bfd..3af3366 100644 --- a/src/manager/action/belt_up_initial_accel_action.hpp +++ b/src/manager/action/belt_up_initial_accel_action.hpp @@ -19,7 +19,7 @@ class BeltUpInitialAccelAction : public IAction { std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_offset, double& belt_torque_limit, double& belt_error_gain, bool& belt_use_decel_pid, const double& left_belt_angle, const double& right_belt_angle, - double pulley_radius, double slow_velocity, double torque_offset_value, double torque_limit, + double pulley_radius, double slow_velocity, const double& torque_offset_value, double torque_limit, double accel_distance = 0.1, uint64_t timeout_ticks = 5000) : IAction(std::move(name)) , belt_command_(belt_command) @@ -76,7 +76,7 @@ class BeltUpInitialAccelAction : public IAction { double pulley_radius_; double slow_velocity_; - double torque_offset_value_; + const double& torque_offset_value_; double torque_limit_; double accel_distance_; uint64_t timeout_ticks_; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index c947bf3..03eb59a 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -316,6 +316,11 @@ class DartManager } catch (...) { force_channel_ = 1; } + try { + belt_torque_scale_ = get_parameter("belt_torque_scale").as_double(); + } catch (...) { + belt_torque_scale_ = 0.0005; + } state_pub_ = create_publisher("/dart/manager/state", 10); @@ -749,7 +754,7 @@ class DartManager *force_control_velocity_, *current_force_ch1_, *current_force_ch2_, force_channel_, last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, - is_first_shot); + is_first_shot, belt_torque_scale_, get_logger()); } if (cmd == "unload" || cmd == "cancel_launch") { @@ -907,6 +912,7 @@ class DartManager uint64_t force_stall_confirm_ticks_{100}; uint64_t force_stall_min_run_ticks_{100}; int force_channel_{1}; // 1 = ch1, 2 = ch2 + double belt_torque_scale_{0.0005}; // N⋅m/g，传送带自适应力矩补偿倍率 bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 3abf48a..984693f 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -16,6 +16,7 @@ #include #include +#include #include namespace rmcs_dart_guidance::manager { @@ -43,7 +44,8 @@ class LaunchPreparationTask : public Task { const int& current_force_ch2, int force_channel, double target_force, bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, - double force_max_velocity, bool is_first_shot = false) + double force_max_velocity, bool is_first_shot = false, double belt_torque_scale = 0.0005, + const rclcpp::Logger& logger = rclcpp::get_logger("launch_preparation_task")) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -71,7 +73,8 @@ class LaunchPreparationTask : public Task { 100, // 堵转确认帧数 0.80)); // 下行最大距离限制（m，正值） - // 步骤2：减速阶段（目标速度=0，加常态力矩偏移补偿负载，使用减速PID+error增益） + // 步骤2：减速阶段（目标速度=0，加自适应力矩偏移补偿负载，使用减速PID+error增益） + // on_enter_hook 在减速启动的同一帧内采样力传感器，计算三个阶段的自适应补偿值 // 同时启用零速检测和堵转检测：正常减速到0或碰到机械限位都能成功 then( std::make_shared( @@ -85,8 +88,8 @@ class LaunchPreparationTask : public Task { left_belt_torque, // 左电机力矩反馈（输入） right_belt_torque, // 右电机力矩反馈（输入） 0.0, // 目标速度（rad/s） - down_torque_offset, // 力矩偏移值（N⋅m） - 5.0, // error增益倍数（2倍） + adaptive_decel_offset_, // 力矩偏移值（N⋅m，自适应） + 5.0, // error增益倍数 true, // 启用堵转检测（碰到机械限位） true, // 启用零速检测（正常减速到0） 0.3, // 堵转速度阈值（rad/s） @@ -95,7 +98,17 @@ class LaunchPreparationTask : public Task { 50, // 堵转确认帧数 down_zero_confirm_ticks, // 零速确认帧数 50, // 最小运行帧数 - 800)); // 超时帧数 + 800, // 超时帧数 + nullptr, nullptr, false, + [this, ¤t_force_ch1, ¤t_force_ch2, force_channel, belt_torque_scale, + logger]() { + double force_g = static_cast( + force_channel == 2 ? current_force_ch2 : current_force_ch1); + double base = force_g * belt_torque_scale; + adaptive_decel_offset_ = base * 0.75; + adaptive_hold_offset_ = base * 1.0; + adaptive_up_accel_offset_ = base * 0.5; + })); if (require_lifting_down) { // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 @@ -112,7 +125,7 @@ class LaunchPreparationTask : public Task { belt_wait_zero_velocity, // WAIT模式选择（输出） belt_torque_offset, // 力矩偏移（输出） down_hold_torque, // 保持力矩值（N⋅m） - down_torque_offset, // 力矩偏移值（N⋅m） + adaptive_hold_offset_, // 力矩偏移值（N⋅m，自适应） 1000)) // 保持时长（ticks） .also( std::make_shared( @@ -146,7 +159,7 @@ class LaunchPreparationTask : public Task { belt_wait_zero_velocity, // WAIT模式选择（输出） belt_torque_offset, // 力矩偏移（输出） down_hold_torque, // 保持力矩值（N⋅m） - down_torque_offset, // 力矩偏移值（N⋅m） + adaptive_hold_offset_, // 力矩偏移值（N⋅m，自适应） 1000)) // 保持时长（ticks） .also( std::make_shared( @@ -159,21 +172,21 @@ class LaunchPreparationTask : public Task { // 步骤4a：上行初始加速阶段（前0.2m慢速 + 常态力矩偏移） then( std::make_shared( - "belt_up_initial_accel", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - belt_pulley_radius, // 滑轮半径（m） - 5.0, // 慢速（rad/s） - 3.0, // 力矩偏移值（N⋅m） - up_torque_limit, // 扭矩限制（N⋅m） - 0.05, // 加速距离（m） - 5000)); // 超时帧数 + "belt_up_initial_accel", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + belt_pulley_radius, // 滑轮半径（m） + 5.0, // 慢速（rad/s） + adaptive_up_accel_offset_, // 力矩偏移值（N⋅m，自适应） + up_torque_limit, // 扭矩限制（N⋅m） + 0.05, // 加速距离（m） + 5000)); // 超时帧数 // 步骤4b：上行匀速阶段（到达目标位置前，无力矩偏移） // 目标位置 = 初始位置 - (belt_up_distance - 0.2) @@ -245,6 +258,12 @@ class LaunchPreparationTask : public Task { force_max_velocity)); } } + +private: + // 自适应力矩补偿值（由步骤2的 on_enter_hook 在运行时写入，供步骤3/4a引用） + double adaptive_decel_offset_{0.0}; + double adaptive_hold_offset_{0.0}; + double adaptive_up_accel_offset_{0.0}; }; } // namespace rmcs_dart_guidance::manager From 34e686603602ce5f74891279c3726dd28a0849ca Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Tue, 14 Apr 2026 22:36:36 +0800 Subject: [PATCH 14/19] Parameter reduction --- .../action/belt_hold_torque_action.hpp | 2 +- .../action/force_screw_calibration_action.hpp | 9 +- src/manager/dart_manager.cpp | 327 ++++-------------- src/manager/task/cancel_launch_task.hpp | 19 +- src/manager/task/fire_and_preload_task.hpp | 10 +- src/manager/task/launch_preparation_task.hpp | 128 ++++--- 6 files changed, 150 insertions(+), 345 deletions(-) diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index c2418f6..a58449c 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -32,7 +32,7 @@ class BeltHoldTorqueAction : public IAction { belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; belt_target_velocity_ = 0.0; belt_hold_torque_ = hold_torque_value_; - belt_wait_zero_velocity_ = false; // 使用 HOLD_TORQUE 模式（常数力矩） + belt_wait_zero_velocity_ = true; // 使用 HOLD_TORQUE 模式（常数力矩） belt_torque_offset_ = torque_offset_value_; // 叠加常态扭矩偏移补偿负载 } diff --git a/src/manager/action/force_screw_calibration_action.hpp b/src/manager/action/force_screw_calibration_action.hpp index 441e6da..5dc93aa 100644 --- a/src/manager/action/force_screw_calibration_action.hpp +++ b/src/manager/action/force_screw_calibration_action.hpp @@ -15,8 +15,8 @@ class ForceScrewCalibrationAction : public IAction { ForceScrewCalibrationAction( std::string name, double& force_screw_velocity, const int& current_force_ch1, const int& current_force_ch2, int force_channel, double target_force, - double force_tolerance, uint64_t settle_ticks, uint64_t timeout_ticks, double kp, - double ki, double kd, double max_velocity) + double force_tolerance, uint64_t settle_ticks, uint64_t timeout_ticks, double kp, double ki, + double kd, double max_velocity) : IAction(std::move(name)) , force_screw_velocity_(force_screw_velocity) , current_force_ch1_(current_force_ch1) @@ -76,7 +76,10 @@ class ForceScrewCalibrationAction : public IAction { return ActionStatus::RUNNING; } - void on_exit() override { force_screw_velocity_ = 0.0; } + void on_exit() override { + force_screw_velocity_ = 0.0; + integral_ = 0.0; + } private: double& force_screw_velocity_; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 03eb59a..c88b9a9 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -102,225 +102,58 @@ class DartManager "/dart/manager/limiting/command", limiting_command_, rmcs_msgs::DartLimitingServoStatus::LOCK); - try { - max_transform_rate_ = get_parameter("max_transform_rate").as_double(); - } catch (...) { - max_transform_rate_ = 500.0; - } - try { - manual_force_scale_ = get_parameter("manual_force_scale").as_double(); - } catch (...) { - manual_force_scale_ = 5.0; - } + manual_force_scale_ = get_parameter("manual_force_scale").as_double(); + launch_prepare_enable_visual_assist_ = - has_parameter("launch_prepare_enable_visual_assist") - ? get_parameter("launch_prepare_enable_visual_assist").as_bool() - : false; + get_parameter("launch_prepare_enable_visual_assist").as_bool(); if (launch_prepare_enable_visual_assist_) { load_auto_aim_configuration(); } - limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); - belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m belt_pulley_radius_ = get_parameter("belt_pulley_radius").as_double(); // m - // 传送带速度和扭矩限制参数（集中管理） - try { - belt_prepare_down_velocity_first_ = - get_parameter("belt_prepare_down_velocity_first").as_double(); - } catch (...) { - belt_prepare_down_velocity_first_ = 5.0; - } - try { - belt_prepare_down_velocity_ = get_parameter("belt_prepare_down_velocity").as_double(); - } catch (...) { - belt_prepare_down_velocity_ = 10.0; - } - try { - belt_prepare_torque_limit_ = get_parameter("belt_prepare_torque_limit").as_double(); - } catch (...) { - belt_prepare_torque_limit_ = 5.0; - } - try { - belt_prepare_up_torque_limit_ = - get_parameter("belt_prepare_up_torque_limit").as_double(); - } catch (...) { - belt_prepare_up_torque_limit_ = 1.5; - } - try { - belt_prepare_down_ramp_ticks_ = - (uint64_t)get_parameter("belt_prepare_down_ramp_ticks").as_int(); - } catch (...) { - belt_prepare_down_ramp_ticks_ = 400; - } - try { - belt_prepare_down_torque_offset_ = - get_parameter("belt_prepare_down_torque_offset").as_double(); - } catch (...) { - belt_prepare_down_torque_offset_ = 2.0; - } - try { - belt_prepare_down_hold_torque_ = - get_parameter("belt_prepare_down_hold_torque").as_double(); - } catch (...) { - belt_prepare_down_hold_torque_ = 5.0; - } - try { - belt_prepare_down_zero_velocity_threshold_ = - get_parameter("belt_prepare_down_zero_velocity_threshold").as_double(); - } catch (...) { - belt_prepare_down_zero_velocity_threshold_ = 0.15; - } - try { - belt_prepare_down_zero_confirm_ticks_ = - (uint64_t)get_parameter("belt_prepare_down_zero_confirm_ticks").as_int(); - } catch (...) { - belt_prepare_down_zero_confirm_ticks_ = 80; - } - try { - belt_prepare_down_ramp_timeout_ticks_ = - (uint64_t)get_parameter("belt_prepare_down_ramp_timeout_ticks").as_int(); - } catch (...) { - belt_prepare_down_ramp_timeout_ticks_ = 2000; - } - - // 加载上行控制参数 - try { - belt_up_distance_ = get_parameter("belt_up_distance").as_double(); - } catch (...) { - belt_up_distance_ = 0.65; - } - try { - belt_prepare_up_velocity_ = get_parameter("belt_prepare_up_velocity").as_double(); - } catch (...) { - belt_prepare_up_velocity_ = 15.0; - } - try { - belt_up_decel_target_velocity_ = - get_parameter("belt_up_decel_target_velocity").as_double(); - } catch (...) { - belt_up_decel_target_velocity_ = 1.0; - } - try { - belt_up_decel_torque_offset_ = get_parameter("belt_up_decel_torque_offset").as_double(); - } catch (...) { - belt_up_decel_torque_offset_ = 0.0; - } - try { - belt_up_stall_velocity_threshold_ = - get_parameter("belt_up_stall_velocity_threshold").as_double(); - } catch (...) { - belt_up_stall_velocity_threshold_ = 0.15; - } - try { - belt_up_stall_confirm_ticks_ = - (uint64_t)get_parameter("belt_up_stall_confirm_ticks").as_int(); - } catch (...) { - belt_up_stall_confirm_ticks_ = 100; - } - try { - belt_up_stall_min_run_ticks_ = - (uint64_t)get_parameter("belt_up_stall_min_run_ticks").as_int(); - } catch (...) { - belt_up_stall_min_run_ticks_ = 300; - } - try { - belt_up_decel_timeout_ticks_ = - (uint64_t)get_parameter("belt_up_decel_timeout_ticks").as_int(); - } catch (...) { - belt_up_decel_timeout_ticks_ = 3000; - } - - lifting_stall_threshold_ = get_parameter("lifting_stall_threshold").as_double(); - lifting_stall_confirm_ticks_ = - (uint64_t)get_parameter("lifting_stall_confirm_ticks").as_int(); - lifting_stall_min_run_ticks_ = - (uint64_t)get_parameter("lifting_stall_min_run_ticks").as_int(); - lifting_stall_timeout_ticks_ = - (uint64_t)get_parameter("lifting_stall_timeout_ticks").as_int(); - - // 力矩闭环参数 - try { - enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); - } catch (...) { - enable_force_calibration_ = false; - } - try { - force_tolerance_ = get_parameter("force_tolerance").as_double(); - } catch (...) { - force_tolerance_ = 5.0; // N - } - try { - force_settle_ticks_ = (uint64_t)get_parameter("force_settle_ticks").as_int(); - } catch (...) { - force_settle_ticks_ = 50; - } - try { - force_timeout_ticks_ = (uint64_t)get_parameter("force_timeout_ticks").as_int(); - } catch (...) { - force_timeout_ticks_ = 2000; - } - try { - force_kp_ = get_parameter("force_kp").as_double(); - } catch (...) { - force_kp_ = 0.1; - } - try { - force_ki_ = get_parameter("force_ki").as_double(); - } catch (...) { - force_ki_ = 0.0; - } - try { - force_kd_ = get_parameter("force_kd").as_double(); - } catch (...) { - force_kd_ = 0.01; - } - try { - force_max_velocity_ = get_parameter("force_max_velocity").as_double(); - } catch (...) { - force_max_velocity_ = 5.0; // rad/s - } - try { - force_stall_velocity_threshold_ = - get_parameter("force_stall_velocity_threshold").as_double(); - } catch (...) { - force_stall_velocity_threshold_ = 0.15; - } - try { - force_stall_torque_threshold_ = - get_parameter("force_stall_torque_threshold").as_double(); - } catch (...) { - force_stall_torque_threshold_ = 0.5; - } - try { - force_stall_confirm_ticks_ = - (uint64_t)get_parameter("force_stall_confirm_ticks").as_int(); - } catch (...) { - force_stall_confirm_ticks_ = 100; - } - try { - force_stall_min_run_ticks_ = - (uint64_t)get_parameter("force_stall_min_run_ticks").as_int(); - } catch (...) { - force_stall_min_run_ticks_ = 100; - } - try { - force_channel_ = get_parameter("force_channel").as_int(); - if (force_channel_ != 1 && force_channel_ != 2) { - RCLCPP_WARN( - get_logger(), "[DartManager] force_channel=%d invalid, defaulting to 1", - force_channel_); - force_channel_ = 1; - } - } catch (...) { + belt_prepare_down_velocity_first_ = + get_parameter("belt_prepare_down_velocity_first").as_double(); + + belt_prepare_down_velocity_ = get_parameter("belt_prepare_down_velocity").as_double(); + + belt_prepare_torque_limit_ = get_parameter("belt_prepare_torque_limit").as_double(); + + belt_prepare_up_torque_limit_ = get_parameter("belt_prepare_up_torque_limit").as_double(); + + belt_prepare_down_torque_offset_ = + get_parameter("belt_prepare_down_torque_offset").as_double(); + + belt_prepare_down_hold_torque_ = get_parameter("belt_prepare_down_hold_torque").as_double(); + + belt_up_distance_ = get_parameter("belt_up_distance").as_double(); + + belt_prepare_up_velocity_ = get_parameter("belt_prepare_up_velocity").as_double(); + + belt_up_decel_target_velocity_ = get_parameter("belt_up_decel_target_velocity").as_double(); + + belt_up_decel_torque_offset_ = get_parameter("belt_up_decel_torque_offset").as_double(); + + enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); + + force_tolerance_ = get_parameter("force_tolerance").as_double(); + + force_kp_ = get_parameter("force_kp").as_double(); + + force_ki_ = get_parameter("force_ki").as_double(); + + force_kd_ = get_parameter("force_kd").as_double(); + + force_channel_ = (int)get_parameter("force_channel").as_int(); + if (force_channel_ != 1 && force_channel_ != 2) { + RCLCPP_WARN( + get_logger(), "[DartManager] force_channel=%d invalid, defaulting to 1", + force_channel_); force_channel_ = 1; } - try { - belt_torque_scale_ = get_parameter("belt_torque_scale").as_double(); - } catch (...) { - belt_torque_scale_ = 0.0005; - } + + belt_torque_scale_ = get_parameter("belt_torque_scale").as_double(); state_pub_ = create_publisher("/dart/manager/state", 10); @@ -725,8 +558,8 @@ class DartManager belt_down_distance_, belt_pulley_radius_, down_velocity); // 记录当前力值（用于下次fire前的力矩闭环，根据 force_channel_ 选择通道） - bool force_ready = (force_channel_ == 2) ? current_force_ch2_.ready() - : current_force_ch1_.ready(); + bool force_ready = + (force_channel_ == 2) ? current_force_ch2_.ready() : current_force_ch1_.ready(); if (force_ready) { last_fire_force_ = static_cast( force_channel_ == 2 ? *current_force_ch2_ : *current_force_ch1_); @@ -742,25 +575,17 @@ class DartManager *belt_wait_zero_velocity_, *belt_torque_offset_, *belt_error_gain_, *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, - *trigger_lock_enable_, belt_down_distance_, belt_pulley_radius_, down_velocity, - belt_prepare_torque_limit_, belt_prepare_up_torque_limit_, - belt_prepare_down_torque_offset_, belt_prepare_down_hold_torque_, - belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, - belt_prepare_down_ramp_timeout_ticks_, require_lifting_down, *lifting_command_, + *trigger_lock_enable_, belt_down_distance_, down_velocity, + belt_prepare_down_hold_torque_, require_lifting_down, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_, - belt_up_distance_, belt_up_decel_target_velocity_, belt_up_decel_torque_offset_, - belt_up_stall_velocity_threshold_, belt_up_stall_confirm_ticks_, - belt_up_stall_min_run_ticks_, belt_up_decel_timeout_ticks_, - *force_control_velocity_, *current_force_ch1_, *current_force_ch2_, force_channel_, - last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, + belt_up_distance_, belt_up_decel_target_velocity_, *force_control_velocity_, + *current_force_ch1_, *current_force_ch2_, force_channel_, last_fire_force_, + enable_force_calibration_, force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, is_first_shot, belt_torque_scale_, get_logger()); } if (cmd == "unload" || cmd == "cancel_launch") { - // 取消发射使用与准备发射相同的下行参数 - double down_velocity = belt_prepare_down_velocity_; - // 第一发时不需要升降上行（因为还没有下行过） bool require_lifting_up = (fire_count_ > 0); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, @@ -768,20 +593,16 @@ class DartManager *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, - *lifting_right_vel_fb_, belt_down_distance_, belt_pulley_radius_, down_velocity, - belt_prepare_torque_limit_, belt_prepare_up_torque_limit_, - belt_prepare_down_hold_torque_, belt_prepare_down_zero_velocity_threshold_, - belt_prepare_down_zero_confirm_ticks_, belt_prepare_down_ramp_timeout_ticks_, - *belt_zero_calibration_, require_lifting_up); + *lifting_right_vel_fb_, belt_down_distance_, belt_prepare_down_hold_torque_, + belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, + belt_prepare_down_ramp_timeout_ticks_, *belt_zero_calibration_, require_lifting_up); } if (cmd == "fire") { bool is_first_shot = (fire_count_ == 0); return std::make_shared( *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, - *lifting_right_vel_fb_, lifting_stall_threshold_, lifting_stall_confirm_ticks_, - lifting_stall_min_run_ticks_, lifting_stall_timeout_ticks_, *limiting_command_, - limiting_fill_ticks_, is_first_shot); + *lifting_right_vel_fb_, *limiting_command_, limiting_fill_ticks_, is_first_shot); } if (cmd == "manual_angle") { @@ -800,20 +621,20 @@ class DartManager const double* force_screw_torque_feedback = force_screw_torque_fb_.ready() ? &*force_screw_torque_fb_ : nullptr; - if (force_screw_velocity_feedback == nullptr || force_screw_torque_feedback == nullptr) { + if (force_screw_velocity_feedback == nullptr + || force_screw_torque_feedback == nullptr) { RCLCPP_WARN( - logger_, - "[DartManager] force screw feedback not ready, manual_force stall detection disabled"); + logger_, "[DartManager] force screw feedback not ready, manual_force stall " + "detection disabled"); } auto task = std::make_shared("manual_force", "手动力丝杆速度调整"); task->then( std::make_shared( *force_control_velocity_, *joystick_right_, max_transform_rate_, - manual_force_scale_, force_screw_velocity_feedback, - force_screw_torque_feedback, force_stall_velocity_threshold_, - force_stall_torque_threshold_, force_stall_confirm_ticks_, - force_stall_min_run_ticks_)); + manual_force_scale_, force_screw_velocity_feedback, force_screw_torque_feedback, + force_stall_velocity_threshold_, force_stall_torque_threshold_, + force_stall_confirm_ticks_, force_stall_min_run_ticks_)); return task; } return nullptr; @@ -876,7 +697,6 @@ class DartManager double belt_prepare_down_velocity_{10.0}; // rad/s double belt_prepare_torque_limit_{5.0}; // N⋅m double belt_prepare_up_torque_limit_{1.5}; // N⋅m - uint64_t belt_prepare_down_ramp_ticks_{400}; // ticks double belt_prepare_down_torque_offset_{2.0}; // N⋅m double belt_prepare_down_hold_torque_{5.0}; // N⋅m double belt_prepare_down_zero_velocity_threshold_{0.15}; // rad/s @@ -884,34 +704,25 @@ class DartManager uint64_t belt_prepare_down_ramp_timeout_ticks_{2000}; // ticks // 传送带上行控制参数 - double belt_up_distance_{0.65}; // m - 上行目标距离（略低于下行起点） - double belt_prepare_up_velocity_{15.0}; // rad/s - 上行速度 - double belt_up_decel_target_velocity_{1.0}; // rad/s - 上行减速阶段目标速度 - double belt_up_decel_torque_offset_{0.0}; // N⋅m - 上行减速阶段力矩偏移 - double belt_up_stall_velocity_threshold_{0.15}; // rad/s - 上行堵转速度阈值 - uint64_t belt_up_stall_confirm_ticks_{100}; // ticks - 上行堵转确认帧数 - uint64_t belt_up_stall_min_run_ticks_{300}; // ticks - 上行堵转最小运行帧数 - uint64_t belt_up_decel_timeout_ticks_{3000}; // ticks - 上行减速超时帧数 - - double lifting_stall_threshold_{0.5}; - uint64_t lifting_stall_confirm_ticks_{100}; - uint64_t lifting_stall_min_run_ticks_{500}; - uint64_t lifting_stall_timeout_ticks_{5000}; + double belt_up_distance_{0.65}; // m - 上行目标距离（略低于下行起点） + double belt_prepare_up_velocity_{15.0}; // rad/s - 上行速度 + double belt_up_decel_target_velocity_{1.0}; // rad/s - 上行减速阶段目标速度 + double belt_up_decel_torque_offset_{0.0}; // N⋅m - 上行减速阶段力矩偏移 // 力矩闭环参数 bool enable_force_calibration_{false}; - double force_tolerance_{5.0}; // N + double force_tolerance_{5.0}; // N uint64_t force_settle_ticks_{50}; uint64_t force_timeout_ticks_{2000}; double force_kp_{0.1}; double force_ki_{0.0}; double force_kd_{0.01}; - double force_max_velocity_{5.0}; // rad/s + double force_max_velocity_{5.0}; // rad/s double force_stall_velocity_threshold_{0.15}; double force_stall_torque_threshold_{0.5}; uint64_t force_stall_confirm_ticks_{100}; uint64_t force_stall_min_run_ticks_{100}; - int force_channel_{1}; // 1 = ch1, 2 = ch2 + int force_channel_{1}; // 1 = ch1, 2 = ch2 double belt_torque_scale_{0.0005}; // N⋅m/g，传送带自适应力矩补偿倍率 bool launch_prepare_enable_visual_assist_{false}; @@ -939,9 +750,9 @@ class DartManager std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; - uint32_t fire_count_{0}; // 当前轮次已完成发射数 + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; - double last_fire_force_{0.0}; // 上次fire前记录的力值 + double last_fire_force_{0.0}; // 上次fire前记录的力值 }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index a0612ab..d7c822f 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -35,11 +35,10 @@ class CancelLaunchTask : public Task { const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, double belt_down_distance, double belt_pulley_radius, - double down_velocity, double torque_limit, double up_torque_limit, - double down_hold_torque, double down_zero_velocity_threshold, - uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, - bool& belt_zero_calibration, bool require_lifting_up = true) + const double& lifting_right_vel_fb, double belt_down_distance, double down_hold_torque, + double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, + uint64_t down_ramp_timeout_ticks, bool& belt_zero_calibration, + bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） @@ -56,15 +55,15 @@ class CancelLaunchTask : public Task { left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） +belt_down_distance, // 目标距离（m，正值=下行） - belt_pulley_radius, // 滑轮半径（m） - down_velocity, // 运动速度（rad/s） - torque_limit, // 扭矩限制（N⋅m） + 0.0195, // 滑轮半径（m） + 10, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） 10000, // 超时帧数 100, // 最小运行帧数 0.5, // 位置到达容差（rad）- 增大容差 0.3, // 堵转速度阈值（rad/s） 200, // 堵转确认帧数 - 0.20)); // 下行最大距离限制（m，正值） + 0.80)); // 下行最大距离限制（m，正值） // 步骤2：减速阶段（目标速度=0，加常态力矩偏移补偿负载，使用零速检测） then( @@ -145,7 +144,7 @@ class CancelLaunchTask : public Task { right_belt_torque, // 右同步带力矩（输入） rmcs_msgs::DartSliderStatus::UP, // 指令状态 10, // 设定速度 - up_torque_limit, // 设定力矩限制 + 3.0, // 设定力矩限制 0.5, // 设定保持力矩 10000, // 超时帧数 1.0, // 堵转速度阈值 diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index 12f4fcb..28bd43f 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -22,8 +22,6 @@ class FireAndPreloadTask : public Task { FireAndPreloadTask( bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - double lifting_stall_threshold, uint64_t lifting_stall_confirm_ticks, - uint64_t lifting_stall_min_run_ticks, uint64_t lifting_stall_timeout_ticks, rmcs_msgs::DartLimitingServoStatus& limiting_command, uint64_t preload_fill_ticks, bool is_first_shot = false) : Task("fire", "发射并预装填") { @@ -52,10 +50,10 @@ class FireAndPreloadTask : public Task { rmcs_msgs::DartSliderStatus::UP, // 指令状态 lifting_left_vel_fb, // 左升降电机速度反馈（输入） lifting_right_vel_fb, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 + 0.5, // 堵转速度阈值 + 100, // 堵转确认帧数 + 500, // 最短运行帧数 + 5000 // 超时帧数 )); then( diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 984693f..d46afcf 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -31,20 +31,15 @@ class LaunchPreparationTask : public Task { const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, - double belt_down_distance, double belt_pulley_radius, double down_velocity, - double torque_limit, double up_torque_limit, double down_torque_offset, - double down_hold_torque, double down_zero_velocity_threshold, - uint64_t down_zero_confirm_ticks, uint64_t down_ramp_timeout_ticks, + double belt_down_distance, double down_velocity, double down_hold_torque, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, bool& belt_zero_calibration, double belt_up_distance, double up_decel_target_velocity, - double up_decel_torque_offset, double up_stall_velocity_threshold, - uint64_t up_stall_confirm_ticks, uint64_t up_stall_min_run_ticks, - uint64_t up_decel_timeout_ticks, double& force_screw_velocity, const int& current_force_ch1, - const int& current_force_ch2, int force_channel, double target_force, - bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, - uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, - double force_max_velocity, bool is_first_shot = false, double belt_torque_scale = 0.0005, + double& force_screw_velocity, const int& current_force_ch1, const int& current_force_ch2, + int force_channel, double target_force, bool enable_force_calibration, + double force_tolerance, uint64_t force_settle_ticks, uint64_t force_timeout_ticks, + double force_kp, double force_ki, double force_kd, double force_max_velocity, + bool is_first_shot = false, double belt_torque_scale = 0.0005, const rclcpp::Logger& logger = rclcpp::get_logger("launch_preparation_task")) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { @@ -63,9 +58,9 @@ class LaunchPreparationTask : public Task { left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） +belt_down_distance, // 目标距离（m，正值=下行） - belt_pulley_radius, // 滑轮半径（m） + 0.0195, // 滑轮半径（m） down_velocity, // 运动速度（rad/s） - torque_limit, // 扭矩限制（N⋅m） + 10, // 扭矩限制（N⋅m） 10000, // 超时帧数 100, // 最小运行帧数 0.5, // 位置到达容差（rad） @@ -78,27 +73,27 @@ class LaunchPreparationTask : public Task { // 同时启用零速检测和堵转检测：正常减速到0或碰到机械限位都能成功 then( std::make_shared( - "belt_deceleration", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - 0.0, // 目标速度（rad/s） - adaptive_decel_offset_, // 力矩偏移值（N⋅m，自适应） - 5.0, // error增益倍数 - true, // 启用堵转检测（碰到机械限位） - true, // 启用零速检测（正常减速到0） - 0.3, // 堵转速度阈值（rad/s） - 1.0, // 堵转扭矩阈值（N⋅m） - down_zero_velocity_threshold, // 零速阈值（rad/s） - 50, // 堵转确认帧数 - down_zero_confirm_ticks, // 零速确认帧数 - 50, // 最小运行帧数 - 800, // 超时帧数 + "belt_deceleration", // 动作名称 + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + 0.0, // 目标速度（rad/s） + adaptive_decel_offset_, // 力矩偏移值（N⋅m，自适应） + 5.0, // error增益倍数 + true, // 启用堵转检测（碰到机械限位） + true, // 启用零速检测（正常减速到0） + 0.3, // 堵转速度阈值（rad/s） + 1.0, // 堵转扭矩阈值（N⋅m） + 0.05, // 零速阈值（rad/s） + 50, // 堵转确认帧数 + 0.05, // 零速确认帧数 + 50, // 最小运行帧数 + 800, // 超时帧数 nullptr, nullptr, false, [this, ¤t_force_ch1, ¤t_force_ch2, force_channel, belt_torque_scale, logger]() { @@ -160,12 +155,12 @@ class LaunchPreparationTask : public Task { belt_torque_offset, // 力矩偏移（输出） down_hold_torque, // 保持力矩值（N⋅m） adaptive_hold_offset_, // 力矩偏移值（N⋅m，自适应） - 1000)) // 保持时长（ticks） + 500)) // 保持时长（ticks） .also( std::make_shared( trigger_lock_enable, // 扳机锁定使能（输出） true, // 锁定（true） - 1000)); // 等待锁定完成帧数 + 500)); // 等待锁定完成帧数 then(parallel_hold_and_lock); } @@ -181,10 +176,10 @@ class LaunchPreparationTask : public Task { belt_use_decel_pid, // 使用减速PID标志（输出） left_belt_angle, // 左电机角度反馈（输入） right_belt_angle, // 右电机角度反馈（输入） - belt_pulley_radius, // 滑轮半径（m） + 0.0195, // 滑轮半径（m） 5.0, // 慢速（rad/s） adaptive_up_accel_offset_, // 力矩偏移值（N⋅m，自适应） - up_torque_limit, // 扭矩限制（N⋅m） + 3.0, // 扭矩限制（N⋅m） 0.05, // 加速距离（m） 5000)); // 超时帧数 @@ -203,9 +198,9 @@ class LaunchPreparationTask : public Task { left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） -(belt_up_distance - 0.2), // 目标距离（m，负值=上行，减去已行进的0.2m） - belt_pulley_radius, // 滑轮半径（m） + 0.0195, // 滑轮半径（m） 12.0, // 快速（rad/s） - up_torque_limit, // 扭矩限制（N⋅m） + 3.0, // 扭矩限制（N⋅m） 10000, // 超时帧数 50, // 最小运行帧数 0.5, // 位置到达容差（rad） @@ -216,30 +211,30 @@ class LaunchPreparationTask : public Task { // 步骤4c：上行减速+堵转检测（堵转标志成功，作为下次下行起点，无力矩偏移） then( std::make_shared( - "belt_up_decel_and_stall", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - up_decel_target_velocity, // 目标速度（rad/s） - 0.0, // 力矩偏移值（无） - 1.0, // error增益倍数（正常） - true, // 启用堵转检测 - false, // 不启用零速检测 - up_stall_velocity_threshold, // 堵转速度阈值（rad/s） - 0.5, // 堵转扭矩阈值（N⋅m） - 0.1, // 零速阈值（未使用） - up_stall_confirm_ticks, // 堵转确认帧数 - 100, // 零速确认帧数（未使用） - up_stall_min_run_ticks, // 最小运行帧数 - 5000, // 超时帧数 - &belt_command, // 仅上行减速结束时显式下发 WAIT - &belt_wait_zero_velocity, // 上行减速结束后显式进入 WAIT_ZERO - true)); // 仅上行减速结束时显式下发 WAIT/0 + "belt_up_decel_and_stall", // 动作名称 + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_error_gain, // error增益（输出） + belt_use_decel_pid, // 使用减速PID标志（输出） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + left_belt_torque, // 左电机力矩反馈（输入） + right_belt_torque, // 右电机力矩反馈（输入） + up_decel_target_velocity, // 目标速度（rad/s） + 0.0, // 力矩偏移值（无） + 1.0, // error增益倍数（正常） + true, // 启用堵转检测 + false, // 不启用零速检测 + 0.15, // 堵转速度阈值（rad/s） + 0.5, // 堵转扭矩阈值（N⋅m） + 0.1, // 零速阈值（未使用） + 100, // 堵转确认帧数 + 100, // 零速确认帧数（未使用） + 300, // 最小运行帧数 + 5000, // 超时帧数 + &belt_command, // 仅上行减速结束时显式下发 WAIT + &belt_wait_zero_velocity, // 上行减速结束后显式进入 WAIT_ZERO + true)); // 仅上行减速结束时显式下发 WAIT/0 then(std::make_shared("stabilize_wait", 50)); @@ -247,8 +242,8 @@ class LaunchPreparationTask : public Task { // 力矩闭环：第一发使用固定目标力，后续发使用上次记录的力值 if (enable_force_calibration) { - double target_force_value = is_first_shot ? 11300.0 : target_force; - double tolerance_value = is_first_shot ? 1.0 : force_tolerance; + double target_force_value = is_first_shot ? 16000.0 : target_force; + double tolerance_value = is_first_shot ? 0.5 : force_tolerance; then( std::make_shared( @@ -260,7 +255,6 @@ class LaunchPreparationTask : public Task { } private: - // 自适应力矩补偿值（由步骤2的 on_enter_hook 在运行时写入，供步骤3/4a引用） double adaptive_decel_offset_{0.0}; double adaptive_hold_offset_{0.0}; double adaptive_up_accel_offset_{0.0}; From 998301482af617f1a05debde9bf4c468ca651e14 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Thu, 16 Apr 2026 21:17:41 +0800 Subject: [PATCH 15/19] parameter cleared and new control tested ac --- .../belt_constant_velocity_move_action.hpp | 71 +---- .../action/belt_deceleration_action.hpp | 174 ----------- .../action/belt_hold_torque_action.hpp | 7 +- .../action/filling_limit_servo_action.hpp | 11 +- src/manager/action/manual_force_control.hpp | 21 +- src/manager/dart_manager.cpp | 109 ++----- src/manager/task/cancel_launch_task.hpp | 195 +++++++------ src/manager/task/fire_and_preload_task.hpp | 7 +- src/manager/task/launch_preparation_task.hpp | 275 +++++++----------- tests/manual_force_control_test.cpp | 6 +- 10 files changed, 255 insertions(+), 621 deletions(-) delete mode 100644 src/manager/action/belt_deceleration_action.hpp diff --git a/src/manager/action/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp index f4177d6..95e69db 100644 --- a/src/manager/action/belt_constant_velocity_move_action.hpp +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -25,14 +25,11 @@ class BeltConstantVelocityMoveAction : public IAction { double& belt_torque_offset, double& belt_torque_limit, double torque_offset_value, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, - double target_distance, // 目标距离（m，正=上行，负=下行） - double pulley_radius, // 滑轮半径（m） - double velocity, // 运动速度（rad/s） - double torque_limit, // 扭矩限制（N⋅m） - uint64_t timeout_ticks, uint64_t min_running_ticks = 50, - double position_tolerance = 0.01, // 位置到达容差（rad） - double stall_velocity_threshold = 0.3, uint64_t stall_confirm_ticks = 200, - double max_down_distance = 0.80) // 下行最大距离限制（m，正值，防止过度下行） + double target_distance, // 目标距离（m，正=上行，负=下行） + double velocity, // 运动速度（rad/s） + double torque_limit, // 扭矩限制（N⋅m） + uint64_t timeout_ticks, + uint64_t min_running_ticks = 50) // 下行最大距离限制（m，正值，防止过度下行） : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -44,46 +41,23 @@ class BeltConstantVelocityMoveAction : public IAction { , left_belt_velocity_(left_belt_velocity) , right_belt_velocity_(right_belt_velocity) , target_distance_(target_distance) - , pulley_radius_(pulley_radius) , velocity_(velocity) , torque_limit_(torque_limit) , timeout_ticks_(timeout_ticks) - , min_running_ticks_(min_running_ticks) - , position_tolerance_(position_tolerance) - , stall_velocity_threshold_(stall_velocity_threshold) - , stall_confirm_ticks_(stall_confirm_ticks) - , max_down_distance_(max_down_distance) {} + , min_running_ticks_(min_running_ticks) {} void on_enter() override { stall_counter_ = 0; - - // 读取初始角度（左传送带，右传送带使用同步控制） initial_angle_ = left_belt_angle_; - - // 将目标距离（m）换算为角度偏移（rad） - double target_angle_offset = target_distance_ / pulley_radius_; - - // 计算实际目标位置 = 初始角度 + 角度偏移 + double target_angle_offset = + target_distance_ / 0.0195; // 将目标距离换算为角度偏移（rad），滑轮半径为0.0195m target_position_ = initial_angle_ + target_angle_offset; - // 下行最大距离限制检查（防止过度下行） - // 将最大下行距离（m）换算为角度偏移（rad） - double max_down_angle_offset = -max_down_distance_ / pulley_radius_; // 负值表示下行 - double max_down_position = initial_angle_ + max_down_angle_offset; - - if (target_position_ < max_down_position) { - target_position_ = max_down_position; - } - - // 根据目标位置自动确定运动方向 - // 实际测量：角度增大=下行，角度减小=上行 double distance_to_target = target_position_ - initial_angle_; if (distance_to_target > 0) { - // 目标位置 > 初始位置，角度增大方向 = 下行 belt_command_ = rmcs_msgs::DartSliderStatus::DOWN; belt_target_velocity_ = std::abs(velocity_); } else { - // 目标位置 < 初始位置，角度减小方向 = 上行 belt_command_ = rmcs_msgs::DartSliderStatus::UP; belt_target_velocity_ = std::abs(velocity_); } @@ -94,7 +68,7 @@ class BeltConstantVelocityMoveAction : public IAction { ActionStatus update() override { if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::SUCCESS; + return ActionStatus::FAILURE; } double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; @@ -102,27 +76,14 @@ class BeltConstantVelocityMoveAction : public IAction { double avg_velocity = (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - if (target_distance_ < 0) { // 上行（负距离） - double distance_traveled = std::abs(avg_angle - initial_angle_) * pulley_radius_; - if (distance_traveled < 0.2) { - belt_target_velocity_ = 5.0; // 前0.2m慢速 - } else { - belt_target_velocity_ = 15.0; // 0.2m后快速 - } - } - // 位置到达判断（优先级最高） if (elapsed_ticks() > min_running_ticks_) { - bool within_tolerance = std::abs(position_error) < position_tolerance_; + bool within_tolerance = std::abs(position_error) < 0.01; bool overshot = false; if (target_distance_ > 0) { - // 下行：目标位置 > 初始位置，期望 current 增大 - // 如果 error < 0，说明 current > target，已经越过 overshot = (position_error < 0); } else { - // 上行：目标位置 < 初始位置，期望 current 减小 - // 如果 error > 0，说明 current < target，已经越过 overshot = (position_error > 0); } @@ -131,11 +92,10 @@ class BeltConstantVelocityMoveAction : public IAction { } } - // 堵转检测 - if (elapsed_ticks() > min_running_ticks_) { - if (avg_velocity < stall_velocity_threshold_) { + if (elapsed_ticks() > 50) { + if (avg_velocity < 0.3) { ++stall_counter_; - if (stall_counter_ >= stall_confirm_ticks_) { + if (stall_counter_ >= 200) { return ActionStatus::SUCCESS; } } else { @@ -160,15 +120,10 @@ class BeltConstantVelocityMoveAction : public IAction { const double& right_belt_velocity_; double target_distance_; // 目标距离（m） - double pulley_radius_; // 滑轮半径（m） double velocity_; // 运动速度（rad/s） double torque_limit_; // 扭矩限制（N⋅m） uint64_t timeout_ticks_; uint64_t min_running_ticks_; - double position_tolerance_; // 位置到达容差（rad） - double stall_velocity_threshold_; - uint64_t stall_confirm_ticks_; - double max_down_distance_; // 下行最大距离限制（m，正值） double initial_angle_{0.0}; // 初始角度（rad，在on_enter中读取） double target_position_{0.0}; // 实际目标位置（rad，在on_enter中计算） diff --git a/src/manager/action/belt_deceleration_action.hpp b/src/manager/action/belt_deceleration_action.hpp deleted file mode 100644 index 097ba61..0000000 --- a/src/manager/action/belt_deceleration_action.hpp +++ /dev/null @@ -1,174 +0,0 @@ -#pragma once - -#include "action.hpp" - -#include -#include - -#include -#include - -namespace rmcs_dart_guidance::manager { - -// BeltDecelerationAction - 传送带减速动作 -// 支持多种减速模式： -// 1. 零速检测模式：减速到零速，检测速度低于阈值作为成功条件 -// 2. 堵转检测模式：减速到目标速度，检测堵转（低速+高扭矩）作为成功条件 -// 3. 混合模式：同时支持零速和堵转检测 -// -// 参数说明： -// - target_velocity: 目标速度（rad/s），通常为0或小的正值 -// - torque_offset_value: 力矩偏移（N⋅m），用于补偿负载 -// - enable_stall_detection: 是否启用堵转检测 -// - enable_zero_velocity_detection: 是否启用零速检测 -// - stall_velocity_threshold: 堵转速度阈值（rad/s） -// - stall_torque_threshold: 堵转扭矩阈值（N⋅m） -// - zero_velocity_threshold: 零速阈值（rad/s） -// - stall_confirm_ticks: 堵转确认帧数 -// - zero_confirm_ticks: 零速确认帧数 -// - min_running_ticks: 最小运行帧数（避免启动瞬间误触发） -// - timeout_ticks: 超时帧数 -class BeltDecelerationAction : public IAction { -public: - BeltDecelerationAction( - std::string name, double& belt_target_velocity, double& belt_torque_offset, - double& belt_error_gain, bool& belt_use_decel_pid, - const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque, - double target_velocity = 0.0, const double& torque_offset_value = kZero_, - double error_gain_value = 2.0, // 默认2倍error增益 - bool enable_stall_detection = false, bool enable_zero_velocity_detection = true, - double stall_velocity_threshold = 0.5, double stall_torque_threshold = 0.5, - double zero_velocity_threshold = 0.1, uint64_t stall_confirm_ticks = 100, - uint64_t zero_confirm_ticks = 100, uint64_t min_running_ticks = 50, - uint64_t timeout_ticks = 3000, - rmcs_msgs::DartSliderStatus* belt_command = nullptr, - bool* belt_wait_zero_velocity = nullptr, bool send_wait_zero_on_exit = false, - std::function on_enter_hook = nullptr) - : IAction(std::move(name)) - , belt_target_velocity_(belt_target_velocity) - , belt_torque_offset_(belt_torque_offset) - , belt_error_gain_(belt_error_gain) - , belt_use_decel_pid_(belt_use_decel_pid) - , left_belt_velocity_(left_belt_velocity) - , right_belt_velocity_(right_belt_velocity) - , left_belt_torque_(left_belt_torque) - , right_belt_torque_(right_belt_torque) - , target_velocity_(target_velocity) - , torque_offset_value_(torque_offset_value) - , error_gain_value_(error_gain_value) - , enable_stall_detection_(enable_stall_detection) - , enable_zero_velocity_detection_(enable_zero_velocity_detection) - , stall_velocity_threshold_(stall_velocity_threshold) - , stall_torque_threshold_(stall_torque_threshold) - , zero_velocity_threshold_(zero_velocity_threshold) - , stall_confirm_ticks_(stall_confirm_ticks) - , zero_confirm_ticks_(zero_confirm_ticks) - , min_running_ticks_(min_running_ticks) - , timeout_ticks_(timeout_ticks) - , belt_command_(belt_command) - , belt_wait_zero_velocity_(belt_wait_zero_velocity) - , send_wait_zero_on_exit_(send_wait_zero_on_exit) - , on_enter_hook_(std::move(on_enter_hook)) {} - - void on_enter() override { - if (on_enter_hook_) - on_enter_hook_(); - belt_target_velocity_ = target_velocity_; - belt_torque_offset_ = torque_offset_value_; - belt_error_gain_ = error_gain_value_; - belt_use_decel_pid_ = true; // 启用减速PID参数 - stall_counter_ = 0; - zero_counter_ = 0; - } - - ActionStatus update() override { - // 超时检测 - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::SUCCESS; - } - - // 计算平均速度和扭矩 - double avg_velocity = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - bool torque_active = std::abs(left_belt_torque_) > stall_torque_threshold_ - || std::abs(right_belt_torque_) > stall_torque_threshold_; - - // 最小运行帧数后才开始检测 - if (elapsed_ticks() > min_running_ticks_) { - // 优先检测堵转：速度低且扭矩高（碰到机械限位） - if (enable_stall_detection_) { - if (avg_velocity < stall_velocity_threshold_ && torque_active) { - ++stall_counter_; - if (stall_counter_ >= stall_confirm_ticks_) { - return ActionStatus::SUCCESS; - } - } else { - stall_counter_ = 0; - } - } - - // 零速检测：速度低于阈值且扭矩不高（正常减速到0） - // 只有在没有堵转的情况下才检测零速 - if (enable_zero_velocity_detection_ && !torque_active) { - if (avg_velocity < zero_velocity_threshold_) { - ++zero_counter_; - if (zero_counter_ >= zero_confirm_ticks_) { - return ActionStatus::SUCCESS; - } - } else { - zero_counter_ = 0; - } - } - } - - return ActionStatus::RUNNING; - } - - void on_exit() override { - if (send_wait_zero_on_exit_ && belt_command_ != nullptr) { - *belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; - belt_target_velocity_ = 0.0; - if (belt_wait_zero_velocity_ != nullptr) { - *belt_wait_zero_velocity_ = true; - } - } - belt_torque_offset_ = 0.0; - belt_error_gain_ = 1.0; - belt_use_decel_pid_ = false; // 恢复正常PID参数 - } - -private: - double& belt_target_velocity_; - double& belt_torque_offset_; - double& belt_error_gain_; - bool& belt_use_decel_pid_; - const double& left_belt_velocity_; - const double& right_belt_velocity_; - const double& left_belt_torque_; - const double& right_belt_torque_; - - double target_velocity_; - const double& torque_offset_value_; - double error_gain_value_; - bool enable_stall_detection_; - bool enable_zero_velocity_detection_; - double stall_velocity_threshold_; - double stall_torque_threshold_; - double zero_velocity_threshold_; - uint64_t stall_confirm_ticks_; - uint64_t zero_confirm_ticks_; - uint64_t min_running_ticks_; - uint64_t timeout_ticks_; - rmcs_msgs::DartSliderStatus* belt_command_; - bool* belt_wait_zero_velocity_; - bool send_wait_zero_on_exit_; - std::function on_enter_hook_; - - uint64_t stall_counter_{0}; - uint64_t zero_counter_{0}; - - static constexpr double kZero_ = 0.0; -}; - -} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_hold_torque_action.hpp b/src/manager/action/belt_hold_torque_action.hpp index a58449c..d651111 100644 --- a/src/manager/action/belt_hold_torque_action.hpp +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -17,7 +17,7 @@ class BeltHoldTorqueAction : public IAction { BeltHoldTorqueAction( std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_hold_torque, bool& belt_wait_zero_velocity, double& belt_torque_offset, - double hold_torque_value, const double& torque_offset_value, uint64_t hold_duration_ticks) + double hold_torque_value, uint64_t hold_duration_ticks) : IAction(std::move(name)) , belt_command_(belt_command) , belt_target_velocity_(belt_target_velocity) @@ -25,15 +25,13 @@ class BeltHoldTorqueAction : public IAction { , belt_wait_zero_velocity_(belt_wait_zero_velocity) , belt_torque_offset_(belt_torque_offset) , hold_torque_value_(hold_torque_value) - , torque_offset_value_(torque_offset_value) , hold_duration_ticks_(hold_duration_ticks) {} void on_enter() override { belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; belt_target_velocity_ = 0.0; belt_hold_torque_ = hold_torque_value_; - belt_wait_zero_velocity_ = true; // 使用 HOLD_TORQUE 模式（常数力矩） - belt_torque_offset_ = torque_offset_value_; // 叠加常态扭矩偏移补偿负载 + belt_wait_zero_velocity_ = false; // 使用 HOLD_TORQUE 模式（常数力矩） } ActionStatus update() override { @@ -56,7 +54,6 @@ class BeltHoldTorqueAction : public IAction { bool& belt_wait_zero_velocity_; double& belt_torque_offset_; double hold_torque_value_; - const double& torque_offset_value_; uint64_t hold_duration_ticks_; }; diff --git a/src/manager/action/filling_limit_servo_action.hpp b/src/manager/action/filling_limit_servo_action.hpp index 80cbc0f..b8783e1 100644 --- a/src/manager/action/filling_limit_servo_action.hpp +++ b/src/manager/action/filling_limit_servo_action.hpp @@ -2,8 +2,6 @@ #include "action.hpp" -#include - #include namespace rmcs_dart_guidance::manager { @@ -14,18 +12,16 @@ class FillingLimitServoAction : public IAction { FillingLimitServoAction( rmcs_msgs::DartLimitingServoStatus& limiting_command, rmcs_msgs::DartLimitingServoStatus trigger_command, - rmcs_msgs::DartLimitingServoStatus lock_command, - uint64_t fill_ticks) + rmcs_msgs::DartLimitingServoStatus lock_command) : IAction("limiting_fill") , limiting_command_(limiting_command) , trigger_command_(trigger_command) - , lock_command_(lock_command) - , fill_ticks_(fill_ticks) {} + , lock_command_(lock_command) {} void on_enter() override { limiting_command_ = trigger_command_; } ActionStatus update() override { - if (elapsed_ticks() >= fill_ticks_) { + if (elapsed_ticks() >= 200) { limiting_command_ = lock_command_; return ActionStatus::SUCCESS; } @@ -38,7 +34,6 @@ class FillingLimitServoAction : public IAction { rmcs_msgs::DartLimitingServoStatus& limiting_command_; rmcs_msgs::DartLimitingServoStatus trigger_command_; rmcs_msgs::DartLimitingServoStatus lock_command_; - uint64_t fill_ticks_; }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/manual_force_control.hpp b/src/manager/action/manual_force_control.hpp index 98feef2..e69e119 100644 --- a/src/manager/action/manual_force_control.hpp +++ b/src/manager/action/manual_force_control.hpp @@ -17,20 +17,14 @@ class DartManualForceControlAction : public IAction { double& force_control_velocity, const Eigen::Vector2d& joystick_right, double max_transform_rate, double manual_force_scale = 5.0, const double* force_screw_velocity_feedback = nullptr, - const double* force_screw_torque_feedback = nullptr, - double stall_velocity_threshold = 0.15, double stall_torque_threshold = 0.5, - uint64_t stall_confirm_ticks = 100, uint64_t min_running_ticks = 100) + const double* force_screw_torque_feedback = nullptr) : IAction("dart_manual_force_control") , force_control_velocity_(force_control_velocity) , joystick_right_(joystick_right) , max_transform_rate_(max_transform_rate) , manual_force_scale_(manual_force_scale) , force_screw_velocity_feedback_(force_screw_velocity_feedback) - , force_screw_torque_feedback_(force_screw_torque_feedback) - , stall_velocity_threshold_(stall_velocity_threshold) - , stall_torque_threshold_(stall_torque_threshold) - , stall_confirm_ticks_(stall_confirm_ticks) - , min_running_ticks_(min_running_ticks) {} + , force_screw_torque_feedback_(force_screw_torque_feedback) {} void on_enter() override { force_control_velocity_ = 0.0; @@ -48,14 +42,13 @@ class DartManualForceControlAction : public IAction { return ActionStatus::RUNNING; } - if (elapsed_ticks() > min_running_ticks_) { + if (elapsed_ticks() > 100) { const double measured_velocity = std::abs(*force_screw_velocity_feedback_); const double measured_torque = std::abs(*force_screw_torque_feedback_); - if (measured_velocity < stall_velocity_threshold_ - && measured_torque > stall_torque_threshold_) { + if (measured_velocity < 0.15 && measured_torque > 0.5) { ++stall_counter_; - if (stall_counter_ >= stall_confirm_ticks_) { + if (stall_counter_ >= 100) { return ActionStatus::SUCCESS; } } else { @@ -79,10 +72,6 @@ class DartManualForceControlAction : public IAction { double manual_force_scale_; const double* force_screw_velocity_feedback_; const double* force_screw_torque_feedback_; - double stall_velocity_threshold_; - double stall_torque_threshold_; - uint64_t stall_confirm_ticks_; - uint64_t min_running_ticks_; uint64_t stall_counter_{0}; }; diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index c88b9a9..d5c673d 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -109,40 +109,16 @@ class DartManager if (launch_prepare_enable_visual_assist_) { load_auto_aim_configuration(); } - belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m - belt_pulley_radius_ = get_parameter("belt_pulley_radius").as_double(); // m - - belt_prepare_down_velocity_first_ = - get_parameter("belt_prepare_down_velocity_first").as_double(); - - belt_prepare_down_velocity_ = get_parameter("belt_prepare_down_velocity").as_double(); - - belt_prepare_torque_limit_ = get_parameter("belt_prepare_torque_limit").as_double(); - - belt_prepare_up_torque_limit_ = get_parameter("belt_prepare_up_torque_limit").as_double(); - - belt_prepare_down_torque_offset_ = - get_parameter("belt_prepare_down_torque_offset").as_double(); - - belt_prepare_down_hold_torque_ = get_parameter("belt_prepare_down_hold_torque").as_double(); belt_up_distance_ = get_parameter("belt_up_distance").as_double(); - belt_prepare_up_velocity_ = get_parameter("belt_prepare_up_velocity").as_double(); - - belt_up_decel_target_velocity_ = get_parameter("belt_up_decel_target_velocity").as_double(); - - belt_up_decel_torque_offset_ = get_parameter("belt_up_decel_torque_offset").as_double(); - enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); - force_tolerance_ = get_parameter("force_tolerance").as_double(); - + force_settle_ticks_ = (uint64_t)get_parameter("force_settle_ticks").as_double(); + force_timeout_ticks_ = (uint64_t)get_parameter("force_timeout_ticks").as_double(); force_kp_ = get_parameter("force_kp").as_double(); - force_ki_ = get_parameter("force_ki").as_double(); - force_kd_ = get_parameter("force_kd").as_double(); force_channel_ = (int)get_parameter("force_channel").as_int(); @@ -153,8 +129,6 @@ class DartManager force_channel_ = 1; } - belt_torque_scale_ = get_parameter("belt_torque_scale").as_double(); - state_pub_ = create_publisher("/dart/manager/state", 10); sync_current_dart_outputs(); @@ -534,8 +508,7 @@ class DartManager std::shared_ptr make_task(const std::string& cmd) { if (cmd == "launch_prepare" || cmd == "launch-prepare") { // 根据当前 fire_count 选择下降速度（fire_count=0 表示第一次准备） - double down_velocity = (fire_count_ == 0) ? belt_prepare_down_velocity_first_ - : belt_prepare_down_velocity_; + double down_velocity = (fire_count_ == 0) ? 8.0 : 10.0; bool require_lifting_down = (fire_count_ > 0); bool is_first_shot = (fire_count_ == 0); @@ -552,10 +525,8 @@ class DartManager RCLCPP_WARN(logger_, "[DartManager] Belt angle feedback NOT READY!"); } RCLCPP_INFO( - logger_, - "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m, " - "down_velocity=%.2f rad/s", - belt_down_distance_, belt_pulley_radius_, down_velocity); + logger_, "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m", + belt_down_distance_, down_velocity); // 记录当前力值（用于下次fire前的力矩闭环，根据 force_channel_ 选择通道） bool force_ready = @@ -572,37 +543,32 @@ class DartManager return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *belt_torque_offset_, *belt_error_gain_, - *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, - *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, - *trigger_lock_enable_, belt_down_distance_, down_velocity, - belt_prepare_down_hold_torque_, require_lifting_down, *lifting_command_, - *lifting_left_vel_fb_, *lifting_right_vel_fb_, *belt_zero_calibration_, - belt_up_distance_, belt_up_decel_target_velocity_, *force_control_velocity_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, + *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, + *trigger_lock_enable_, belt_down_distance_, down_velocity, require_lifting_down, + *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, + *belt_zero_calibration_, belt_up_distance_, *force_control_velocity_, *current_force_ch1_, *current_force_ch2_, force_channel_, last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, - force_timeout_ticks_, force_kp_, force_ki_, force_kd_, force_max_velocity_, - is_first_shot, belt_torque_scale_, get_logger()); + force_timeout_ticks_, force_kp_, force_ki_, force_kd_, is_first_shot); } if (cmd == "unload" || cmd == "cancel_launch") { bool require_lifting_up = (fire_count_ > 0); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, - *belt_wait_zero_velocity_, *belt_torque_offset_, *belt_error_gain_, - *belt_use_decel_pid_, *left_belt_angle_, *right_belt_angle_, *left_belt_velocity_, - *right_belt_velocity_, *left_belt_torque_, *right_belt_torque_, + *belt_wait_zero_velocity_, *belt_torque_offset_, *left_belt_angle_, + *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, - *lifting_right_vel_fb_, belt_down_distance_, belt_prepare_down_hold_torque_, - belt_prepare_down_zero_velocity_threshold_, belt_prepare_down_zero_confirm_ticks_, - belt_prepare_down_ramp_timeout_ticks_, *belt_zero_calibration_, require_lifting_up); + *lifting_right_vel_fb_, belt_down_distance_, *belt_zero_calibration_, + belt_up_distance_, require_lifting_up); } if (cmd == "fire") { bool is_first_shot = (fire_count_ == 0); return std::make_shared( *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, - *lifting_right_vel_fb_, *limiting_command_, limiting_fill_ticks_, is_first_shot); + *lifting_right_vel_fb_, *limiting_command_, is_first_shot); } if (cmd == "manual_angle") { @@ -632,9 +598,8 @@ class DartManager task->then( std::make_shared( *force_control_velocity_, *joystick_right_, max_transform_rate_, - manual_force_scale_, force_screw_velocity_feedback, force_screw_torque_feedback, - force_stall_velocity_threshold_, force_stall_torque_threshold_, - force_stall_confirm_ticks_, force_stall_min_run_ticks_)); + manual_force_scale_, force_screw_velocity_feedback, + force_screw_torque_feedback)); return task; } return nullptr; @@ -688,42 +653,20 @@ class DartManager double max_transform_rate_{500.0}; double manual_force_scale_{5.0}; double auto_aim_max_transform_rate_{500.0}; - uint64_t limiting_fill_ticks_{500}; - - double belt_down_distance_{0.0}; // m - double belt_pulley_radius_{0.0}; // m - - double belt_prepare_down_velocity_first_{5.0}; // rad/s - double belt_prepare_down_velocity_{10.0}; // rad/s - double belt_prepare_torque_limit_{5.0}; // N⋅m - double belt_prepare_up_torque_limit_{1.5}; // N⋅m - double belt_prepare_down_torque_offset_{2.0}; // N⋅m - double belt_prepare_down_hold_torque_{5.0}; // N⋅m - double belt_prepare_down_zero_velocity_threshold_{0.15}; // rad/s - uint64_t belt_prepare_down_zero_confirm_ticks_{60}; // ticks - uint64_t belt_prepare_down_ramp_timeout_ticks_{2000}; // ticks - - // 传送带上行控制参数 - double belt_up_distance_{0.65}; // m - 上行目标距离（略低于下行起点） - double belt_prepare_up_velocity_{15.0}; // rad/s - 上行速度 - double belt_up_decel_target_velocity_{1.0}; // rad/s - 上行减速阶段目标速度 - double belt_up_decel_torque_offset_{0.0}; // N⋅m - 上行减速阶段力矩偏移 + + double belt_down_distance_{0.0}; // m + + double belt_up_distance_{0.70}; // 力矩闭环参数 bool enable_force_calibration_{false}; - double force_tolerance_{5.0}; // N + double force_tolerance_{5.0}; // N uint64_t force_settle_ticks_{50}; uint64_t force_timeout_ticks_{2000}; double force_kp_{0.1}; double force_ki_{0.0}; double force_kd_{0.01}; - double force_max_velocity_{5.0}; // rad/s - double force_stall_velocity_threshold_{0.15}; - double force_stall_torque_threshold_{0.5}; - uint64_t force_stall_confirm_ticks_{100}; - uint64_t force_stall_min_run_ticks_{100}; - int force_channel_{1}; // 1 = ch1, 2 = ch2 - double belt_torque_scale_{0.0005}; // N⋅m/g，传送带自适应力矩补偿倍率 + int force_channel_{1}; // 1 = ch1, 2 = ch2 bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; @@ -750,9 +693,9 @@ class DartManager std::shared_ptr current_task_; std::deque> task_queue_; bool first_fill_pending_{true}; - uint32_t fire_count_{0}; // 当前轮次已完成发射数 + uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; - double last_fire_force_{0.0}; // 上次fire前记录的力值 + double last_fire_force_{0.0}; // 上次fire前记录的力值 }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index d7c822f..62dc1bf 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -2,9 +2,7 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" -#include "manager/action/belt_deceleration_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" -#include "manager/action/belt_move_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -12,7 +10,6 @@ #include "manager/task/task.hpp" #include -#include #include #include @@ -30,89 +27,69 @@ class CancelLaunchTask : public Task { CancelLaunchTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, - const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, - rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, - const double& lifting_right_vel_fb, double belt_down_distance, double down_hold_torque, - double down_zero_velocity_threshold, uint64_t down_zero_confirm_ticks, - uint64_t down_ramp_timeout_ticks, bool& belt_zero_calibration, + bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, + const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, + double belt_down_distance, bool& belt_zero_calibration, double belt_up_distance, bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） then( std::make_shared( - "belt_move_down_constant_velocity", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） 0.0, - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - +belt_down_distance, // 目标距离（m，正值=下行） - 0.0195, // 滑轮半径（m） - 10, // 运动速度（rad/s） - 10, // 扭矩限制（N⋅m） - 10000, // 超时帧数 - 100, // 最小运行帧数 - 0.5, // 位置到达容差（rad）- 增大容差 - 0.3, // 堵转速度阈值（rad/s） - 200, // 堵转确认帧数 - 0.80)); // 下行最大距离限制（m，正值） - - // 步骤2：减速阶段（目标速度=0，加常态力矩偏移补偿负载，使用零速检测） - then( - std::make_shared( - "belt_down_ramp_to_zero", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_error_gain, // 误差增益（输出） - belt_use_decel_pid, // 使用减速PID（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - 0.0, // 目标速度（rad/s） - down_hold_torque, // 力矩偏移值（N⋅m） - 2.0, // 误差增益倍数 - true, // 启用堵转检测 - true, // 启用零速检测 - 0.3, // 堵转速度阈值（rad/s） - 1.0, // 堵转扭矩阈值（N⋅m） - down_zero_velocity_threshold, // 零速阈值（rad/s） - 50, // 堵转确认帧数 - down_zero_confirm_ticks, // 零速确认帧数 - 50, // 最小运行帧数 - down_ramp_timeout_ticks)); // 超时帧数 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance, // 目标距离（m，正值=下行） + 10, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） + 10000)); // 超时帧数 + + auto down_and_hold_ = std::make_shared("down_and_hold"); + down_and_hold_ + ->then( + std::make_shared( + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +0.1, // 目标距离（m，正值=下行） + 8, // 运动速度（rad/s） + 5, // 扭矩限制（N⋅m） + 10000 // 超时帧数 + )) + .then( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + 5.0, // 保持力矩值（N⋅m） + 2000)); // 步骤3：传送带保持高扭矩 + 解锁扳机 + (可选)升降上行并行 auto parallel_hold_unlock_and_lift = std::make_shared("hold_unlock_and_lift", ActionSet::Policy::ALL_SUCCESS); - parallel_hold_unlock_and_lift - ->also( - std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - 2.5, // 力矩偏移值（N⋅m）- 取消发射不需要偏移 - 500)) // 保持时长（ticks） - .also( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 500)); // 等待释放完成帧数 - - // 只有在需要时才添加升降上行动作（第二发及以后才需要） + parallel_hold_unlock_and_lift->also(down_and_hold_) + .also(std::make_shared(trigger_lock_enable, false, 500)); + if (require_lifting_up) { parallel_hold_unlock_and_lift->also( std::make_shared( @@ -129,30 +106,56 @@ class CancelLaunchTask : public Task { then(parallel_hold_unlock_and_lift); - // 步骤4：同步带上行复位到机械限位 then( - std::make_shared( - "belt_reset", // 动作名称 - belt_command, // 同步带目标状态（输出） - belt_target_velocity, // 同步带目标速度（输出） - belt_torque_limit, // 同步带力矩限制（输出） - belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 - left_belt_velocity, // 左同步带反馈（输入） - right_belt_velocity, // 右同步带反馈（输入） - left_belt_torque, // 左同步带力矩（输入） - right_belt_torque, // 右同步带力矩（输入） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - 10, // 设定速度 - 3.0, // 设定力矩限制 - 0.5, // 设定保持力矩 - 10000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY, // 退出模式 - false)); // 超时返回失败 + std::make_shared( + "belt_up_first_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 1.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.01), // 目标距离 + 10.0, // 快速（rad/s） + 5.0, // 扭矩限制（N⋅m） + 10000)); + + then( + std::make_shared( + "belt_up_second_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.70), // 目标距离 + 15.0, // 快速（rad/s） + 3.0, // 扭矩限制（N⋅m） + 10000)); + + then( + std::make_shared( + "belt_up_third_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值（无） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.30), // 目标距离） + 5.0, // 快速（rad/s） + 0.8, // 扭矩限制（N⋅m） + 10000)); then(std::make_shared("stabilize_wait", 50)); diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index 28bd43f..575117c 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -5,7 +5,6 @@ #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" -#include #include #include @@ -22,8 +21,7 @@ class FireAndPreloadTask : public Task { FireAndPreloadTask( bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - rmcs_msgs::DartLimitingServoStatus& limiting_command, uint64_t preload_fill_ticks, - bool is_first_shot = false) + rmcs_msgs::DartLimitingServoStatus& limiting_command, bool is_first_shot = false) : Task("fire", "发射并预装填") { // 第一发特殊处理：只解锁扳机，无需升降和预装填 @@ -60,8 +58,7 @@ class FireAndPreloadTask : public Task { std::make_shared( limiting_command, // 限位舵机状态（输出） rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 - rmcs_msgs::DartLimitingServoStatus::LOCK, // 再锁回 - preload_fill_ticks // 预装填持续帧数 + rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 )); } } diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index d46afcf..6f56ce1 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -2,9 +2,7 @@ #include "manager/action/action_set.hpp" #include "manager/action/belt_constant_velocity_move_action.hpp" -#include "manager/action/belt_deceleration_action.hpp" #include "manager/action/belt_hold_torque_action.hpp" -#include "manager/action/belt_up_initial_accel_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" @@ -27,101 +25,73 @@ class LaunchPreparationTask : public Task { LaunchPreparationTask( rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, - const double& left_belt_angle, const double& right_belt_angle, + double& belt_torque_offset, const double& left_belt_angle, const double& right_belt_angle, const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque, bool& trigger_lock_enable, - double belt_down_distance, double down_velocity, double down_hold_torque, + bool& trigger_lock_enable, double belt_down_distance, double down_velocity, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - bool& belt_zero_calibration, double belt_up_distance, double up_decel_target_velocity, - double& force_screw_velocity, const int& current_force_ch1, const int& current_force_ch2, - int force_channel, double target_force, bool enable_force_calibration, - double force_tolerance, uint64_t force_settle_ticks, uint64_t force_timeout_ticks, - double force_kp, double force_ki, double force_kd, double force_max_velocity, - bool is_first_shot = false, double belt_torque_scale = 0.0005, - const rclcpp::Logger& logger = rclcpp::get_logger("launch_preparation_task")) + bool& belt_zero_calibration, double belt_up_distance, double& force_screw_velocity, + const int& current_force_ch1, const int& current_force_ch2, int force_channel, + double target_force, bool enable_force_calibration, double force_tolerance, + uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, + double force_kd, bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） // 注意：target_distance为正值表示下行（角度增大方向） then( std::make_shared( - "belt_move_down_constant_velocity", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - 0.0, // 力矩偏移值 - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - +belt_down_distance, // 目标距离（m，正值=下行） - 0.0195, // 滑轮半径（m） - down_velocity, // 运动速度（rad/s） - 10, // 扭矩限制（N⋅m） - 10000, // 超时帧数 - 100, // 最小运行帧数 - 0.5, // 位置到达容差（rad） - 0.3, // 堵转速度阈值（rad/s） - 100, // 堵转确认帧数 - 0.80)); // 下行最大距离限制（m，正值） - - // 步骤2：减速阶段（目标速度=0，加自适应力矩偏移补偿负载，使用减速PID+error增益） - // on_enter_hook 在减速启动的同一帧内采样力传感器，计算三个阶段的自适应补偿值 - // 同时启用零速检测和堵转检测：正常减速到0或碰到机械限位都能成功 - then( - std::make_shared( - "belt_deceleration", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - 0.0, // 目标速度（rad/s） - adaptive_decel_offset_, // 力矩偏移值（N⋅m，自适应） - 5.0, // error增益倍数 - true, // 启用堵转检测（碰到机械限位） - true, // 启用零速检测（正常减速到0） - 0.3, // 堵转速度阈值（rad/s） - 1.0, // 堵转扭矩阈值（N⋅m） - 0.05, // 零速阈值（rad/s） - 50, // 堵转确认帧数 - 0.05, // 零速确认帧数 - 50, // 最小运行帧数 - 800, // 超时帧数 - nullptr, nullptr, false, - [this, ¤t_force_ch1, ¤t_force_ch2, force_channel, belt_torque_scale, - logger]() { - double force_g = static_cast( - force_channel == 2 ? current_force_ch2 : current_force_ch1); - double base = force_g * belt_torque_scale; - adaptive_decel_offset_ = base * 0.75; - adaptive_hold_offset_ = base * 1.0; - adaptive_up_accel_offset_ = base * 0.5; - })); + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance, // 目标距离（m，正值=下行） + down_velocity, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） + 10000 // 超时帧数 + )); + + auto down_and_hold_ = std::make_shared("down_and_hold"); + down_and_hold_ + ->then( + std::make_shared( + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +0.1, // 目标距离（m，正值=下行） + 8, // 运动速度（rad/s） + 5, // 扭矩限制（N⋅m） + 10000 // 超时帧数 + )) + .then( + std::make_shared( + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + 5.0, // 保持力矩值（N⋅m） + 2000)); if (require_lifting_down) { - // 第2-4发：传送带保持高扭矩 + 升降平台下行 + 扳机锁定并行 auto parallel_hold_lock_and_lift = std::make_shared("hold_lock_and_lift", ActionSet::Policy::ALL_SUCCESS); - parallel_hold_lock_and_lift - ->also( - std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - adaptive_hold_offset_, // 力矩偏移值（N⋅m，自适应） - 1000)) // 保持时长（ticks） + parallel_hold_lock_and_lift->also(down_and_hold_) .also( std::make_shared( trigger_lock_enable, // 扳机锁定使能（输出） @@ -144,97 +114,65 @@ class LaunchPreparationTask : public Task { auto parallel_hold_and_lock = std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); - parallel_hold_and_lock - ->also( - std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - down_hold_torque, // 保持力矩值（N⋅m） - adaptive_hold_offset_, // 力矩偏移值（N⋅m，自适应） - 500)) // 保持时长（ticks） + parallel_hold_and_lock->also(down_and_hold_) .also( std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - true, // 锁定（true） - 500)); // 等待锁定完成帧数 + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 500)); // 等待锁定完成帧数 then(parallel_hold_and_lock); } - // 步骤4a：上行初始加速阶段（前0.2m慢速 + 常态力矩偏移） then( - std::make_shared( - "belt_up_initial_accel", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - 0.0195, // 滑轮半径（m） - 5.0, // 慢速（rad/s） - adaptive_up_accel_offset_, // 力矩偏移值（N⋅m，自适应） - 3.0, // 扭矩限制（N⋅m） - 0.05, // 加速距离（m） - 5000)); // 超时帧数 + std::make_shared( + "belt_up_first_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 1.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.01), // 目标距离 + 10.0, // 快速（rad/s） + 5.0, // 扭矩限制（N⋅m） + 10000)); - // 步骤4b：上行匀速阶段（到达目标位置前，无力矩偏移） - // 目标位置 = 初始位置 - (belt_up_distance - 0.2) then( std::make_shared( - "belt_up_cruise", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - 0.0, // 力矩偏移值（无） - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.2), // 目标距离（m，负值=上行，减去已行进的0.2m） - 0.0195, // 滑轮半径（m） - 12.0, // 快速（rad/s） - 3.0, // 扭矩限制（N⋅m） - 10000, // 超时帧数 - 50, // 最小运行帧数 - 0.5, // 位置到达容差（rad） - 0.3, // 堵转速度阈值（rad/s） - 100, // 堵转确认帧数 - 0.90)); // 下行最大距离限制（m，正值） + "belt_up_second_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.70), // 目标距离 + 15.0, // 快速（rad/s） + 3.0, // 扭矩限制（N⋅m） + 10000)); - // 步骤4c：上行减速+堵转检测（堵转标志成功，作为下次下行起点，无力矩偏移） then( - std::make_shared( - "belt_up_decel_and_stall", // 动作名称 - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_error_gain, // error增益（输出） - belt_use_decel_pid, // 使用减速PID标志（输出） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - left_belt_torque, // 左电机力矩反馈（输入） - right_belt_torque, // 右电机力矩反馈（输入） - up_decel_target_velocity, // 目标速度（rad/s） - 0.0, // 力矩偏移值（无） - 1.0, // error增益倍数（正常） - true, // 启用堵转检测 - false, // 不启用零速检测 - 0.15, // 堵转速度阈值（rad/s） - 0.5, // 堵转扭矩阈值（N⋅m） - 0.1, // 零速阈值（未使用） - 100, // 堵转确认帧数 - 100, // 零速确认帧数（未使用） - 300, // 最小运行帧数 - 5000, // 超时帧数 - &belt_command, // 仅上行减速结束时显式下发 WAIT - &belt_wait_zero_velocity, // 上行减速结束后显式进入 WAIT_ZERO - true)); // 仅上行减速结束时显式下发 WAIT/0 + std::make_shared( + "belt_up_third_stage", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值（无） + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + -(belt_up_distance - 0.30), // 目标距离） + 5.0, // 快速（rad/s） + 0.8, // 扭矩限制（N⋅m） + 10000)); then(std::make_shared("stabilize_wait", 50)); @@ -249,15 +187,8 @@ class LaunchPreparationTask : public Task { std::make_shared( "force_screw_calibration", force_screw_velocity, current_force_ch1, current_force_ch2, force_channel, target_force_value, tolerance_value, - force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, - force_max_velocity)); + force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, 5.0)); } } - -private: - double adaptive_decel_offset_{0.0}; - double adaptive_hold_offset_{0.0}; - double adaptive_up_accel_offset_{0.0}; }; - -} // namespace rmcs_dart_guidance::manager +} // namespace rmcs_dart_guidance::manager \ No newline at end of file diff --git a/tests/manual_force_control_test.cpp b/tests/manual_force_control_test.cpp index e12a5d5..70ef4e8 100644 --- a/tests/manual_force_control_test.cpp +++ b/tests/manual_force_control_test.cpp @@ -26,8 +26,7 @@ TEST(DartManualForceControlActionTest, ReturnsSuccessAfterConfirmedStall) { double measured_torque = 0.8; DartManualForceControlAction action( - force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque, - 0.15, 0.5, 3, 0); + force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque); EXPECT_EQ(action.tick_first(), ActionStatus::RUNNING); EXPECT_DOUBLE_EQ(force_control_velocity, 15.0); @@ -46,8 +45,7 @@ TEST(DartManualForceControlActionTest, ResetsStallCounterWhenCommandDropsToZero) double measured_torque = 0.8; DartManualForceControlAction action( - force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque, - 0.15, 0.5, 3, 0); + force_control_velocity, joystick_right, 5.0, 3.0, &measured_velocity, &measured_torque); EXPECT_EQ(action.tick_first(), ActionStatus::RUNNING); EXPECT_EQ(action.tick(), ActionStatus::RUNNING); From df49d23aa1778c3e4146ef06c441289d6c521afc Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Fri, 17 Apr 2026 16:12:10 +0800 Subject: [PATCH 16/19] cleaned parameter further and test faster speed ac --- .../belt_launch_prepare_program_action.hpp | 214 ------------------ .../action/belt_up_initial_accel_action.hpp | 87 ------- src/manager/dart_manager.cpp | 20 +- src/manager/task/cancel_launch_task.hpp | 15 +- src/manager/task/launch_preparation_task.hpp | 31 ++- 5 files changed, 30 insertions(+), 337 deletions(-) delete mode 100644 src/manager/action/belt_launch_prepare_program_action.hpp delete mode 100644 src/manager/action/belt_up_initial_accel_action.hpp diff --git a/src/manager/action/belt_launch_prepare_program_action.hpp b/src/manager/action/belt_launch_prepare_program_action.hpp deleted file mode 100644 index 5aca12a..0000000 --- a/src/manager/action/belt_launch_prepare_program_action.hpp +++ /dev/null @@ -1,214 +0,0 @@ -#pragma once - -#include "action.hpp" - -#include -#include -#include - -#include - -namespace rmcs_dart_guidance::manager { - -// BeltLaunchPrepareProgramAction -// 触发 controller 内部的传送带阶段程序（LaunchPreparationTask 专用），并在 manager 侧基于反馈等待其完成。 -// -// 说明： -// - 为避免 rmcs_executor 的循环依赖（manager <-> controller），该 Action 不读取 controller 的 program_done。 -// - 完成条件严格复刻此前 manager 内 BeltDecelerationAction 的检测逻辑（零速/堵转）。 -// - 速度/距离/切换点等阶段字面量由 controller 内部实现；manager 仅注入力矩补偿与力矩限幅。 -class BeltLaunchPrepareProgramAction : public IAction { -public: - enum class Program : uint8_t { - LAUNCH_PREP_DOWN = 1, - LAUNCH_PREP_UP = 2, - }; - - BeltLaunchPrepareProgramAction( - std::string name, uint8_t& belt_program_cmd, bool& belt_program_first_shot, - double& belt_program_down_comp_torque, double& belt_program_up_comp_torque, - double& belt_torque_limit, rmcs_msgs::DartSliderStatus& belt_command, - double& belt_target_velocity, double& belt_hold_torque, bool& belt_wait_zero_velocity, - double& belt_torque_offset, double& belt_error_gain, bool& belt_use_decel_pid, - const double& left_belt_velocity, const double& right_belt_velocity, - const double& left_belt_torque, const double& right_belt_torque, Program program, - bool first_shot, double down_comp_torque_value, double up_comp_torque_value, - double torque_limit_value, uint64_t timeout_ticks) - : IAction(std::move(name)) - , belt_program_cmd_(belt_program_cmd) - , belt_program_first_shot_(belt_program_first_shot) - , belt_program_down_comp_torque_(belt_program_down_comp_torque) - , belt_program_up_comp_torque_(belt_program_up_comp_torque) - , belt_torque_limit_(belt_torque_limit) - , belt_command_(belt_command) - , belt_target_velocity_(belt_target_velocity) - , belt_hold_torque_(belt_hold_torque) - , belt_wait_zero_velocity_(belt_wait_zero_velocity) - , belt_torque_offset_(belt_torque_offset) - , belt_error_gain_(belt_error_gain) - , belt_use_decel_pid_(belt_use_decel_pid) - , left_belt_velocity_(left_belt_velocity) - , right_belt_velocity_(right_belt_velocity) - , left_belt_torque_(left_belt_torque) - , right_belt_torque_(right_belt_torque) - , program_(program) - , first_shot_(first_shot) - , down_comp_torque_value_(down_comp_torque_value) - , up_comp_torque_value_(up_comp_torque_value) - , torque_limit_value_(torque_limit_value) - , timeout_ticks_(timeout_ticks) {} - - void on_enter() override { - belt_program_cmd_ = static_cast(program_); - belt_program_first_shot_ = first_shot_; - belt_program_down_comp_torque_ = down_comp_torque_value_; - belt_program_up_comp_torque_ = up_comp_torque_value_; - belt_torque_limit_ = torque_limit_value_; - - stall_counter_ = 0; - zero_counter_ = 0; - } - - ActionStatus update() override { - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::SUCCESS; - } - - const double avg_velocity = - (std::abs(left_belt_velocity_) + std::abs(right_belt_velocity_)) / 2.0; - - if (program_ == Program::LAUNCH_PREP_DOWN) { - return update_down(avg_velocity); - } - - return update_up(avg_velocity); - } - - void on_exit() override { - belt_program_cmd_ = 0; - - if (program_ == Program::LAUNCH_PREP_UP) { - // 复刻原 BeltDecelerationAction(send_wait_zero_on_exit=true) 的退出效果： - // 1) 显式下发 WAIT/0 - // 2) 切换到 WAIT_ZERO（零速闭环） - // 3) 清理 torque_offset/error_gain/use_decel_pid - belt_command_ = rmcs_msgs::DartSliderStatus::WAIT; - belt_target_velocity_ = 0.0; - belt_hold_torque_ = 0.0; - belt_wait_zero_velocity_ = true; - belt_torque_offset_ = 0.0; - belt_error_gain_ = 1.0; - belt_use_decel_pid_ = false; - return; - } - - // 下行程序结束后下一步立刻进入 hold/lock；这里仅做最小清理，避免残留。 - belt_torque_offset_ = 0.0; - belt_error_gain_ = 1.0; - belt_use_decel_pid_ = false; - belt_wait_zero_velocity_ = false; - } - -private: - ActionStatus update_down(double avg_velocity) { - // 复刻 LaunchPreparationTask 内 BeltDecelerationAction 的检测参数（下行减速）： - // - stall: vel < 0.3 & torque_active, confirm 50 - // - zero : vel < 0.05 & !torque_active, confirm 60 - // - min_running_ticks: 50 - static constexpr uint64_t kMinRunningTicks = 50; - static constexpr double kStallVelocityThreshold = 0.3; - static constexpr double kStallTorqueThreshold = 1.0; - static constexpr uint64_t kStallConfirmTicks = 50; - static constexpr double kZeroVelocityThreshold = 0.05; - static constexpr uint64_t kZeroConfirmTicks = 60; - - const bool torque_active = - std::abs(left_belt_torque_) > kStallTorqueThreshold - || std::abs(right_belt_torque_) > kStallTorqueThreshold; - - if (elapsed_ticks() > kMinRunningTicks) { - if (avg_velocity < kStallVelocityThreshold && torque_active) { - ++stall_counter_; - if (stall_counter_ >= kStallConfirmTicks) { - return ActionStatus::SUCCESS; - } - } else { - stall_counter_ = 0; - } - - if (!torque_active) { - if (avg_velocity < kZeroVelocityThreshold) { - ++zero_counter_; - if (zero_counter_ >= kZeroConfirmTicks) { - return ActionStatus::SUCCESS; - } - } else { - zero_counter_ = 0; - } - } else { - zero_counter_ = 0; - } - } - - return ActionStatus::RUNNING; - } - - ActionStatus update_up(double avg_velocity) { - // 复刻 LaunchPreparationTask 内 BeltDecelerationAction 的检测参数（上行减速+堵转）： - // - stall: vel < 0.15 & torque_active, confirm 100 - // - min_running_ticks: 300 - static constexpr uint64_t kMinRunningTicks = 300; - static constexpr double kStallVelocityThreshold = 0.15; - static constexpr double kStallTorqueThreshold = 0.5; - static constexpr uint64_t kStallConfirmTicks = 100; - - const bool torque_active = - std::abs(left_belt_torque_) > kStallTorqueThreshold - || std::abs(right_belt_torque_) > kStallTorqueThreshold; - - if (elapsed_ticks() > kMinRunningTicks) { - if (avg_velocity < kStallVelocityThreshold && torque_active) { - ++stall_counter_; - if (stall_counter_ >= kStallConfirmTicks) { - return ActionStatus::SUCCESS; - } - } else { - stall_counter_ = 0; - } - } - - return ActionStatus::RUNNING; - } - - uint8_t& belt_program_cmd_; - bool& belt_program_first_shot_; - double& belt_program_down_comp_torque_; - double& belt_program_up_comp_torque_; - double& belt_torque_limit_; - - rmcs_msgs::DartSliderStatus& belt_command_; - double& belt_target_velocity_; - double& belt_hold_torque_; - bool& belt_wait_zero_velocity_; - double& belt_torque_offset_; - double& belt_error_gain_; - bool& belt_use_decel_pid_; - - const double& left_belt_velocity_; - const double& right_belt_velocity_; - const double& left_belt_torque_; - const double& right_belt_torque_; - - Program program_; - bool first_shot_; - double down_comp_torque_value_; - double up_comp_torque_value_; - double torque_limit_value_; - uint64_t timeout_ticks_; - - uint64_t stall_counter_{0}; - uint64_t zero_counter_{0}; -}; - -} // namespace rmcs_dart_guidance::manager - diff --git a/src/manager/action/belt_up_initial_accel_action.hpp b/src/manager/action/belt_up_initial_accel_action.hpp deleted file mode 100644 index 3af3366..0000000 --- a/src/manager/action/belt_up_initial_accel_action.hpp +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include "action.hpp" - -#include -#include -#include - -#include - -namespace rmcs_dart_guidance::manager { - -// BeltUpInitialAccelAction - 上行初始加速阶段 -// 前0.1m慢速（5 rad/s）+ 常态力矩偏移补偿负载 -// 完成条件：行进距离达到0.2m -class BeltUpInitialAccelAction : public IAction { -public: - BeltUpInitialAccelAction( - std::string name, rmcs_msgs::DartSliderStatus& belt_command, double& belt_target_velocity, - double& belt_torque_offset, double& belt_torque_limit, double& belt_error_gain, - bool& belt_use_decel_pid, const double& left_belt_angle, const double& right_belt_angle, - double pulley_radius, double slow_velocity, const double& torque_offset_value, double torque_limit, - double accel_distance = 0.1, uint64_t timeout_ticks = 5000) - : IAction(std::move(name)) - , belt_command_(belt_command) - , belt_target_velocity_(belt_target_velocity) - , belt_torque_offset_(belt_torque_offset) - , belt_torque_limit_(belt_torque_limit) - , belt_error_gain_(belt_error_gain) - , belt_use_decel_pid_(belt_use_decel_pid) - , left_belt_angle_(left_belt_angle) - , right_belt_angle_(right_belt_angle) - , pulley_radius_(pulley_radius) - , slow_velocity_(slow_velocity) - , torque_offset_value_(torque_offset_value) - , torque_limit_(torque_limit) - , accel_distance_(accel_distance) - , timeout_ticks_(timeout_ticks) {} - - void on_enter() override { - initial_angle_ = (left_belt_angle_ + right_belt_angle_) / 2.0; - belt_command_ = rmcs_msgs::DartSliderStatus::UP; - belt_target_velocity_ = slow_velocity_; - belt_torque_offset_ = torque_offset_value_; - belt_torque_limit_ = torque_limit_; - belt_error_gain_ = 1.0; // 恢复正常error增益 - belt_use_decel_pid_ = false; // 恢复正常PID参数 - } - - ActionStatus update() override { - if (elapsed_ticks() >= timeout_ticks_) { - return ActionStatus::SUCCESS; - } - - double avg_angle = (left_belt_angle_ + right_belt_angle_) / 2.0; - double distance_traveled = std::abs(avg_angle - initial_angle_) * pulley_radius_; - - if (distance_traveled >= accel_distance_) { - return ActionStatus::SUCCESS; - } - - return ActionStatus::RUNNING; - } - - void on_exit() override { belt_torque_offset_ = 0.0; } - -private: - rmcs_msgs::DartSliderStatus& belt_command_; - double& belt_target_velocity_; - double& belt_torque_offset_; - double& belt_torque_limit_; - double& belt_error_gain_; - bool& belt_use_decel_pid_; - const double& left_belt_angle_; - const double& right_belt_angle_; - - double pulley_radius_; - double slow_velocity_; - const double& torque_offset_value_; - double torque_limit_; - double accel_distance_; - uint64_t timeout_ticks_; - - double initial_angle_{0.0}; -}; - -} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index d5c673d..0732f19 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -111,12 +111,10 @@ class DartManager } belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m - belt_up_distance_ = get_parameter("belt_up_distance").as_double(); - enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); force_tolerance_ = get_parameter("force_tolerance").as_double(); - force_settle_ticks_ = (uint64_t)get_parameter("force_settle_ticks").as_double(); - force_timeout_ticks_ = (uint64_t)get_parameter("force_timeout_ticks").as_double(); + force_settle_ticks_ = (uint64_t)get_parameter("force_settle_ticks").as_int(); + force_timeout_ticks_ = (uint64_t)get_parameter("force_timeout_ticks").as_int(); force_kp_ = get_parameter("force_kp").as_double(); force_ki_ = get_parameter("force_ki").as_double(); force_kd_ = get_parameter("force_kd").as_double(); @@ -508,7 +506,7 @@ class DartManager std::shared_ptr make_task(const std::string& cmd) { if (cmd == "launch_prepare" || cmd == "launch-prepare") { // 根据当前 fire_count 选择下降速度（fire_count=0 表示第一次准备） - double down_velocity = (fire_count_ == 0) ? 8.0 : 10.0; + double down_velocity = (fire_count_ == 0) ? 12.0 : 10.0; bool require_lifting_down = (fire_count_ > 0); bool is_first_shot = (fire_count_ == 0); @@ -547,10 +545,10 @@ class DartManager *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *trigger_lock_enable_, belt_down_distance_, down_velocity, require_lifting_down, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, - *belt_zero_calibration_, belt_up_distance_, *force_control_velocity_, - *current_force_ch1_, *current_force_ch2_, force_channel_, last_fire_force_, - enable_force_calibration_, force_tolerance_, force_settle_ticks_, - force_timeout_ticks_, force_kp_, force_ki_, force_kd_, is_first_shot); + *belt_zero_calibration_, *force_control_velocity_, *current_force_ch1_, + *current_force_ch2_, force_channel_, last_fire_force_, enable_force_calibration_, + force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, + force_kd_, is_first_shot); } if (cmd == "unload" || cmd == "cancel_launch") { @@ -561,7 +559,7 @@ class DartManager *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, *belt_zero_calibration_, - belt_up_distance_, require_lifting_up); + require_lifting_up); } if (cmd == "fire") { @@ -656,8 +654,6 @@ class DartManager double belt_down_distance_{0.0}; // m - double belt_up_distance_{0.70}; - // 力矩闭环参数 bool enable_force_calibration_{false}; double force_tolerance_{5.0}; // N diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index 62dc1bf..5a49647 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -31,8 +31,7 @@ class CancelLaunchTask : public Task { const double& left_belt_velocity, const double& right_belt_velocity, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - double belt_down_distance, bool& belt_zero_calibration, double belt_up_distance, - bool require_lifting_up = true) + double belt_down_distance, bool& belt_zero_calibration, bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） @@ -118,8 +117,8 @@ class CancelLaunchTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.01), // 目标距离 - 10.0, // 快速（rad/s） + -0.01, // 目标距离 + 15.0, // 快速（rad/s） 5.0, // 扭矩限制（N⋅m） 10000)); @@ -135,8 +134,8 @@ class CancelLaunchTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.70), // 目标距离 - 15.0, // 快速（rad/s） + -0.65, // 目标距离 + 20.0, // 快速（rad/s） 3.0, // 扭矩限制（N⋅m） 10000)); @@ -152,8 +151,8 @@ class CancelLaunchTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.30), // 目标距离） - 5.0, // 快速（rad/s） + -0.75, // 目标距离） + 15.0, // 快速（rad/s） 0.8, // 扭矩限制（N⋅m） 10000)); diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 6f56ce1..c72c4fe 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -30,11 +30,11 @@ class LaunchPreparationTask : public Task { bool& trigger_lock_enable, double belt_down_distance, double down_velocity, bool require_lifting_down, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - bool& belt_zero_calibration, double belt_up_distance, double& force_screw_velocity, - const int& current_force_ch1, const int& current_force_ch2, int force_channel, - double target_force, bool enable_force_calibration, double force_tolerance, - uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, - double force_kd, bool is_first_shot = false) + bool& belt_zero_calibration, double& force_screw_velocity, const int& current_force_ch1, + const int& current_force_ch2, int force_channel, double target_force, + bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, + uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, + bool is_first_shot = false) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -51,7 +51,7 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - +belt_down_distance, // 目标距离（m，正值=下行） + +belt_down_distance - 0.1, // 目标距离（m，正值=下行） down_velocity, // 运动速度（rad/s） 10, // 扭矩限制（N⋅m） 10000 // 超时帧数 @@ -135,10 +135,10 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.01), // 目标距离 + -0.05, // 目标距离 10.0, // 快速（rad/s） 5.0, // 扭矩限制（N⋅m） - 10000)); + 1000)); then( std::make_shared( @@ -152,10 +152,10 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.70), // 目标距离 - 15.0, // 快速（rad/s） + -0.65, // 目标距离 + 20.0, // 快速（rad/s） 3.0, // 扭矩限制（N⋅m） - 10000)); + 2000)); then( std::make_shared( @@ -169,16 +169,15 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -(belt_up_distance - 0.30), // 目标距离） - 5.0, // 快速（rad/s） - 0.8, // 扭矩限制（N⋅m） - 10000)); + -0.75, // 目标距离） + 12.0, // 快速（rad/s） + 0.5, // 扭矩限制（N⋅m） + 1000)); then(std::make_shared("stabilize_wait", 50)); then(std::make_shared(belt_zero_calibration)); - // 力矩闭环：第一发使用固定目标力，后续发使用上次记录的力值 if (enable_force_calibration) { double target_force_value = is_first_shot ? 16000.0 : target_force; double tolerance_value = is_first_shot ? 0.5 : force_tolerance; From bbf25bf121cbbc392958c58fcb522c19eb30adae Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Fri, 17 Apr 2026 17:32:21 +0800 Subject: [PATCH 17/19] vedio saved ac --- src/manager/task/cancel_launch_task.hpp | 68 +++--- src/manager/task/launch_preparation_task.hpp | 2 +- src/vision/dart_vision_core.cpp | 63 +++++ src/vision/video_recorder.hpp | 237 +++++++++++++++++++ 4 files changed, 335 insertions(+), 35 deletions(-) create mode 100644 src/vision/video_recorder.hpp diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index 5a49647..bbfc0c9 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -37,49 +37,49 @@ class CancelLaunchTask : public Task { // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） then( std::make_shared( - "belt_move_down_constant_velocity", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） + "belt_move_down_constant_velocity", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） 0.0, - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - +belt_down_distance, // 目标距离（m，正值=下行） - 10, // 运动速度（rad/s） - 10, // 扭矩限制（N⋅m） - 10000)); // 超时帧数 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance, // 目标距离（m，正值=下行） + 12, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） + 10000)); // 超时帧数 auto down_and_hold_ = std::make_shared("down_and_hold"); down_and_hold_ ->then( std::make_shared( - "belt_move_down_constant_velocity", // 动作名称 - belt_command, // 速度模式方向命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_torque_offset, // 力矩偏移（输出） - belt_torque_limit, // 扭矩限幅（输出） - 0.0, // 力矩偏移值 - left_belt_angle, // 左电机角度反馈（输入） - right_belt_angle, // 右电机角度反馈（输入） - left_belt_velocity, // 左电机速度反馈（输入） - right_belt_velocity, // 右电机速度反馈（输入） - +0.1, // 目标距离（m，正值=下行） - 8, // 运动速度（rad/s） - 5, // 扭矩限制（N⋅m） - 10000 // 超时帧数 + "belt_slowly_down", // 动作名称 + belt_command, // 速度模式方向命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_torque_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, // 力矩偏移值 + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +0.1, // 目标距离（m，正值=下行） + 8, // 运动速度（rad/s） + 5, // 扭矩限制（N⋅m） + 10000 // 超时帧数 )) .then( std::make_shared( - "belt_hold_torque", // 动作名称 - belt_command, // 传送带命令（输出） - belt_target_velocity, // 目标速度（输出） - belt_hold_torque, // 保持力矩（输出） - belt_wait_zero_velocity, // WAIT模式选择（输出） - belt_torque_offset, // 力矩偏移（输出） - 5.0, // 保持力矩值（N⋅m） + "belt_hold_torque", // 动作名称 + belt_command, // 传送带命令（输出） + belt_target_velocity, // 目标速度（输出） + belt_hold_torque, // 保持力矩（输出） + belt_wait_zero_velocity, // WAIT模式选择（输出） + belt_torque_offset, // 力矩偏移（输出） + 5.0, // 保持力矩值（N⋅m） 2000)); // 步骤3：传送带保持高扭矩 + 解锁扳机 + (可选)升降上行并行 diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index c72c4fe..ae71703 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -61,7 +61,7 @@ class LaunchPreparationTask : public Task { down_and_hold_ ->then( std::make_shared( - "belt_move_down_constant_velocity", // 动作名称 + "belt_slowly_down", // 动作名称 belt_command, // 速度模式方向命令（输出） belt_target_velocity, // 目标速度（输出） belt_torque_offset, // 力矩偏移（输出） diff --git a/src/vision/dart_vision_core.cpp b/src/vision/dart_vision_core.cpp index a9168d0..626b7ec 100644 --- a/src/vision/dart_vision_core.cpp +++ b/src/vision/dart_vision_core.cpp @@ -1,6 +1,7 @@ #include "identifier.hpp" #include "image_recorder.hpp" #include "tracker.hpp" +#include "video_recorder.hpp" #include #include #include @@ -58,6 +59,25 @@ class DartVisionCore ? get_parameter("save_processed_image").as_bool() : false; + // Video recording parameters + enable_video_recording_ = has_parameter("enable_video_recording"); + video_save_directory_ = has_parameter("video_save_directory") + ? get_parameter("video_save_directory").as_string() + : "./saved_videos"; + video_codec_ = + has_parameter("video_codec") ? get_parameter("video_codec").as_string() : "H264"; + video_fps_ = + has_parameter("video_fps") ? static_cast(get_parameter("video_fps").as_int()) : 30; + video_rotation_minutes_ = + has_parameter("video_rotation_minutes") + ? static_cast(get_parameter("video_rotation_minutes").as_int()) + : 5; + record_raw_video_ = + has_parameter("record_raw_video") ? get_parameter("record_raw_video").as_bool() : true; + record_processed_video_ = has_parameter("record_processed_video") + ? get_parameter("record_processed_video").as_bool() + : true; + image_capture_ = std::make_unique(camera_profile_); identifier_.set_default_limit(lower_limit_default_, upper_limit_default_); @@ -70,6 +90,15 @@ class DartVisionCore } } + if (enable_video_recording_) { + video_recorder_ = std::make_unique(logger_); + if (!video_recorder_->Init( + video_save_directory_, video_codec_, video_fps_, video_rotation_minutes_)) { + enable_video_recording_ = false; + RCLCPP_ERROR(logger_, "Failed to initialize video recorder"); + } + } + register_output("/dart_guidance/camera/camera_image", camera_image_); register_output("/dart_guidance/camera/display_image", display_image_); register_output("/dart_guidance/camera/target_position", target_position_, PointT(-1, -1)); @@ -84,6 +113,19 @@ class DartVisionCore RCLCPP_INFO(logger_, "Image saving disabled"); } + if (enable_video_recording_) { + RCLCPP_INFO(logger_, "Video recording enabled:"); + RCLCPP_INFO(logger_, " Directory: %s", video_save_directory_.c_str()); + RCLCPP_INFO(logger_, " Codec: %s", video_codec_.c_str()); + RCLCPP_INFO(logger_, " FPS: %d", video_fps_); + RCLCPP_INFO(logger_, " Rotation: %d minutes", video_rotation_minutes_); + RCLCPP_INFO(logger_, " Record raw: %s", record_raw_video_ ? "true" : "false"); + RCLCPP_INFO( + logger_, " Record processed: %s", record_processed_video_ ? "true" : "false"); + } else { + RCLCPP_INFO(logger_, "Video recording disabled"); + } + camera_thread_ = std::thread(&DartVisionCore::camera_update, this); update_time_point_ = std::chrono::steady_clock::now(); } @@ -93,6 +135,9 @@ class DartVisionCore if (image_recorder_) { image_recorder_->stop(); } + if (video_recorder_) { + video_recorder_->stop(); + } if (camera_thread_.joinable()) { camera_thread_.join(); } @@ -135,6 +180,10 @@ class DartVisionCore RCLCPP_INFO(logger_, "Pushed raw image %d to save queue", saved_raw_counter); } + if (enable_video_recording_ && record_raw_video_ && video_recorder_) { + video_recorder_->push_frame(raw_image, "raw"); + } + cv::Mat published_raw = raw_image.clone(); int cx = published_raw.cols / 2; int cy = published_raw.rows / 2; @@ -167,6 +216,10 @@ class DartVisionCore last_save_time = now; } + if (enable_video_recording_ && record_processed_video_ && video_recorder_) { + video_recorder_->push_frame(display_image, "processed"); + } + if (frame_counter % 30 == 0) { RCLCPP_DEBUG(logger_, "Processed %d frames", frame_counter); } @@ -260,6 +313,16 @@ class DartVisionCore std::unique_ptr image_recorder_; + bool enable_video_recording_ = false; + std::string video_save_directory_ = "./saved_videos"; + std::string video_codec_ = "H264"; + int video_fps_ = 30; + int video_rotation_minutes_ = 5; + bool record_raw_video_ = true; + bool record_processed_video_ = true; + + std::unique_ptr video_recorder_; + std::thread camera_thread_; std::mutex camera_thread_mtx_; std::atomic is_running_{true}; diff --git a/src/vision/video_recorder.hpp b/src/vision/video_recorder.hpp new file mode 100644 index 0000000..6e52518 --- /dev/null +++ b/src/vision/video_recorder.hpp @@ -0,0 +1,237 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class VideoRecorder { +public: + explicit VideoRecorder(rclcpp::Logger logger) : logger_(std::move(logger)) {} + + ~VideoRecorder() { + stop(); + } + + bool Init(const std::string& save_directory, const std::string& codec, int fps, int rotation_minutes) { + save_directory_ = save_directory; + fps_ = fps; + rotation_minutes_ = rotation_minutes; + + // Parse codec + if (codec == "H264") { + fourcc_code_ = cv::VideoWriter::fourcc('H', '2', '6', '4'); + file_extension_ = ".mp4"; + } else if (codec == "MJPEG") { + fourcc_code_ = cv::VideoWriter::fourcc('M', 'J', 'P', 'G'); + file_extension_ = ".avi"; + } else { + RCLCPP_ERROR(logger_, "Unknown codec: %s, defaulting to H264", codec.c_str()); + fourcc_code_ = cv::VideoWriter::fourcc('H', '2', '6', '4'); + file_extension_ = ".mp4"; + } + + try { + std::filesystem::create_directories(save_directory_); + RCLCPP_INFO(logger_, "Created video save directory: %s", save_directory_.c_str()); + + if (!test_write_permission()) { + return false; + } + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Failed to create save directory %s: %s", save_directory_.c_str(), e.what()); + return false; + } + + is_running_ = true; + worker_thread_ = std::thread(&VideoRecorder::worker_loop, this); + return true; + } + + void push_frame(const cv::Mat& frame, const std::string& type) { + if (!is_running_) return; + + std::lock_guard lock(queue_mutex_); + if (job_queue_.size() < max_queue_size_) { + job_queue_.push({frame.clone(), type, std::chrono::system_clock::now()}); + queue_cv_.notify_one(); + } else { + if (dropped_frames_++ % 30 == 0) { + RCLCPP_WARN(logger_, "Video frame queue is full, dropping frame (%s)", type.c_str()); + } + } + } + + void stop() { + if (is_running_) { + is_running_ = false; + queue_cv_.notify_all(); + if (worker_thread_.joinable()) { + worker_thread_.join(); + } + + // Release all video writers + for (auto& [type, writer] : video_writers_) { + if (writer.isOpened()) { + writer.release(); + RCLCPP_INFO(logger_, "Finalized video: %s", type.c_str()); + } + } + video_writers_.clear(); + video_start_times_.clear(); + } + } + +private: + struct RecordJob { + cv::Mat frame; + std::string type; + std::chrono::system_clock::time_point timestamp; + }; + + bool test_write_permission() { + std::string test_file = save_directory_ + "/test_write.txt"; + try { + std::ofstream ofs(test_file); + if (ofs.is_open()) { + ofs << "test"; + ofs.close(); + std::filesystem::remove(test_file); + return true; + } else { + RCLCPP_ERROR(logger_, "Write test failed: %s", test_file.c_str()); + return false; + } + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Write test exception: %s", e.what()); + return false; + } + } + + void worker_loop() { + while (is_running_ || !job_queue_.empty()) { + RecordJob job; + { + std::unique_lock lock(queue_mutex_); + queue_cv_.wait(lock, [this] { return !job_queue_.empty() || !is_running_; }); + + if (job_queue_.empty()) { + continue; + } + + job = std::move(job_queue_.front()); + job_queue_.pop(); + } + + process_frame(job); + } + } + + void process_frame(const RecordJob& job) { + try { + // Check if we need to rotate video + rotate_video_if_needed(job.type, job.timestamp); + + // Convert grayscale to BGR if needed (VideoWriter requires BGR) + cv::Mat frame_to_write; + if (job.frame.channels() == 1) { + cv::cvtColor(job.frame, frame_to_write, cv::COLOR_GRAY2BGR); + } else { + frame_to_write = job.frame; + } + + // Get or create video writer for this type + auto& writer = video_writers_[job.type]; + + // Initialize writer on first frame + if (!writer.isOpened()) { + std::string filename = generate_filename(job.type); + writer.open(filename, fourcc_code_, fps_, frame_to_write.size()); + + if (!writer.isOpened()) { + RCLCPP_ERROR(logger_, "Failed to open VideoWriter: %s", filename.c_str()); + return; + } + + video_start_times_[job.type] = job.timestamp; + RCLCPP_INFO(logger_, "Started recording video: %s (channels: %d)", + filename.c_str(), frame_to_write.channels()); + } + + // Write frame + writer.write(frame_to_write); + + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Error processing video frame: %s", e.what()); + } + } + + void rotate_video_if_needed(const std::string& type, const std::chrono::system_clock::time_point& current_time) { + auto it = video_start_times_.find(type); + if (it == video_start_times_.end()) { + return; // No video started yet + } + + auto elapsed = std::chrono::duration_cast(current_time - it->second); + if (elapsed.count() >= rotation_minutes_) { + // Close current video + auto& writer = video_writers_[type]; + if (writer.isOpened()) { + writer.release(); + RCLCPP_INFO(logger_, "Rotated video file for type: %s", type.c_str()); + } + // Next frame will create a new video file + video_start_times_.erase(it); + } + } + + std::string generate_filename(const std::string& type) { + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf; + localtime_r(&time_t, &tm_buf); + + std::ostringstream oss; + oss << save_directory_ << "/" << type << "_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") + << file_extension_; + + return oss.str(); + } + + rclcpp::Logger logger_; + std::string save_directory_; + int fourcc_code_; + int fps_; + int rotation_minutes_; + std::string file_extension_; + int dropped_frames_ = 0; + + // Video writers per type + std::map video_writers_; + std::map video_start_times_; + + // Threading + std::queue job_queue_; + std::mutex queue_mutex_; + std::condition_variable queue_cv_; + std::thread worker_thread_; + std::atomic is_running_{false}; + const size_t max_queue_size_ = 100; +}; + +} // namespace rmcs_dart_guidance From 494a50c95b5e2e5ca274e2e4ffae7cabb9952ed0 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 26 Apr 2026 02:35:08 +0800 Subject: [PATCH 18/19] refacted kalman filter --- plugins.xml | 8 +- .../action/force_screw_calibration_action.hpp | 79 ++++- src/manager/action/manual_force_control.hpp | 6 +- src/manager/action/vision_yaw_calibration.hpp | 36 ++ src/manager/dart_manager.cpp | 155 ++++++--- src/manager/task/cancel_launch_task.hpp | 8 +- src/manager/task/launch_preparation_task.hpp | 47 ++- src/manager/task/silder_init_task.hpp | 18 +- src/vision/dart_vision_core.cpp | 11 +- test/force_sensor_recorder.cpp | 152 ++++++++ test/image_displayer.cpp | 329 ++++++++++++++++++ 11 files changed, 770 insertions(+), 79 deletions(-) create mode 100644 src/manager/action/vision_yaw_calibration.hpp create mode 100644 test/force_sensor_recorder.cpp create mode 100644 test/image_displayer.cpp diff --git a/plugins.xml b/plugins.xml index ceaa893..b28c0a6 100644 --- a/plugins.xml +++ b/plugins.xml @@ -11,6 +11,9 @@ test node. + + test node. + Dart task manager: schedules Actions and Tasks, manages state machine and error recovery. @@ -23,5 +26,8 @@ Dart task manager ForFillingTest: 4-shot group counting with lifting and limiting servo control. - + + Records force sensor dual-channel readings before fire command, with dual counting logic (total shots and round number). + + diff --git a/src/manager/action/force_screw_calibration_action.hpp b/src/manager/action/force_screw_calibration_action.hpp index 5dc93aa..2428f68 100644 --- a/src/manager/action/force_screw_calibration_action.hpp +++ b/src/manager/action/force_screw_calibration_action.hpp @@ -9,8 +9,12 @@ namespace rmcs_dart_guidance::manager { // ForceScrewCalibrationAction — 力矩闭环控制动作 // 通过PID控制force_screw_motor，使当前力闭环到目标力值 // 电机正转力增大，反转力减小 +// 支持两种力值输入源： +// 1. 原始传感器 (current_force_ch1/ch2) +// 2. 卡尔曼滤波后 (filtered_force, 由 ForceKalmanFilter 组件提供) class ForceScrewCalibrationAction : public IAction { public: + // Constructor for raw sensor input (legacy mode) // force_channel: 1 = ch1, 2 = ch2 ForceScrewCalibrationAction( std::string name, double& force_screw_velocity, const int& current_force_ch1, @@ -21,6 +25,8 @@ class ForceScrewCalibrationAction : public IAction { , force_screw_velocity_(force_screw_velocity) , current_force_ch1_(current_force_ch1) , current_force_ch2_(current_force_ch2) + , filtered_force_(nullptr) + , force_rate_(nullptr) , force_channel_(force_channel) , target_force_(target_force) , force_tolerance_(force_tolerance) @@ -29,7 +35,35 @@ class ForceScrewCalibrationAction : public IAction { , kp_(kp) , ki_(ki) , kd_(kd) - , max_velocity_(max_velocity) {} + , max_velocity_(max_velocity) + , use_filtered_input_(false) + , use_rate_feedforward_(false) + , rate_gain_(0.0) {} + + // Constructor for Kalman-filtered input (new mode) + ForceScrewCalibrationAction( + std::string name, double& force_screw_velocity, const double& filtered_force, + const double& force_rate, double target_force, double force_tolerance, + uint64_t settle_ticks, uint64_t timeout_ticks, double kp, double ki, double kd, + double max_velocity, bool use_rate_feedforward = false, double rate_gain = 0.0) + : IAction(std::move(name)) + , force_screw_velocity_(force_screw_velocity) + , current_force_ch1_(*(int*)nullptr) // dummy, won't be used + , current_force_ch2_(*(int*)nullptr) + , filtered_force_(&filtered_force) + , force_rate_(&force_rate) + , force_channel_(0) + , target_force_(target_force) + , force_tolerance_(force_tolerance) + , settle_ticks_(settle_ticks) + , timeout_ticks_(timeout_ticks) + , kp_(kp) + , ki_(ki) + , kd_(kd) + , max_velocity_(max_velocity) + , use_filtered_input_(true) + , use_rate_feedforward_(use_rate_feedforward) + , rate_gain_(rate_gain) {} void on_enter() override { integral_ = 0.0; @@ -42,28 +76,42 @@ class ForceScrewCalibrationAction : public IAction { return ActionStatus::SUCCESS; } - double current_force = - static_cast(force_channel_ == 2 ? current_force_ch2_ : current_force_ch1_); - double error = (target_force_ - current_force) * 5; // 增加响应速度 + // Get current force from either raw sensors or filtered input + double current_force; + if (use_filtered_input_) { + // Kalman-filtered force (already in Newtons) + current_force = *filtered_force_; + } else { + // Raw sensor (in grams, convert to Newtons) + current_force = + static_cast(force_channel_ == 2 ? current_force_ch2_ : current_force_ch1_) * + 0.00981; + } + + double error = (target_force_ - current_force); + if (!use_filtered_input_) { + error *= 5; // Legacy scaling for raw sensors + } integral_ += error; double derivative = error - last_error_; last_error_ = error; - // PID输出 + // PID output double output = kp_ * error + ki_ * integral_ + kd_ * derivative; - // 限幅 - if (output > max_velocity_) { - output = max_velocity_; - } else if (output < -max_velocity_) { - output = -max_velocity_; + // Optional rate feedforward (only for filtered input) + if (use_filtered_input_ && use_rate_feedforward_ && force_rate_ != nullptr) { + output -= rate_gain_ * (*force_rate_); } - // 输出到电机 + // Clamp output + output = std::clamp(output, -max_velocity_, max_velocity_); + + // Output to motor force_screw_velocity_ = output; - // 检查是否在容差范围内 + // Check if within tolerance if (std::abs(error) <= force_tolerance_) { ++settle_counter_; if (settle_counter_ >= settle_ticks_) { @@ -85,7 +133,9 @@ class ForceScrewCalibrationAction : public IAction { double& force_screw_velocity_; const int& current_force_ch1_; const int& current_force_ch2_; - int force_channel_; // 1 = ch1, 2 = ch2 + const double* filtered_force_; // nullptr if using raw sensors + const double* force_rate_; // nullptr if using raw sensors + int force_channel_; // 1 = ch1, 2 = ch2 (only for raw mode) double target_force_; double force_tolerance_; uint64_t settle_ticks_; @@ -94,6 +144,9 @@ class ForceScrewCalibrationAction : public IAction { double ki_; double kd_; double max_velocity_; + bool use_filtered_input_; // true = use filtered_force_, false = use raw sensors + bool use_rate_feedforward_; // true = use dF/dt feedforward (filtered mode only) + double rate_gain_; // feedforward gain double integral_{0.0}; double last_error_{0.0}; uint64_t settle_counter_{0}; diff --git a/src/manager/action/manual_force_control.hpp b/src/manager/action/manual_force_control.hpp index e69e119..ffe9ee7 100644 --- a/src/manager/action/manual_force_control.hpp +++ b/src/manager/action/manual_force_control.hpp @@ -29,11 +29,13 @@ class DartManualForceControlAction : public IAction { void on_enter() override { force_control_velocity_ = 0.0; stall_counter_ = 0; + if (manual_force_scale_ > max_transform_rate_) { + manual_force_scale_ = max_transform_rate_; + } } ActionStatus update() override { - const double target_velocity = - joystick_right_.x() * max_transform_rate_ * manual_force_scale_; + const double target_velocity = joystick_right_.x() * manual_force_scale_; force_control_velocity_ = target_velocity; if (std::abs(target_velocity) <= 1e-6 || force_screw_velocity_feedback_ == nullptr diff --git a/src/manager/action/vision_yaw_calibration.hpp b/src/manager/action/vision_yaw_calibration.hpp new file mode 100644 index 0000000..6f4e76c --- /dev/null +++ b/src/manager/action/vision_yaw_calibration.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "action.hpp" + +#include +#include + +namespace rmcs_dart_guidance::manager { + +class VisionYawCalibration : public IAction { +public: + VisionYawCalibration( + Eigen::Vector2d& yaw_pitch_angle, const Eigen::Vector2d& yaw_pitch_distance, + double yaw_angle_mapping_rate) + : IAction("vision_yaw_calibration") + , yaw_control_angle_(yaw_pitch_angle.x()) + , yaw_distance_(yaw_pitch_distance.x()) + , yaw_rate_(yaw_angle_mapping_rate) {} + + void on_enter() override {} + + ActionStatus update() override { + if (elapsed_ticks() > 5000) { + return ActionStatus::SUCCESS; + } + yaw_control_angle_ = yaw_distance_ * yaw_rate_; + return ActionStatus::RUNNING; + } + +private: + double& yaw_control_angle_; + double yaw_distance_; + double yaw_rate_; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index 0732f19..d4840e6 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -62,13 +62,20 @@ class DartManager register_input("/force_sensor/channel_1/weight", current_force_ch1_); register_input("/force_sensor/channel_2/weight", current_force_ch2_); - register_input("/dart/manager/command", remote_command_input_, false); - register_input("/dart/manager/web_command", web_command_input_, false); + // Kalman filter inputs (optional, for force calibration) + register_input("/dart/kalman/filtered_force", kalman_filtered_force_, false); + register_input("/dart/kalman/force_rate", kalman_force_rate_, false); - register_input("/remote/joystick/left", joystick_left_, false); - register_input("/remote/joystick/right", joystick_right_, false); - register_input("/dart_guidance/camera/target_position", target_position_input_, false); - register_input("/dart_guidance/tracker/tracking", target_tracking_input_, false); + // register_input("/dart_guidance/camera/target_position", target_position_); + + register_input("/dart/manager/command", remote_command_input_); + register_input("/dart/manager/web_command", web_command_input_); + + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/joystick/right", joystick_right_); + // register_input("/dart_guidance/tracker/tracking", target_tracking_input_); + // register_input("/dart_guidance/tracker/yaw_pitch_target_distance", + // target_position_input_); register_output( "/dart/manager/belt/command", belt_command_, rmcs_msgs::DartSliderStatus::WAIT); @@ -84,6 +91,7 @@ class DartManager register_output( "/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); + register_output("/pitch/control/angle", yaw_pitch_control_angle_, Eigen::Vector2d::Zero()); register_output("/force/control/velocity", force_control_velocity_, 0.0); register_output("/dart/manager/aim/ready", aim_ready_, false); register_output( @@ -102,8 +110,14 @@ class DartManager "/dart/manager/limiting/command", limiting_command_, rmcs_msgs::DartLimitingServoStatus::LOCK); + // 力传感器记录触发信号（给ForceSensorRecorder组件使用） + register_output("/dart/manager/fire_trigger", fire_trigger_, false); + + yaw_transform_rate_ = get_parameter("yaw_transform_rate").as_double(); manual_force_scale_ = get_parameter("manual_force_scale").as_double(); + enable_pixel_to_angle_ = get_parameter("enable_pixel_to_angle").as_bool(); + launch_prepare_enable_visual_assist_ = get_parameter("launch_prepare_enable_visual_assist").as_bool(); if (launch_prepare_enable_visual_assist_) { @@ -127,8 +141,20 @@ class DartManager force_channel_ = 1; } + // Kalman filter force calibration mode + use_kalman_force_ = get_parameter_or("use_kalman_force", false); + kalman_rate_feedforward_ = get_parameter_or("kalman_rate_feedforward", false); + kalman_rate_gain_ = get_parameter_or("kalman_rate_gain", 0.0); + + if (use_kalman_force_) { + RCLCPP_INFO( + get_logger(), "[DartManager] Kalman force mode enabled: rate_ff=%d, rate_gain=%.3f", + kalman_rate_feedforward_, kalman_rate_gain_); + } + state_pub_ = create_publisher("/dart/manager/state", 10); + temporary_flag_ = true; sync_current_dart_outputs(); clear_auto_aim_feedback(); submit_task(make_slider_init_task()); @@ -136,7 +162,13 @@ class DartManager } void update() override { - refresh_auto_aim_inputs(); + // 重置fire触发信号（单次脉冲） + *fire_trigger_ = false; + + if (temporary_flag_) { + submit_task(make_slider_init_task()); + } + temporary_flag_ = false; poll_command(); switch (state_) { @@ -147,6 +179,14 @@ class DartManager } private: + template + T get_parameter_or(const std::string& name, T default_value) { + if (has_parameter(name)) { + return get_parameter(name).get_value(); + } + return default_value; + } + void poll_command() { std::string cmd; @@ -272,9 +312,8 @@ class DartManager } } - // 处理 cancel_launch 任务完成 - 重置 fire_count + // 处理 cancel_launch 任务完成 if (completed_task_name == "cancel_launch") { - fire_count_ = 0; RCLCPP_INFO( logger_, "[DartManager] cancel_launch completed, fire_count reset to 0"); } @@ -326,16 +365,6 @@ class DartManager } } - void refresh_auto_aim_inputs() { - if (target_position_input_.ready()) { - current_target_position_ = *target_position_input_; - } else { - current_target_position_ = cv::Point2i(-1, -1); - } - - current_target_tracking_ = target_tracking_input_.ready() ? *target_tracking_input_ : false; - } - double get_numeric_parameter_or_throw(const std::string& name) const { if (!has_parameter(name)) { throw std::runtime_error("Missing required parameter: " + name); @@ -411,7 +440,6 @@ class DartManager aim_ready_confirm_ticks_ = has_parameter("aim_ready_confirm_ticks") ? get_uint_parameter_or_throw("aim_ready_confirm_ticks") : 5; - aim_timeout_ticks_ = get_uint_parameter_or_throw("aim_timeout_ticks"); aim_min_transform_rate_ = has_parameter("aim_min_transform_rate") ? get_numeric_parameter_or_throw("aim_min_transform_rate") : 0.0; @@ -496,10 +524,20 @@ class DartManager } std::shared_ptr make_slider_init_task() { + // 为视觉接口提供默认值，即使视觉模块未启动也能正常工作 + static Eigen::Vector2d default_target_position = Eigen::Vector2d::Zero(); + + // yaw_pitch_control_angle_ 是 OutputInterface，总是有效的 + // target_positionfire_count_input_ 是 InputInterface，需要检查是否 ready + const Eigen::Vector2d* target_position_ptr = + target_position_input_.ready() ? &(*target_position_input_) : &default_target_position; + + RCLCPP_INFO(logger_, "slider_init_task_GO"); return std::make_shared( *belt_command_, *belt_target_velocity_, *belt_torque_limit_, *belt_hold_torque_, *belt_wait_zero_velocity_, *left_belt_velocity_, *right_belt_velocity_, - *left_belt_torque_, *right_belt_torque_, *belt_zero_calibration_); + *left_belt_torque_, *right_belt_torque_, *belt_zero_calibration_, + &(*yaw_pitch_control_angle_), target_position_ptr, yaw_transform_rate_); } // 任务工厂 @@ -526,17 +564,34 @@ class DartManager logger_, "[DartManager] Belt params: down_distance=%.4f m, pulley_radius=%.4f m", belt_down_distance_, down_velocity); - // 记录当前力值（用于下次fire前的力矩闭环，根据 force_channel_ 选择通道） - bool force_ready = - (force_channel_ == 2) ? current_force_ch2_.ready() : current_force_ch1_.ready(); - if (force_ready) { - last_fire_force_ = static_cast( - force_channel_ == 2 ? *current_force_ch2_ : *current_force_ch1_); + // 读取力值用于力矩闭环（不在此处记录） + bool ch1_ready = current_force_ch1_.ready(); + bool ch2_ready = current_force_ch2_.ready(); + + int force_ch1_value = ch1_ready ? *current_force_ch1_ : 0; + int force_ch2_value = ch2_ready ? *current_force_ch2_ : 0; + + // 根据 force_channel_ 选择主控通道 + if (force_channel_ == 2 && ch2_ready) { + last_fire_force_ = static_cast(force_ch2_value); + } else if (force_channel_ == 1 && ch1_ready) { + last_fire_force_ = static_cast(force_ch1_value); + } + + // Prepare Kalman filter pointers (if enabled and ready) + const double* kalman_force_ptr = nullptr; + const double* kalman_rate_ptr = nullptr; + if (use_kalman_force_ && kalman_filtered_force_.ready() && kalman_force_rate_.ready()) { + kalman_force_ptr = &(*kalman_filtered_force_); + kalman_rate_ptr = &(*kalman_force_rate_); RCLCPP_INFO( - logger_, "[DartManager] Recorded force before fire: %.2f (ch%d)", - last_fire_force_, force_channel_); - } else { - RCLCPP_WARN(logger_, "[DartManager] Force sensor NOT READY, using last value"); + logger_, "[DartManager] Using Kalman force: F_filt=%.1fN, dF/dt=%.1fN/s", + *kalman_force_ptr, *kalman_rate_ptr); + } else if (use_kalman_force_) { + RCLCPP_WARN( + logger_, + "[DartManager] use_kalman_force=true but Kalman inputs not ready, falling back " + "to raw sensors"); } return std::make_shared( @@ -548,7 +603,8 @@ class DartManager *belt_zero_calibration_, *force_control_velocity_, *current_force_ch1_, *current_force_ch2_, force_channel_, last_fire_force_, enable_force_calibration_, force_tolerance_, force_settle_ticks_, force_timeout_ticks_, force_kp_, force_ki_, - force_kd_, is_first_shot); + force_kd_, is_first_shot, use_kalman_force_, kalman_force_ptr, kalman_rate_ptr, + kalman_rate_feedforward_, kalman_rate_gain_); } if (cmd == "unload" || cmd == "cancel_launch") { @@ -559,10 +615,14 @@ class DartManager *right_belt_angle_, *left_belt_velocity_, *right_belt_velocity_, *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, *lifting_right_vel_fb_, belt_down_distance_, *belt_zero_calibration_, - require_lifting_up); + *force_control_velocity_, require_lifting_up); } if (cmd == "fire") { + // 触发力传感器记录（由独立的ForceSensorRecorder组件处理） + *fire_trigger_ = true; + RCLCPP_INFO(logger_, "[DartManager] Fire command received, trigger signal sent"); + bool is_first_shot = (fire_count_ == 0); return std::make_shared( *trigger_lock_enable_, *lifting_command_, *lifting_left_vel_fb_, @@ -614,8 +674,8 @@ class DartManager InputInterface joystick_left_; InputInterface joystick_right_; - InputInterface target_position_input_; - InputInterface target_tracking_input_; + InputInterface target_position_input_; + // InputInterface target_tracking_input_; // 升降速度反馈（FillingLiftAction 堵转检测用） InputInterface lifting_left_vel_fb_; @@ -627,6 +687,10 @@ class DartManager InputInterface current_force_ch1_; InputInterface current_force_ch2_; + // Kalman filter inputs (optional) + InputInterface kalman_filtered_force_; + InputInterface kalman_force_rate_; + OutputInterface belt_command_; OutputInterface belt_target_velocity_; OutputInterface belt_torque_limit_; @@ -637,8 +701,10 @@ class DartManager OutputInterface belt_error_gain_; OutputInterface belt_use_decel_pid_; OutputInterface trigger_lock_enable_; + OutputInterface fire_trigger_; // 力传感器记录触发信号 OutputInterface yaw_pitch_control_velocity_; + OutputInterface yaw_pitch_control_angle_; OutputInterface force_control_velocity_; OutputInterface aim_ready_; OutputInterface aim_current_dart_index_; @@ -649,10 +715,14 @@ class DartManager OutputInterface limiting_command_; double max_transform_rate_{500.0}; + double yaw_transform_rate_; double manual_force_scale_{5.0}; double auto_aim_max_transform_rate_{500.0}; - double belt_down_distance_{0.0}; // m + double belt_down_distance_{0.0}; // m + + // 像素到角度映射参数 + bool enable_pixel_to_angle_{false}; // 力矩闭环参数 bool enable_force_calibration_{false}; @@ -664,6 +734,11 @@ class DartManager double force_kd_{0.01}; int force_channel_{1}; // 1 = ch1, 2 = ch2 + // Kalman filter force calibration + bool use_kalman_force_{false}; + bool kalman_rate_feedforward_{false}; + double kalman_rate_gain_{0.0}; + bool launch_prepare_enable_visual_assist_{false}; AutoAimFeedback auto_aim_feedback_; DartLaunchSequence dart_launch_sequence_; @@ -673,11 +748,7 @@ class DartManager double aim_yaw_gain_{0.0}; double aim_pitch_gain_{0.0}; uint64_t aim_ready_confirm_ticks_{5}; - uint64_t aim_timeout_ticks_{3000}; double aim_min_transform_rate_{0.0}; - cv::Point2i current_target_position_{-1, -1}; - bool current_target_tracking_{false}; - InputInterface remote_command_input_; InputInterface web_command_input_; std::string last_command_; @@ -692,6 +763,10 @@ class DartManager uint32_t fire_count_{0}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; double last_fire_force_{0.0}; // 上次fire前记录的力值 + + bool temporary_flag_ = true; + + InputInterface target_position_; }; } // namespace rmcs_dart_guidance::manager diff --git a/src/manager/task/cancel_launch_task.hpp b/src/manager/task/cancel_launch_task.hpp index bbfc0c9..d8a5544 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -31,9 +31,13 @@ class CancelLaunchTask : public Task { const double& left_belt_velocity, const double& right_belt_velocity, bool& trigger_lock_enable, rmcs_msgs::DartSliderStatus& lifting_command, const double& lifting_left_vel_fb, const double& lifting_right_vel_fb, - double belt_down_distance, bool& belt_zero_calibration, bool require_lifting_up = true) + double belt_down_distance, bool& belt_zero_calibration, + double& force_screw_control_velocity, bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { + // 立即停止力丝杆电机（直接设置速度为0） + force_screw_control_velocity = 0.0; + // 步骤1：传送带匀速下行到卸载位（使用速度控制+多圈角度反馈） then( std::make_shared( @@ -47,7 +51,7 @@ class CancelLaunchTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - +belt_down_distance, // 目标距离（m，正值=下行） + +belt_down_distance - 0.1, // 目标距离（m，正值=下行） 12, // 运动速度（rad/s） 10, // 扭矩限制（N⋅m） 10000)); // 超时帧数 diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index ae71703..1346395 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -34,7 +34,9 @@ class LaunchPreparationTask : public Task { const int& current_force_ch2, int force_channel, double target_force, bool enable_force_calibration, double force_tolerance, uint64_t force_settle_ticks, uint64_t force_timeout_ticks, double force_kp, double force_ki, double force_kd, - bool is_first_shot = false) + bool is_first_shot = false, bool use_kalman_force = false, + const double* kalman_filtered_force = nullptr, const double* kalman_force_rate = nullptr, + bool kalman_rate_feedforward = false, double kalman_rate_gain = 0.0) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） @@ -85,7 +87,7 @@ class LaunchPreparationTask : public Task { belt_wait_zero_velocity, // WAIT模式选择（输出） belt_torque_offset, // 力矩偏移（输出） 5.0, // 保持力矩值（N⋅m） - 2000)); + 1000)); if (require_lifting_down) { auto parallel_hold_lock_and_lift = @@ -135,10 +137,10 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -0.05, // 目标距离 - 10.0, // 快速（rad/s） + -0.20, // 目标距离 + 5.0, // 快速（rad/s） 5.0, // 扭矩限制（N⋅m） - 1000)); + 10000)); then( std::make_shared( @@ -152,10 +154,10 @@ class LaunchPreparationTask : public Task { right_belt_angle, // 右电机角度反馈（输入） left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） - -0.65, // 目标距离 + -0.50, // 目标距离 20.0, // 快速（rad/s） 3.0, // 扭矩限制（N⋅m） - 2000)); + 10000)); then( std::make_shared( @@ -170,23 +172,36 @@ class LaunchPreparationTask : public Task { left_belt_velocity, // 左电机速度反馈（输入） right_belt_velocity, // 右电机速度反馈（输入） -0.75, // 目标距离） - 12.0, // 快速（rad/s） - 0.5, // 扭矩限制（N⋅m） - 1000)); + 8.0, // 快速（rad/s） + 1.0, // 扭矩限制（N⋅m） + 10000)); then(std::make_shared("stabilize_wait", 50)); then(std::make_shared(belt_zero_calibration)); if (enable_force_calibration) { - double target_force_value = is_first_shot ? 16000.0 : target_force; + double target_force_value = is_first_shot ? 21800.0 : target_force; double tolerance_value = is_first_shot ? 0.5 : force_tolerance; - then( - std::make_shared( - "force_screw_calibration", force_screw_velocity, current_force_ch1, - current_force_ch2, force_channel, target_force_value, tolerance_value, - force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, 5.0)); + if (use_kalman_force && kalman_filtered_force != nullptr + && kalman_force_rate != nullptr) { + // Use Kalman-filtered force (already in Newtons) + then( + std::make_shared( + "force_calibration_kalman", force_screw_velocity, *kalman_filtered_force, + *kalman_force_rate, target_force_value, tolerance_value, force_settle_ticks, + force_timeout_ticks, force_kp, force_ki, force_kd, 5.0, + kalman_rate_feedforward, kalman_rate_gain)); + } else { + // Use raw force sensor readings (in grams) + then( + std::make_shared( + "force_calibration_raw", force_screw_velocity, current_force_ch1, + current_force_ch2, force_channel, target_force_value, tolerance_value, + force_settle_ticks, force_timeout_ticks, force_kp, force_ki, force_kd, + 5.0)); + } } } }; diff --git a/src/manager/task/silder_init_task.hpp b/src/manager/task/silder_init_task.hpp index b04fd2a..db185c7 100644 --- a/src/manager/task/silder_init_task.hpp +++ b/src/manager/task/silder_init_task.hpp @@ -3,9 +3,13 @@ #include "manager/action/belt_move_action.hpp" #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" +#include "manager/action/vision_yaw_calibration.hpp" #include "manager/task/task.hpp" #include +#include "manager/action/filling_limit_servo_action.hpp" +#include + #include namespace rmcs_dart_guidance::manager { @@ -21,7 +25,8 @@ class SliderInitTask : public Task { double& belt_torque_limit, double& belt_hold_torque, bool& belt_wait_zero_velocity, const double& left_belt_velocity, const double& right_belt_velocity, const double& left_belt_torque, const double& right_belt_torque, - bool& belt_zero_calibration) + bool& belt_zero_calibration, Eigen::Vector2d* yaw_pitch_angle, + const Eigen::Vector2d* yaw_pitch_distance, double yaw_angle_mapping_rate) : Task("slider_init", "传送带上行复位并校准零点") { // 步骤1：同步带上行到机械限位（堵转检测） @@ -32,7 +37,7 @@ class SliderInitTask : public Task { belt_target_velocity, // 同步带目标速度（输出） belt_torque_limit, // 同步带力矩限制（输出） belt_hold_torque, // 同步带保持力矩（输出） - belt_wait_zero_velocity, // Wait 时使用零速闭环还是保留力矩 + belt_wait_zero_velocity, // Wait left_belt_velocity, // 左同步带反馈（输入） right_belt_velocity, // 右同步带反馈（输入） left_belt_torque, // 左同步带力矩（输入） @@ -52,9 +57,14 @@ class SliderInitTask : public Task { then(std::make_shared("stabilize_wait", 50)); then(std::make_shared(belt_zero_calibration)); - } -private: + // 视觉校准步骤（如果视觉未启动，使用默认值0也是安全的） + // if (yaw_pitch_angle && yaw_pitch_distance) { + // then( + // std::make_shared( + // *yaw_pitch_angle, *yaw_pitch_distance, yaw_angle_mapping_rate)); + // } + } }; } // namespace rmcs_dart_guidance::manager diff --git a/src/vision/dart_vision_core.cpp b/src/vision/dart_vision_core.cpp index 626b7ec..2502b9b 100644 --- a/src/vision/dart_vision_core.cpp +++ b/src/vision/dart_vision_core.cpp @@ -4,6 +4,7 @@ #include "video_recorder.hpp" #include #include +#include #include #include #include @@ -103,6 +104,9 @@ class DartVisionCore register_output("/dart_guidance/camera/display_image", display_image_); register_output("/dart_guidance/camera/target_position", target_position_, PointT(-1, -1)); register_output("/dart_guidance/tracker/tracking", tracking_); + register_output( + "/dart_guidance/tracker/yaw_pitch_target_distance", yaw_pitch_distance_, + Eigen::Vector2d::Zero()); if (enable_image_saving_) { RCLCPP_INFO(logger_, "Image saving enabled:"); RCLCPP_INFO(logger_, " Directory: %s", save_directory_.c_str()); @@ -257,7 +261,9 @@ class DartVisionCore is_tracker_stage_ = true; tracker_.Init(initial_position); - *target_position_ = initial_position; + // Output relative position (target - center) + cv::Point2i diff = *target_position_ - image_center_; + *yaw_pitch_distance_ = Eigen::Vector2d(diff.x, diff.y); *tracking_ = true; } } else { @@ -328,6 +334,7 @@ class DartVisionCore std::atomic is_running_{true}; cv::Scalar lower_limit_default_, upper_limit_default_; + cv::Point2i image_center_{701, 565}; hikcamera::ImageCapturer::CameraProfile camera_profile_; std::unique_ptr image_capture_; @@ -337,6 +344,8 @@ class DartVisionCore OutputInterface tracking_; OutputInterface target_position_; + OutputInterface yaw_pitch_distance_; + DartGuidanceIdentifier identifier_; DartGuidanceTracker tracker_; bool is_tracker_stage_ = false; diff --git a/test/force_sensor_recorder.cpp b/test/force_sensor_recorder.cpp new file mode 100644 index 0000000..5974c78 --- /dev/null +++ b/test/force_sensor_recorder.cpp @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace rmcs_dart_guidance::test { + +// ForceSensorRecorder +// 独立的力传感器数据记录组件，在fire命令前异步保存双通道力传感器读数 +// - 保留双重计数逻辑：总发射次数和当前轮次（1-4循环） +// - 异步写入，不阻塞控制循环 +class ForceSensorRecorder + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ForceSensorRecorder() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + + // 输入：力传感器双通道数据 + register_input("/force_sensor/channel_1/weight", force_ch1_); + register_input("/force_sensor/channel_2/weight", force_ch2_); + + // 输入：fire触发信号（由DartManager在fire命令时发出） + register_input("/dart/manager/fire_trigger", fire_trigger_); + + // 获取配置参数（使用相对路径，默认保存到当前工作目录） + output_file_path_ = get_parameter("output_file_path").as_string(); + dart_forebody_ = get_parameter("dart_forebody").as_string(); + + // 初始化文件（写入表头） + initialize_output_file(); + + RCLCPP_INFO( + logger_, "[ForceSensorRecorder] Initialized, output: %s, forebody: %s", + output_file_path_.c_str(), dart_forebody_.c_str()); + } + + ~ForceSensorRecorder() { + // 等待所有异步写入完成 + if (write_thread_.joinable()) { + write_thread_.join(); + } + } + + void update() override { + // 检测fire触发信号的上升沿 + if (fire_trigger_.ready()) { + bool current_trigger = *fire_trigger_; + if (current_trigger && !last_trigger_state_) { + // 上升沿：记录当前力传感器数据 + record_force_data(); + } + last_trigger_state_ = current_trigger; + } + } + +private: + void initialize_output_file() { + std::ofstream file(output_file_path_, std::ios::trunc); + if (!file.is_open()) { + RCLCPP_ERROR( + logger_, "[ForceSensorRecorder] Failed to create file: %s", + output_file_path_.c_str()); + return; + } + + file << "# 发射前力传感器数值记录\n\n"; + file << "**镖体构型**: " << dart_forebody_ << "\n\n"; + file << "| 总发射次数 | 当前轮次 | 通道1力值 | 通道2力值 |\n"; + file << "|-----------|---------|----------|----------|\n"; + file.close(); + + RCLCPP_INFO(logger_, "[ForceSensorRecorder] Output file initialized with header"); + } + + void record_force_data() { + // 读取当前力传感器数据 + int ch1_value = force_ch1_.ready() ? *force_ch1_ : 0; + int ch2_value = force_ch2_.ready() ? *force_ch2_ : 0; + + // 递增总发射次数 + total_shot_count_++; + + // 计算当前轮次 (1-4循环) + uint32_t round_number = ((total_shot_count_ - 1) % 4) + 1; + + RCLCPP_INFO( + logger_, "[ForceSensorRecorder] Recording shot #%u (round %u): ch1=%d, ch2=%d", + total_shot_count_, round_number, ch1_value, ch2_value); + + // 异步写入文件（避免阻塞控制循环） + async_write_to_file(total_shot_count_, round_number, ch1_value, ch2_value); + } + + void async_write_to_file( + uint32_t shot_number, uint32_t round_number, int ch1_value, int ch2_value) { + + // 如果上一个写入线程还在运行，等待它完成 + if (write_thread_.joinable()) { + write_thread_.join(); + } + + // 启动新的写入线程 + write_thread_ = std::thread([this, shot_number, round_number, ch1_value, ch2_value]() { + std::lock_guard lock(file_mutex_); + + std::ofstream file(output_file_path_, std::ios::app); + if (!file.is_open()) { + RCLCPP_WARN( + logger_, "[ForceSensorRecorder] Failed to open file for writing: %s", + output_file_path_.c_str()); + return; + } + + file << "| " << shot_number << " | " << round_number << " | " << ch1_value << " | " + << ch2_value << " |\n"; + file.close(); + }); + } + + rclcpp::Logger logger_; + + // 输入接口 + InputInterface force_ch1_; + InputInterface force_ch2_; + InputInterface fire_trigger_; + + // 配置参数 + std::string output_file_path_; + std::string dart_forebody_; + + // 状态变量 + uint32_t total_shot_count_{0}; // 总发射次数（持续递增） + bool last_trigger_state_{false}; // 上一次触发信号状态 + + // 异步写入 + std::mutex file_mutex_; + std::thread write_thread_; +}; + +} // namespace rmcs_dart_guidance::test + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::test::ForceSensorRecorder, rmcs_executor::Component) diff --git a/test/image_displayer.cpp b/test/image_displayer.cpp new file mode 100644 index 0000000..24c1d60 --- /dev/null +++ b/test/image_displayer.cpp @@ -0,0 +1,329 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class ImageDisplayer + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ImageDisplayer() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + // Store parameters in member variables for later use + raw_image_topic_ = get_parameter("raw_image_topic").as_string(); + processed_image_topic_ = get_parameter("processed_image_topic").as_string(); + target_topic_ = get_parameter("target_topic").as_string(); + + display_raw_ = get_parameter("display_raw").as_bool(); + display_processed_ = get_parameter("display_processed").as_bool(); + max_fps_ = static_cast(get_parameter("max_fps").as_int()); + window_scale_ = get_parameter("window_scale").as_double(); + save_on_error_ = get_parameter("save_on_error").as_bool(); + save_directory_ = get_parameter("save_directory").as_string(); + + // Check if display is available + const char* display_env = std::getenv("DISPLAY"); + if (!display_env || std::string(display_env).empty()) { + RCLCPP_WARN(logger_, "DISPLAY environment variable not set. Display windows disabled."); + display_available_ = false; + } else { + display_available_ = true; + } + + RCLCPP_INFO(logger_, "DebugDisplayComponent constructed"); + } + + ~ImageDisplayer() { stop_display(); } + + void update() override { + // Initialize subscriptions and display on first update + if (!initialized_) { + initialize(); + initialized_ = true; + } + } + +private: + void initialize() { + RCLCPP_INFO(logger_, "Initializing DebugDisplayComponent"); + + // Create subscriptions + if (display_raw_) { + raw_image_sub_ = this->create_subscription( + raw_image_topic_, 10, [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + raw_image_callback(msg); + }); + RCLCPP_INFO(logger_, "Subscribed to raw image topic: %s", raw_image_topic_.c_str()); + } + + if (display_processed_) { + processed_image_sub_ = this->create_subscription( + processed_image_topic_, 10, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + processed_image_callback(msg); + }); + RCLCPP_INFO( + logger_, "Subscribed to processed image topic: %s", processed_image_topic_.c_str()); + + target_sub_ = this->create_subscription( + target_topic_, 10, + [this](const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { + target_callback(msg); + }); + RCLCPP_INFO(logger_, "Subscribed to target topic: %s", target_topic_.c_str()); + } + + // Start display thread if available + if (display_available_) { + display_running_ = true; + display_thread_ = std::thread(&ImageDisplayer::display_loop, this); + RCLCPP_INFO(logger_, "Display thread started"); + } + } + + void stop_display() { + display_running_ = false; + if (display_thread_.joinable()) { + display_thread_.join(); + RCLCPP_INFO(logger_, "Display thread stopped"); + } + + // Clean up OpenCV windows + if (display_raw_) { + cv::destroyWindow("Raw Image"); + } + if (display_processed_) { + cv::destroyWindow("Processed Image"); + } + } + + void raw_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + std::lock_guard lock(raw_mutex_); + try { + raw_image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; + raw_image_timestamp_ = std::chrono::steady_clock::now(); + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(logger_, "cv_bridge exception for raw image: %s", e.what()); + } + } + + void processed_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + std::lock_guard lock(processed_mutex_); + try { + processed_image_ = cv_bridge::toCvCopy(msg, "mono8")->image; + processed_image_timestamp_ = std::chrono::steady_clock::now(); + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(logger_, "cv_bridge exception for processed image: %s", e.what()); + } + } + + void target_callback(const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { + std::lock_guard lock(target_mutex_); + target_position_.x = static_cast(msg->point.x); + target_position_.y = static_cast(msg->point.y); + target_tracking_ = (msg->point.z > 0.5); + last_target_time_ = std::chrono::steady_clock::now(); + } + + void display_loop() { + // Create display windows + if (display_raw_) { + cv::namedWindow("Raw Image", cv::WINDOW_NORMAL); + cv::resizeWindow( + "Raw Image", static_cast(640 * window_scale_), + static_cast(480 * window_scale_)); + } + + if (display_processed_) { + cv::namedWindow("Processed Image", cv::WINDOW_NORMAL); + cv::resizeWindow( + "Processed Image", static_cast(640 * window_scale_), + static_cast(480 * window_scale_)); + } + + const int frame_delay_ms = 1000 / std::max(1, max_fps_); + int frame_counter = 0; + auto last_fps_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO(logger_, "Display loop started with delay %d ms", frame_delay_ms); + + while (display_running_ && rclcpp::ok()) { + try { + // Display raw image + if (display_raw_) { + cv::Mat display_raw; + { + std::lock_guard lock(raw_mutex_); + if (!raw_image_.empty()) { + display_raw = raw_image_.clone(); + + // Check for stale data + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast( + now - raw_image_timestamp_) + .count(); + + if (elapsed > 500) { + cv::putText( + display_raw, "STALE", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); + } + } + } + + if (!display_raw.empty()) { + cv::imshow("Raw Image", display_raw); + } + } + + // Display processed image + if (display_processed_) { + cv::Mat display_processed; + { + std::lock_guard lock(processed_mutex_); + if (!processed_image_.empty()) { + display_processed = processed_image_.clone(); + + // Convert to color for visualization + if (display_processed.channels() == 1) { + cv::cvtColor( + display_processed, display_processed, cv::COLOR_GRAY2BGR); + } + + // Add target information + { + std::lock_guard lock(target_mutex_); + if (target_tracking_) { + cv::circle( + display_processed, target_position_, 20, + cv::Scalar(0, 255, 255), 2); + cv::putText( + display_processed, "TRACKING", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); + } + } + + // Check for stale data + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast( + now - processed_image_timestamp_) + .count(); + + if (elapsed > 500) { + cv::putText( + display_processed, "STALE", cv::Point(500, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); + } + } + } + + if (!display_processed.empty()) { + cv::imshow("Processed Image", display_processed); + } + } + + // Handle keyboard input + int key = cv::waitKey(frame_delay_ms) & 0xFF; + if (key == 27) { // ESC key + display_running_ = false; + RCLCPP_INFO(logger_, "ESC pressed, stopping display"); + } else if (key == 's' || key == 'S') { + save_current_frame(); + } + + frame_counter++; + + // Log FPS every 60 frames + if (frame_counter % 60 == 0) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = + std::chrono::duration_cast(now - last_fps_time) + .count(); + double fps = 60000.0 / static_cast(std::max(1L, elapsed)); + RCLCPP_DEBUG(logger_, "Display FPS: %.1f", fps); + last_fps_time = now; + } + + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Display error: %s", e.what()); + } + } + + RCLCPP_INFO(logger_, "Display loop ended"); + } + + void save_current_frame() { + std::lock_guard lock(processed_mutex_); + if (!processed_image_.empty()) { + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + char time_str[100]; + std::strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", std::localtime(&time_t)); + + std::string filename = save_directory_ + "/frame_" + time_str + ".jpg"; + cv::imwrite(filename, processed_image_); + RCLCPP_INFO(logger_, "Saved frame: %s", filename.c_str()); + } + } + +private: + rclcpp::Logger logger_; + + // Topic names + std::string raw_image_topic_; + std::string processed_image_topic_; + std::string target_topic_; + + // ROS2 subscribers + rclcpp::Subscription::SharedPtr raw_image_sub_; + rclcpp::Subscription::SharedPtr processed_image_sub_; + rclcpp::Subscription::SharedPtr target_sub_; + + // Image data + cv::Mat raw_image_; + cv::Mat processed_image_; + std::chrono::steady_clock::time_point raw_image_timestamp_; + std::chrono::steady_clock::time_point processed_image_timestamp_; + + // Target data + cv::Point target_position_; + bool target_tracking_ = false; + std::chrono::steady_clock::time_point last_target_time_; + + // Mutexes for thread safety + std::mutex raw_mutex_; + std::mutex processed_mutex_; + std::mutex target_mutex_; + + // Display thread + std::thread display_thread_; + std::atomic display_running_{true}; + std::atomic display_available_{true}; + std::atomic initialized_{false}; + + // Configuration parameters + bool display_raw_ = false; + bool display_processed_ = true; + int max_fps_ = 30; + double window_scale_ = 1.0; + bool save_on_error_ = false; + std::string save_directory_ = "./debug_saved"; +}; + +} // namespace rmcs_dart_guidance + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::ImageDisplayer, rmcs_executor::Component) \ No newline at end of file From 422a431d44bba23257302388ca1ced607b5bd4e2 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 26 Apr 2026 18:00:00 +0800 Subject: [PATCH 19/19] cleaned auto-aim residue --- src/manager/dart_manager.cpp | 147 +------------------ src/manager/task/fire_and_preload_task.hpp | 75 +++++----- src/manager/task/launch_preparation_task.hpp | 7 + src/manager/task/silder_init_task.hpp | 1 - 4 files changed, 53 insertions(+), 177 deletions(-) diff --git a/src/manager/dart_manager.cpp b/src/manager/dart_manager.cpp index d4840e6..6631cf9 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -1,8 +1,6 @@ #include "manager/action/action.hpp" #include "manager/action/manual_angle_control.hpp" #include "manager/action/manual_force_control.hpp" -#include "manager/auto_aim_feedback.hpp" -#include "manager/dart_launch_sequence.hpp" #include "manager/task/cancel_launch_task.hpp" #include "manager/task/fire_and_preload_task.hpp" #include "manager/task/launch_preparation_task.hpp" @@ -100,8 +98,6 @@ class DartManager register_output("/dart/manager/aim/error_px", aim_error_px_, Eigen::Vector2d::Zero()); register_output( "/dart/manager/aim/desired_target_px", aim_desired_target_px_, Eigen::Vector2d::Zero()); - auto_aim_feedback_.bind( - *yaw_pitch_control_velocity_, *aim_ready_, *aim_error_px_, *aim_desired_target_px_); // 升降指令总线 register_output( @@ -118,11 +114,6 @@ class DartManager enable_pixel_to_angle_ = get_parameter("enable_pixel_to_angle").as_bool(); - launch_prepare_enable_visual_assist_ = - get_parameter("launch_prepare_enable_visual_assist").as_bool(); - if (launch_prepare_enable_visual_assist_) { - load_auto_aim_configuration(); - } belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m enable_force_calibration_ = get_parameter("enable_force_calibration").as_bool(); @@ -154,9 +145,6 @@ class DartManager state_pub_ = create_publisher("/dart/manager/state", 10); - temporary_flag_ = true; - sync_current_dart_outputs(); - clear_auto_aim_feedback(); submit_task(make_slider_init_task()); RCLCPP_INFO(logger_, "[DartManager] initialized, queued SliderInitTask on startup"); } @@ -165,10 +153,6 @@ class DartManager // 重置fire触发信号（单次脉冲） *fire_trigger_ = false; - if (temporary_flag_) { - submit_task(make_slider_init_task()); - } - temporary_flag_ = false; poll_command(); switch (state_) { @@ -251,7 +235,6 @@ class DartManager *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); *force_control_velocity_ = 0.0; - clear_auto_aim_feedback(); transition_to(State::IDLE); } @@ -266,10 +249,6 @@ class DartManager first_fill_pending_ = true; *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; - if (launch_prepare_enable_visual_assist_) { - reset_dart_sequence(); - } - clear_auto_aim_feedback(); // 无论 ERROR 还是 IDLE，都重新排队传送带复位 submit_task(make_slider_init_task()); @@ -307,9 +286,6 @@ class DartManager if (completed_task_name == "fire") { fire_count_++; RCLCPP_INFO(logger_, "[DartManager] fire completed, fire_count=%u", fire_count_); - if (launch_prepare_enable_visual_assist_) { - advance_dart_sequence_after_fire(); - } } // 处理 cancel_launch 任务完成 @@ -339,7 +315,6 @@ class DartManager *limiting_command_ = rmcs_msgs::DartLimitingServoStatus::LOCK; *yaw_pitch_control_velocity_ = Eigen::Vector2d::Zero(); *force_control_velocity_ = 0.0; // 停止丝杆电机（已包含） - clear_auto_aim_feedback(); transition_to(State::ERROR); } @@ -416,111 +391,13 @@ class DartManager if (values.size() != 2) { throw std::runtime_error("Parameter must contain exactly 2 values: " + name); } - return Eigen::Vector2d(values[0], values[1]); - } - - void load_auto_aim_configuration() { - int64_t dart_count = 4; - if (has_parameter("dart_count")) { - dart_count = get_parameter("dart_count").as_int(); - } - - aim_deadband_px_ = has_parameter("aim_deadband_px") - ? get_vector2_parameter_or_throw("aim_deadband_px") - : Eigen::Vector2d::Constant(3.0); - aim_ready_exit_deadband_px_ = - has_parameter("aim_ready_exit_deadband_px") - ? get_vector2_parameter_or_throw("aim_ready_exit_deadband_px") - : Eigen::Vector2d::Constant(5.0); - aim_accept_deadband_px_ = has_parameter("aim_accept_deadband_px") - ? get_vector2_parameter_or_throw("aim_accept_deadband_px") - : aim_ready_exit_deadband_px_; - aim_yaw_gain_ = get_numeric_parameter_or_throw("aim_yaw_gain"); - aim_pitch_gain_ = get_numeric_parameter_or_throw("aim_pitch_gain"); - aim_ready_confirm_ticks_ = has_parameter("aim_ready_confirm_ticks") - ? get_uint_parameter_or_throw("aim_ready_confirm_ticks") - : 5; - aim_min_transform_rate_ = has_parameter("aim_min_transform_rate") - ? get_numeric_parameter_or_throw("aim_min_transform_rate") - : 0.0; - auto_aim_max_transform_rate_ = - has_parameter("auto_aim_max_transform_rate") - ? get_numeric_parameter_or_throw("auto_aim_max_transform_rate") - : max_transform_rate_; - - if (aim_deadband_px_.x() < 0.0 || aim_deadband_px_.y() < 0.0) { - throw std::runtime_error("Parameter aim_deadband_px must be non-negative"); - } - if (aim_ready_exit_deadband_px_.x() < aim_deadband_px_.x() - || aim_ready_exit_deadband_px_.y() < aim_deadband_px_.y()) { - throw std::runtime_error( - "Parameter aim_ready_exit_deadband_px must be >= aim_deadband_px"); - } - if (aim_accept_deadband_px_.x() < aim_ready_exit_deadband_px_.x() - || aim_accept_deadband_px_.y() < aim_ready_exit_deadband_px_.y()) { - throw std::runtime_error( - "Parameter aim_accept_deadband_px must be >= aim_ready_exit_deadband_px"); - } - if (aim_min_transform_rate_ < 0.0) { - throw std::runtime_error("Parameter aim_min_transform_rate must be non-negative"); - } - if (auto_aim_max_transform_rate_ < 0.0) { - throw std::runtime_error("Parameter auto_aim_max_transform_rate must be non-negative"); - } - if (aim_min_transform_rate_ > auto_aim_max_transform_rate_) { - throw std::runtime_error( - "Parameter aim_min_transform_rate must be <= auto_aim_max_transform_rate"); - } - - dart_launch_sequence_.configure_from_parameter_values( - DartLaunchSequenceRawConfig{ - .dart_count = dart_count, - .aim_reference_pixel = get_numeric_array_parameter_or_throw("aim_reference_pixel"), - .aim_dart_offsets_px = get_numeric_array_parameter_or_throw("aim_dart_offsets_px"), - }); - } - - Eigen::Vector2d current_desired_target_px() const { - return launch_prepare_enable_visual_assist_ - ? dart_launch_sequence_.current_desired_target_px() - : Eigen::Vector2d::Zero(); - } - - void sync_current_dart_outputs() { - *aim_current_dart_index_ = launch_prepare_enable_visual_assist_ - ? dart_launch_sequence_.current_dart_index_u8() - : static_cast(0); - auto_aim_feedback_.set_desired_target_px(current_desired_target_px()); - } - - void clear_auto_aim_feedback() { - *aim_current_dart_index_ = launch_prepare_enable_visual_assist_ - ? dart_launch_sequence_.current_dart_index_u8() - : static_cast(0); - auto_aim_feedback_.reset(current_desired_target_px()); - } - - void reset_dart_sequence() { - if (launch_prepare_enable_visual_assist_) { - dart_launch_sequence_.reset(); - } - sync_current_dart_outputs(); + return {values[0], values[1]}; } - void advance_dart_sequence_after_fire() { - if (!dart_launch_sequence_.advance_after_fire()) { - RCLCPP_WARN(logger_, "[DartManager] current dart index already at the last dart"); - } - sync_current_dart_outputs(); - clear_auto_aim_feedback(); - } - - void prepare_outputs_for_task(const Task& task) { + static void prepare_outputs_for_task(const Task& task) { if (task.name() == "launch_preparation" || task.name() == "fire" || task.name() == "cancel_launch" || task.name() == "slider_init" - || task.name() == "manual_angle" || task.name() == "manual_force") { - clear_auto_aim_feedback(); - } + || task.name() == "manual_angle" || task.name() == "manual_force") {} } std::shared_ptr make_slider_init_task() { @@ -633,9 +510,8 @@ class DartManager auto task = std::make_shared("manual_angle", "手动 yaw/pitch 调整"); task->then( std::make_shared( - auto_aim_feedback_.yaw_pitch_control_velocity()[0], - auto_aim_feedback_.yaw_pitch_control_velocity()[1], *joystick_left_, - *joystick_right_, max_transform_rate_)); + yaw_pitch_control_velocity_->x(), yaw_pitch_control_velocity_->y(), + *joystick_left_, *joystick_right_, max_transform_rate_)); return task; } @@ -717,7 +593,6 @@ class DartManager double max_transform_rate_{500.0}; double yaw_transform_rate_; double manual_force_scale_{5.0}; - double auto_aim_max_transform_rate_{500.0}; double belt_down_distance_{0.0}; // m @@ -739,16 +614,6 @@ class DartManager bool kalman_rate_feedforward_{false}; double kalman_rate_gain_{0.0}; - bool launch_prepare_enable_visual_assist_{false}; - AutoAimFeedback auto_aim_feedback_; - DartLaunchSequence dart_launch_sequence_; - Eigen::Vector2d aim_deadband_px_{Eigen::Vector2d::Constant(3.0)}; - Eigen::Vector2d aim_ready_exit_deadband_px_{Eigen::Vector2d::Constant(5.0)}; - Eigen::Vector2d aim_accept_deadband_px_{Eigen::Vector2d::Constant(8.0)}; - double aim_yaw_gain_{0.0}; - double aim_pitch_gain_{0.0}; - uint64_t aim_ready_confirm_ticks_{5}; - double aim_min_transform_rate_{0.0}; InputInterface remote_command_input_; InputInterface web_command_input_; std::string last_command_; @@ -764,8 +629,6 @@ class DartManager bool first_tick_of_task_{true}; double last_fire_force_{0.0}; // 上次fire前记录的力值 - bool temporary_flag_ = true; - InputInterface target_position_; }; diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index 575117c..aeaab82 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -24,43 +24,50 @@ class FireAndPreloadTask : public Task { rmcs_msgs::DartLimitingServoStatus& limiting_command, bool is_first_shot = false) : Task("fire", "发射并预装填") { + then( + std::make_shared( + limiting_command, // 限位舵机状态（输出） + rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 + rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 + )); + // 第一发特殊处理：只解锁扳机，无需升降和预装填 - if (is_first_shot) { - then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 500 // 等待释放完成帧数 - )); - return; - } else { - then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 500 // 等待释放完成帧数 - )); + // if (is_first_shot) { + // then( + // std::make_shared( + // trigger_lock_enable, // 扳机锁定使能（输出） + // false, // 解锁（false） + // 500 // 等待释放完成帧数 + // )); + // return; + // } else { + // then( + // std::make_shared( + // trigger_lock_enable, // 扳机锁定使能（输出） + // false, // 解锁（false） + // 500 // 等待释放完成帧数 + // )); - then( - std::make_shared( - "filling_lift_up", // 动作名称 - lifting_command, // 升降指令（输出） - rmcs_msgs::DartSliderStatus::UP, // 指令状态 - lifting_left_vel_fb, // 左升降电机速度反馈（输入） - lifting_right_vel_fb, // 右升降电机速度反馈（输入） - 0.5, // 堵转速度阈值 - 100, // 堵转确认帧数 - 500, // 最短运行帧数 - 5000 // 超时帧数 - )); + // then( + // std::make_shared( + // "filling_lift_up", // 动作名称 + // lifting_command, // 升降指令（输出） + // rmcs_msgs::DartSliderStatus::UP, // 指令状态 + // lifting_left_vel_fb, // 左升降电机速度反馈（输入） + // lifting_right_vel_fb, // 右升降电机速度反馈（输入） + // 0.5, // 堵转速度阈值 + // 100, // 堵转确认帧数 + // 500, // 最短运行帧数 + // 5000 // 超时帧数 + // )); - then( - std::make_shared( - limiting_command, // 限位舵机状态（输出） - rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 - rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 - )); - } + // then( + // std::make_shared( + // limiting_command, // 限位舵机状态（输出） + // rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 + // rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 + // )); + // } } }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index 1346395..540c32b 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -6,6 +6,7 @@ #include "manager/action/belt_zero_calibration.hpp" #include "manager/action/delay_action.hpp" #include "manager/action/filling_lift_action.hpp" +#include "manager/action/filling_limit_servo_action.hpp" #include "manager/action/force_screw_calibration_action.hpp" #include "manager/action/trigger_control_action.hpp" #include "manager/task/task.hpp" @@ -39,6 +40,12 @@ class LaunchPreparationTask : public Task { bool kalman_rate_feedforward = false, double kalman_rate_gain = 0.0) : Task("launch_preparation", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { + then( + std::make_shared( + limiting_command, // 限位舵机状态（输出） + rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 + rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 + )); // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） // 注意：target_distance为正值表示下行（角度增大方向） then( diff --git a/src/manager/task/silder_init_task.hpp b/src/manager/task/silder_init_task.hpp index db185c7..fe9c7f2 100644 --- a/src/manager/task/silder_init_task.hpp +++ b/src/manager/task/silder_init_task.hpp @@ -7,7 +7,6 @@ #include "manager/task/task.hpp" #include -#include "manager/action/filling_limit_servo_action.hpp" #include #include