diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e3b8e5..3c27b65 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,52 @@ 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 + ) + + 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 + manual_force_control_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/plugins.xml b/plugins.xml index 4ed63cb..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. @@ -20,11 +23,11 @@ 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. - + + Records force sensor dual-channel readings before fire command, with dual counting logic (total shots and round number). + + 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/auto_aim_action.hpp b/src/manager/action/auto_aim_action.hpp new file mode 100644 index 0000000..ea7ae59 --- /dev/null +++ b/src/manager/action/auto_aim_action.hpp @@ -0,0 +1,222 @@ +#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, 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_(std::move(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/belt_constant_velocity_move_action.hpp b/src/manager/action/belt_constant_velocity_move_action.hpp new file mode 100644 index 0000000..95e69db --- /dev/null +++ b/src/manager/action/belt_constant_velocity_move_action.hpp @@ -0,0 +1,133 @@ +#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_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 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) + , 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) + , right_belt_velocity_(right_belt_velocity) + , target_distance_(target_distance) + , velocity_(velocity) + , torque_limit_(torque_limit) + , timeout_ticks_(timeout_ticks) + , min_running_ticks_(min_running_ticks) {} + + void on_enter() override { + stall_counter_ = 0; + initial_angle_ = left_belt_angle_; + double target_angle_offset = + target_distance_ / 0.0195; // 将目标距离换算为角度偏移（rad），滑轮半径为0.0195m + target_position_ = initial_angle_ + target_angle_offset; + + 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_offset_ = torque_offset_value_; + belt_torque_limit_ = torque_limit_; + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_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; + + // 位置到达判断（优先级最高） + if (elapsed_ticks() > min_running_ticks_) { + bool within_tolerance = std::abs(position_error) < 0.01; + bool overshot = false; + + if (target_distance_ > 0) { + overshot = (position_error < 0); + } else { + overshot = (position_error > 0); + } + + if (within_tolerance || overshot) { + return ActionStatus::SUCCESS; + } + } + + if (elapsed_ticks() > 50) { + if (avg_velocity < 0.3) { + ++stall_counter_; + if (stall_counter_ >= 200) { + return ActionStatus::SUCCESS; + } + } else { + stall_counter_ = 0; + } + } + + 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 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 velocity_; // 运动速度（rad/s） + double torque_limit_; // 扭矩限制（N⋅m） + uint64_t timeout_ticks_; + uint64_t min_running_ticks_; + + 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..d651111 --- /dev/null +++ b/src/manager/action/belt_hold_torque_action.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include "action.hpp" + +#include +#include + +#include + +namespace rmcs_dart_guidance::manager { + +// BeltHoldTorqueAction - 保持传送带张力 +// 使用零速度闭环 + 常态扭矩偏移，用于在扳机锁定期间保持传送带张力 +// 与减速阶段一致，使用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& belt_torque_offset, + 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) + , belt_torque_offset_(belt_torque_offset) + , hold_torque_value_(hold_torque_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_ = false; // 使用 HOLD_TORQUE 模式（常数力矩） + } + + ActionStatus update() override { + if (elapsed_ticks() >= hold_duration_ticks_) { + return ActionStatus::SUCCESS; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + belt_hold_torque_ = 0.0; + 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_; + uint64_t hold_duration_ticks_; +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/manager/action/belt_move_action.hpp b/src/manager/action/belt_move_action.hpp index 5babf2f..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,15 @@ 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) , belt_target_velocity_(belt_target_velocity) @@ -47,7 +46,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 +59,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 +115,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/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/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/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/force_screw_calibration_action.hpp b/src/manager/action/force_screw_calibration_action.hpp new file mode 100644 index 0000000..2428f68 --- /dev/null +++ b/src/manager/action/force_screw_calibration_action.hpp @@ -0,0 +1,155 @@ +#pragma once + +#include "action.hpp" +#include +#include + +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, + 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_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) + , settle_ticks_(settle_ticks) + , timeout_ticks_(timeout_ticks) + , kp_(kp) + , ki_(ki) + , kd_(kd) + , 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; + last_error_ = 0.0; + settle_counter_ = 0; + } + + ActionStatus update() override { + if (elapsed_ticks() >= timeout_ticks_) { + return ActionStatus::SUCCESS; + } + + // 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 output + double output = kp_ * error + ki_ * integral_ + kd_ * derivative; + + // 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_) { + return ActionStatus::SUCCESS; + } + } else { + settle_counter_ = 0; + } + + return ActionStatus::RUNNING; + } + + void on_exit() override { + force_screw_velocity_ = 0.0; + integral_ = 0.0; + } + +private: + double& force_screw_velocity_; + const int& current_force_ch1_; + const int& current_force_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_; + uint64_t timeout_ticks_; + double kp_; + 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}; +}; + +} // namespace rmcs_dart_guidance::manager 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..ffe9ee7 100644 --- a/src/manager/action/manual_force_control.hpp +++ b/src/manager/action/manual_force_control.hpp @@ -2,41 +2,79 @@ #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& 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) : 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) {} - void on_enter() override {} + 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 { - force_control_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 + || force_screw_torque_feedback_ == nullptr) { + stall_counter_ = 0; + return ActionStatus::RUNNING; + } + + 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 < 0.15 && measured_torque > 0.5) { + ++stall_counter_; + if (stall_counter_ >= 100) { + 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_; + uint64_t stall_counter_{0}; }; } // namespace rmcs_dart_guidance::manager 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/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/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_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 603ad07..6631cf9 100644 --- a/src/manager/dart_manager.cpp +++ b/src/manager/dart_manager.cpp @@ -6,16 +6,19 @@ #include "manager/task/launch_preparation_task.hpp" #include "manager/task/silder_init_task.hpp" #include "manager/task/task.hpp" - #include #include #include +#include #include +#include #include +#include #include #include #include +#include #include #include #include @@ -25,94 +28,149 @@ 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: enum class State : uint8_t { - IDLE = 0, + IDLE = 0, RUNNING = 1, - ERROR = 2, + ERROR = 2, }; - DartManagerV2() + DartManager() : Node( get_component_name(), 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/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("/remote/joystick/left", joystick_left_, false); - register_input("/remote/joystick/right", joystick_right_, false); + 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/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_); + + // 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("/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); + 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); + 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("/pitch/control/velocity", yaw_pitch_control_velocity_, Eigen::Vector2d::Zero()); + 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( + "/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()); // 升降指令总线 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); - 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; + // 力传感器记录触发信号（给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(); + + belt_down_distance_ = get_parameter("belt_down_distance").as_double(); // m + + 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_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(); + + 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; } - limiting_fill_ticks_ = (uint64_t)get_parameter("limiting_fill_ticks").as_int(); + // 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); - 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(); + 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); 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 { + // 重置fire触发信号（单次脉冲） + *fire_trigger_ = false; + 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; } } 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; @@ -138,11 +196,11 @@ class DartManagerV2 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()); } } } @@ -150,7 +208,7 @@ class DartManagerV2 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()); } @@ -159,10 +217,20 @@ class DartManagerV2 if (current_task_) { current_task_->cancel(); current_task_.reset(); - RCLCPP_WARN(logger_, "[DartManagerV2] all tasks cancelled"); + RCLCPP_WARN(logger_, "[DartManager] all tasks cancelled"); } - enter_belt_wait_zero_velocity_mode(); + first_fill_pending_ = true; + fire_count_ = 0; // 重置开火次数 + + *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(); @@ -175,13 +243,16 @@ class DartManagerV2 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); } + + 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"); + RCLCPP_INFO(logger_, "[DartManager] queued SliderInitTask for recovery"); } void dispatch_next_task() { @@ -190,9 +261,9 @@ 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()); + RCLCPP_INFO(logger_, "[DartManager] dispatching task: '%s'", current_task_->name().c_str()); transition_to(State::RUNNING); tick_current_task(); @@ -207,15 +278,28 @@ 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()); + RCLCPP_INFO(logger_, "[DartManager] task '%s' SUCCESS", completed_task_name.c_str()); + + // 处理 fire 任务完成 + if (completed_task_name == "fire") { + fire_count_++; + RCLCPP_INFO(logger_, "[DartManager] fire completed, fire_count=%u", fire_count_); + } + + // 处理 cancel_launch 任务完成 + if (completed_task_name == "cancel_launch") { + RCLCPP_INFO( + logger_, "[DartManager] cancel_launch completed, fire_count reset to 0"); + } + current_task_.reset(); transition_to(State::IDLE); } 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(); } @@ -230,7 +314,7 @@ class DartManagerV2 *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; // 停止丝杆电机（已包含） transition_to(State::ERROR); } @@ -239,11 +323,14 @@ class DartManagerV2 *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) { - state_ = new_state; + state_ = new_state; first_tick_of_task_ = true; if (state_pub_) { @@ -253,69 +340,200 @@ class DartManagerV2 } } + 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 {values[0], values[1]}; + } + + 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") {} + } + 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_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_, + &(*yaw_pitch_control_angle_), target_position_ptr, yaw_transform_rate_); } // 任务工厂 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) ? 12.0 : 10.0; + 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, 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_, + *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", + belt_down_distance_, down_velocity); + + // 读取力值用于力矩闭环（不在此处记录） + 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] 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( *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_, *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_, *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, use_kalman_force_, kalman_force_ptr, kalman_rate_ptr, + kalman_rate_feedforward_, kalman_rate_gain_); } 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_, - *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_, *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_zero_calibration_, + *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_, *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_, *limiting_command_, is_first_shot); } 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_)); + task->then( + std::make_shared( + yaw_pitch_control_velocity_->x(), yaw_pitch_control_velocity_->y(), + *joystick_left_, *joystick_right_, max_transform_rate_)); return task; } 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_)); + task->then( + std::make_shared( + *force_control_velocity_, *joystick_right_, max_transform_rate_, + manual_force_scale_, force_screw_velocity_feedback, + force_screw_torque_feedback)); return task; } return nullptr; @@ -327,50 +545,94 @@ 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_; + InputInterface target_position_input_; + // InputInterface target_tracking_input_; // 升降速度反馈（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_; + 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_; OutputInterface belt_hold_torque_; - OutputInterface belt_wait_zero_velocity_; - OutputInterface trigger_lock_enable_; + 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 fire_trigger_; // 力传感器记录触发信号 OutputInterface yaw_pitch_control_velocity_; - OutputInterface force_control_velocity_; + OutputInterface yaw_pitch_control_angle_; + 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}; - uint64_t limiting_fill_ticks_{500}; + double max_transform_rate_{500.0}; + double yaw_transform_rate_; + double manual_force_scale_{5.0}; - 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_down_distance_{0.0}; // m + + // 像素到角度映射参数 + bool enable_pixel_to_angle_{false}; + + // 力矩闭环参数 + 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}; + 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}; 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}; // 当前轮次已完成发射数 bool first_tick_of_task_{true}; + double last_fire_force_{0.0}; // 上次fire前记录的力值 + + InputInterface target_position_; }; } // 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 122cbc9..d8a5544 100644 --- a/src/manager/task/cancel_launch_task.hpp +++ b/src/manager/task/cancel_launch_task.hpp @@ -1,12 +1,15 @@ #pragma once -#include "manager/action/belt_move_action.hpp" +#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_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 #include @@ -14,8 +17,8 @@ namespace rmcs_dart_guidance::manager { // CancelLaunchTask — 取消当前发射流程并回到安全待机位： -// 1. 同步带下行到卸载位 -// 2. 短暂等待机构稳定 +// 1. 同步带匀速下行到卸载位 +// 2. 斜坡减速到零 // 3. 解锁扳机 // 4. 同步带上行复位 // 5. 填装升降上行回到初始位 @@ -24,90 +27,142 @@ 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, 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) + 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& force_screw_control_velocity, bool require_lifting_up = true) : Task("cancel_launch", "取消发射") { - // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double down_velocity = 15.0; - double up_velocity = 10.0; - double hold_torque = 1.0; // Wait 时的保持力矩 + // 立即停止力丝杆电机（直接设置速度为0） + force_screw_control_velocity = 0.0; + // 步骤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, // 设定速度 - 5.0, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 10000, // 超时帧数 - 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_offset, // 力矩偏移（输出） + belt_torque_limit, // 扭矩限幅（输出） + 0.0, + left_belt_angle, // 左电机角度反馈（输入） + right_belt_angle, // 右电机角度反馈（输入） + left_belt_velocity, // 左电机速度反馈（输入） + right_belt_velocity, // 右电机速度反馈（输入） + +belt_down_distance - 0.1, // 目标距离（m，正值=下行） + 12, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） + 10000)); // 超时帧数 - then( - std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 - )); + auto down_and_hold_ = std::make_shared("down_and_hold"); + down_and_hold_ + ->then( + std::make_shared( + "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） + 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(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( + "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); then( - std::make_shared( - trigger_lock_enable, // 扳机锁定使能（输出） - false, // 解锁（false） - 1000 // 等待释放完成帧数 - )); + 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, // 右电机速度反馈（输入） + -0.01, // 目标距离 + 15.0, // 快速（rad/s） + 5.0, // 扭矩限制（N⋅m） + 10000)); 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, // 设定速度 - 1.0, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 10000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); + 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, // 右电机速度反馈（输入） + -0.65, // 目标距离 + 20.0, // 快速（rad/s） + 3.0, // 扭矩限制（N⋅m） + 10000)); 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 // 超时帧数 - )); + 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, // 右电机速度反馈（输入） + -0.75, // 目标距离） + 15.0, // 快速（rad/s） + 0.8, // 扭矩限制（N⋅m） + 10000)); + + then(std::make_shared("stabilize_wait", 50)); + + then(std::make_shared(belt_zero_calibration)); } }; diff --git a/src/manager/task/fire_and_preload_task.hpp b/src/manager/task/fire_and_preload_task.hpp index 1f45137..aeaab82 100644 --- a/src/manager/task/fire_and_preload_task.hpp +++ b/src/manager/task/fire_and_preload_task.hpp @@ -1,11 +1,10 @@ #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" -#include #include #include @@ -22,38 +21,53 @@ 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) + rmcs_msgs::DartLimitingServoStatus& limiting_command, 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, // 限位舵机状态（输出） + limiting_command, // 限位舵机状态（输出） rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 - rmcs_msgs::DartLimitingServoStatus::LOCK, // 再锁回 - preload_fill_ticks // 预装填持续帧数 + 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 // 等待释放完成帧数 + // )); + + // 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 // 再锁回 + // )); + // } } }; diff --git a/src/manager/task/launch_preparation_task.hpp b/src/manager/task/launch_preparation_task.hpp index ccebd02..540c32b 100644 --- a/src/manager/task/launch_preparation_task.hpp +++ b/src/manager/task/launch_preparation_task.hpp @@ -1,115 +1,215 @@ #pragma once #include "manager/action/action_set.hpp" -#include "manager/action/belt_move_action.hpp" +#include "manager/action/belt_constant_velocity_move_action.hpp" +#include "manager/action/belt_hold_torque_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/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" +#include #include #include +#include #include namespace rmcs_dart_guidance::manager { - -// LaunchPreparationTask — 将机构切换到发射准备态： -// 1. 同步带下行到装填位 -// 2. 短暂等待机构稳定 -// 3. 扳机锁定与填装升降下行并行执行 -// 4. 同步带上行复位 +// LaunchPreparationTask — 每次发射前的准备动作（传送带下行+扳机锁定） +// 速度和扭矩限制由 manager 传入，不再硬编码 class LaunchPreparationTask : public Task { public: 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, 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) - : Task("launch_preparation", "滑块发射准备") { - - // 在任务内部定义相关物理参数，避免从外部传参，让结构更整洁 - double torque_limit = 5.0; - double hold_torque = 1.0; // Wait 时的保持力矩 + 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& 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 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", "发射准备（传送带下行 + 扳机锁定 + 上行复位）") { 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, // 指令状态 - 10.0, // 设定速度 - torque_limit, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 10000, // 超时帧数 - 1.0, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 100, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_HOLD_TORQUE)); - + std::make_shared( + limiting_command, // 限位舵机状态（输出） + rmcs_msgs::DartLimitingServoStatus::FREE, // 先释放 + rmcs_msgs::DartLimitingServoStatus::LOCK // 再锁回 + )); + // 步骤1：传送带匀速下行到目标位置（使用速度控制+多圈角度反馈） + // 注意：target_distance为正值表示下行（角度增大方向） then( - std::make_shared( - "belt_wait", // 动作名称 - 50 // 等待帧数 + 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 - 0.1, // 目标距离（m，正值=下行） + down_velocity, // 运动速度（rad/s） + 10, // 扭矩限制（N⋅m） + 10000 // 超时帧数 )); - auto parallel_prepare = - std::make_shared("parallel_prepare", ActionSet::Policy::ALL_SUCCESS); - parallel_prepare - ->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, // 右升降电机速度反馈（输入） - lifting_stall_threshold, // 堵转速度阈值 - lifting_stall_confirm_ticks, // 堵转确认帧数 - lifting_stall_min_run_ticks, // 最短运行帧数 - lifting_stall_timeout_ticks // 超时帧数 - )); - then(parallel_prepare); + auto down_and_hold_ = std::make_shared("down_and_hold"); + down_and_hold_ + ->then( + std::make_shared( + "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） + 1000)); + + if (require_lifting_down) { + auto parallel_hold_lock_and_lift = + std::make_shared("hold_lock_and_lift", ActionSet::Policy::ALL_SUCCESS); + + parallel_hold_lock_and_lift->also(down_and_hold_) + .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, // 最短运行帧数 + 1000)); // 超时帧数 + then(parallel_hold_lock_and_lift); + } else { + // 步骤3（首发）：传送带保持高扭矩 + 扳机锁定并行 + auto parallel_hold_and_lock = + std::make_shared("hold_and_lock", ActionSet::Policy::ALL_SUCCESS); + + parallel_hold_and_lock->also(down_and_hold_) + .also( + std::make_shared( + trigger_lock_enable, // 扳机锁定使能（输出） + true, // 锁定（true） + 500)); // 等待锁定完成帧数 + then(parallel_hold_and_lock); + } + + then( + 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, // 右电机速度反馈（输入） + -0.20, // 目标距离 + 5.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, // 右电机速度反馈（输入） + -0.50, // 目标距离 + 20.0, // 快速（rad/s） + 3.0, // 扭矩限制（N⋅m） + 10000)); 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, // 指令状态 - 20.0, // 设定速度 - torque_limit, // 设定力矩限制 - hold_torque, // 设定保持力矩 - 5000, // 超时帧数 - 0.5, // 堵转速度阈值 - 0.5, // 堵转力矩阈值 - 200, // 堵转确认帧数 - 50, // 最短运行帧数 - BeltMoveAction::ExitMode::WAIT_ZERO_VELOCITY)); + 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, // 右电机速度反馈（输入） + -0.75, // 目标距离） + 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 ? 21800.0 : target_force; + double tolerance_value = is_first_shot ? 0.5 : force_tolerance; + + 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)); + } + } } }; - -} // namespace rmcs_dart_guidance::manager +} // namespace rmcs_dart_guidance::manager \ No newline at end of file diff --git a/src/manager/task/silder_init_task.hpp b/src/manager/task/silder_init_task.hpp index ed33192..fe9c7f2 100644 --- a/src/manager/task/silder_init_task.hpp +++ b/src/manager/task/silder_init_task.hpp @@ -1,51 +1,68 @@ #pragma once #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 + #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, Eigen::Vector2d* yaw_pitch_angle, + const Eigen::Vector2d* yaw_pitch_distance, double yaw_angle_mapping_rate) + : 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, // 设定保持力矩 + 3000, // 超时帧数（可配置） + 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)); + + // 视觉校准步骤（如果视觉未启动，使用默认值0也是安全的） + // if (yaw_pitch_angle && yaw_pitch_distance) { + // then( + // std::make_shared( + // *yaw_pitch_angle, *yaw_pitch_distance, yaw_angle_mapping_rate)); + // } } }; 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..3b5ace9 --- /dev/null +++ b/src/manager/task/vision_assisted_launch_preparation_task.hpp @@ -0,0 +1,65 @@ +#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, + 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, 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, 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( + 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, 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)); + } +}; + +} // namespace rmcs_dart_guidance::manager diff --git a/src/vision/dart_vision_core.cpp b/src/vision/dart_vision_core.cpp index a9168d0..2502b9b 100644 --- a/src/vision/dart_vision_core.cpp +++ b/src/vision/dart_vision_core.cpp @@ -1,8 +1,10 @@ #include "identifier.hpp" #include "image_recorder.hpp" #include "tracker.hpp" +#include "video_recorder.hpp" #include #include +#include #include #include #include @@ -58,6 +60,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,10 +91,22 @@ 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)); 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()); @@ -84,6 +117,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 +139,9 @@ class DartVisionCore if (image_recorder_) { image_recorder_->stop(); } + if (video_recorder_) { + video_recorder_->stop(); + } if (camera_thread_.joinable()) { camera_thread_.join(); } @@ -135,6 +184,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 +220,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); } @@ -204,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 { @@ -260,11 +319,22 @@ 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}; cv::Scalar lower_limit_default_, upper_limit_default_; + cv::Point2i image_center_{701, 565}; hikcamera::ImageCapturer::CameraProfile camera_profile_; std::unique_ptr image_capture_; @@ -274,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/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 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 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..3713169 --- /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/manual_force_control_test.cpp b/tests/manual_force_control_test.cpp new file mode 100644 index 0000000..70ef4e8 --- /dev/null +++ b/tests/manual_force_control_test.cpp @@ -0,0 +1,63 @@ +#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); + + 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); + + 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 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..516540b --- /dev/null +++ b/tests/vision_assisted_launch_preparation_task_test.cpp @@ -0,0 +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(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