diff --git a/CMakeLists.txt b/CMakeLists.txt index f31ff95..f725597 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ target_sources( PRIVATE src/cxx/context.cc src/cxx/navigation.cc + src/cxx/navigation_gimbal_controller.cc src/cxx/component.cc ) target_include_directories( diff --git a/src/cxx/component.cc b/src/cxx/component.cc index 81fbdb0..43ba947 100644 --- a/src/cxx/component.cc +++ b/src/cxx/component.cc @@ -6,8 +6,10 @@ #include "cxx/context.hh" #include "cxx/navigation.hh" +#include "cxx/navigation_gimbal_controller.hh" #include "cxx/util/node_mixin.hh" #include +#include #include #include @@ -32,6 +34,9 @@ class Navigation bool mock_context = false; + bool chassis_vel_override_active_ = false; + Eigen::Vector2d chassis_vel_override_ = Eigen::Vector2d::Zero(); + std::atomic lua_tick_count = 0; std::unique_ptr lua; sol::table lua_blackboard; @@ -42,19 +47,28 @@ class Navigation details::Context context; details::Navigation navigation; + details::NavigationGimbalController gimbal_controller; struct Command { OutputInterface chassis_speed; + OutputInterface chassis_mode; + OutputInterface enable_local_obstacle; + OutputInterface base_link_yaw; OutputInterface nod_count; - OutputInterface rotate_chassis; OutputInterface detect_targets; OutputInterface enable_autoaim; auto init(Navigation& component) -> void { component.register_output( "/rmcs_navigation/chassis_velocity", chassis_speed, Eigen::Vector2d::Zero()); + component.register_output( + "/rmcs_navigation/chassis_mode", chassis_mode, rmcs_msgs::ChassisMode::AUTO); + component.register_output( + "/rmcs_navigation/enable_local_obstacle", enable_local_obstacle, true); + component.register_output( + "/rmcs_navigation/base_link_yaw", base_link_yaw, + std::numeric_limits::quiet_NaN()); component.register_output("/rmcs_navigation/nod_count", nod_count, 0); - component.register_output("/rmcs_navigation/rotate_chassis", rotate_chassis, false); component.register_output("/rmcs_navigation/detect_targets", detect_targets, false); component.register_output("/rmcs_navigation/start_autoaim", enable_autoaim, false); } @@ -93,14 +107,39 @@ class Navigation navigation.switch_topic_forward(enable); }); api.set_function("update_gimbal_direction", [this](double angle) { - warn("unimplement: update_gimbal_direction({})", angle); + gimbal_controller.set_lua_target_yaw(angle); }); api.set_function("update_chassis_mode", [this](const std::string& mode) { - warn("unimplement: update_chassis_mode(\"{}\")", mode); + if (mode == "auto") { + *command.chassis_mode = rmcs_msgs::ChassisMode::AUTO; + } else if (mode == "spin") { + *command.chassis_mode = rmcs_msgs::ChassisMode::SPIN; + } else if (mode == "step_down") { + *command.chassis_mode = rmcs_msgs::ChassisMode::STEP_DOWN; + } else if (mode == "launch_ramp") { + *command.chassis_mode = rmcs_msgs::ChassisMode::LAUNCH_RAMP; + } else { + warn("unknown chassis mode \"{}\", fallback to AUTO", mode); + *command.chassis_mode = rmcs_msgs::ChassisMode::AUTO; + } }); api.set_function("update_chassis_vel", [this](double x, double y) { + if (chassis_vel_override_active_) + return; *command.chassis_speed = Eigen::Vector2d{x, y}; }); + api.set_function( + "set_local_obstacle", [this](bool enable) { *command.enable_local_obstacle = enable; }); + api.set_function("cancel_target", [this]() { navigation.cancel_target(); }); + api.set_function("set_chassis_vel_override", [this](double vx, double vy) { + chassis_vel_override_ = Eigen::Vector2d{vx, vy}; + chassis_vel_override_active_ = true; + *command.chassis_speed = chassis_vel_override_; + }); + api.set_function("clear_chassis_vel_override", [this]() { + chassis_vel_override_active_ = false; + *command.chassis_speed = Eigen::Vector2d::Zero(); + }); } auto make_option_injection() { @@ -149,22 +188,43 @@ class Navigation auto lua_sync() { const auto [x, y, yaw] = navigation.check_position(); - auto user = lua_blackboard["user"].get(); - user["health"] = *context.robot_health; - user["bullet"] = *context.robot_bullet; - user["chassis_power_limit"] = *context.chassis_power_limit_referee; + if (!lua_blackboard.valid() || lua_blackboard.get_type() != sol::type::table) { + warn("lua blackboard is invalid, skip sync"); + return; + } + + auto user_object = lua_blackboard["user"]; + auto game_object = lua_blackboard["game"]; + auto play_object = lua_blackboard["play"]; + auto meta_object = lua_blackboard["meta"]; + if (user_object.get_type() != sol::type::table || game_object.get_type() != sol::type::table + || play_object.get_type() != sol::type::table + || meta_object.get_type() != sol::type::table) { + warn("lua blackboard schema is invalid, skip sync"); + return; + } + + auto user = user_object.get(); + user["health"] = context.robot_health.ready() ? *context.robot_health : std::uint16_t{0}; + user["bullet"] = context.robot_bullet.ready() ? *context.robot_bullet : std::uint16_t{0}; + user["chassis_power_limit"] = context.chassis_power_limit_referee.ready() + ? *context.chassis_power_limit_referee + : 0.0; user["x"] = x; user["y"] = y; user["yaw"] = yaw; - auto game = lua_blackboard["game"].get(); - game["stage"] = rmcs_msgs::to_string(*context.game_stage); + auto game = game_object.get(); + game["stage"] = + context.game_stage.ready() ? rmcs_msgs::to_string(*context.game_stage) : "UNKNOWN"; - auto play = lua_blackboard["play"].get(); - play["rswitch"] = rmcs_msgs::to_string(*context.switch_right); - play["lswitch"] = rmcs_msgs::to_string(*context.switch_left); + auto play = play_object.get(); + play["rswitch"] = + context.switch_right.ready() ? rmcs_msgs::to_string(*context.switch_right) : "UNKNOWN"; + play["lswitch"] = + context.switch_left.ready() ? rmcs_msgs::to_string(*context.switch_left) : "UNKNOWN"; - auto meta = lua_blackboard["meta"].get(); + auto meta = meta_object.get(); meta["timestamp"] = this->now().seconds(); } @@ -197,6 +257,10 @@ class Navigation lua_on_init = (*lua)["on_init"]; lua_on_tick = (*lua)["on_tick"]; + if (!lua_blackboard.valid() || lua_blackboard.get_type() != sol::type::table) { + throw std::runtime_error("lua endpoint must define blackboard table"); + } + const auto situation = std::array{ lua_on_init.valid(), lua_on_tick.valid(), @@ -228,12 +292,16 @@ class Navigation explicit Navigation() : rclcpp::Node{get_component_name(), option()} , context{*this, *this} - , navigation{*this} { + , navigation{*this} + , gimbal_controller{*this, navigation} { mock_context = param("mock_context"); + gimbal_controller.init_params(); + context.init(io_mutex, mock_context); command.init(*this); + gimbal_controller.init_output(*this); lua_init(); @@ -258,6 +326,8 @@ class Navigation lua_sync(); lua_tick(); } + + gimbal_controller.update(); } }; diff --git a/src/cxx/navigation.cc b/src/cxx/navigation.cc index 9dbf32a..818b305 100644 --- a/src/cxx/navigation.cc +++ b/src/cxx/navigation.cc @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include #include @@ -23,6 +25,25 @@ namespace rmcs::navigation::details { +namespace { + +auto parse_pose_yaw(const geometry_msgs::msg::Quaternion& rotation) -> double { + const auto sin_yaw = 2.0 * ((rotation.w * rotation.z) + (rotation.x * rotation.y)); + const auto cos_yaw = 1.0 - (2.0 * ((rotation.y * rotation.y) + (rotation.z * rotation.z))); + return std::atan2(sin_yaw, cos_yaw); +} + +auto make_yaw_quaternion(double yaw) -> geometry_msgs::msg::Quaternion { + auto orientation = geometry_msgs::msg::Quaternion{}; + orientation.x = 0.0; + orientation.y = 0.0; + orientation.z = std::sin(yaw * 0.5); + orientation.w = std::cos(yaw * 0.5); + return orientation; +} + +} // namespace + struct Navigation::Impl : rmcs::navigation::NodeMixin { using GoalAction = nav2_msgs::action::NavigateToPose; @@ -36,6 +57,7 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { using TfListener = tf2_ros::TransformListener; static constexpr auto kPositionEpsilon = 1e-2; + static constexpr auto kYawEpsilon = 1e-2; static constexpr auto kServerWaitTimeout = std::chrono::seconds{1}; static constexpr auto kNavigateToPoseActionName = "/navigate_to_pose"; static constexpr auto kWorldFrame = "world"; @@ -46,14 +68,26 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { struct Target final { double x = std::numeric_limits::quiet_NaN(); double y = std::numeric_limits::quiet_NaN(); + double yaw = std::numeric_limits::quiet_NaN(); + + static auto same_yaw(double lhs, double rhs) noexcept -> bool { + const auto lhs_finite = std::isfinite(lhs); + const auto rhs_finite = std::isfinite(rhs); + if (!lhs_finite || !rhs_finite) + return lhs_finite == rhs_finite; + + return std::abs(std::remainder(lhs - rhs, 2 * std::numbers::pi_v)) + <= Impl::kYawEpsilon; + } auto operator==(const Target& rhs) const noexcept -> bool { return std::abs(x - rhs.x) <= Impl::kPositionEpsilon - && std::abs(y - rhs.y) <= Impl::kPositionEpsilon; + && std::abs(y - rhs.y) <= Impl::kPositionEpsilon && same_yaw(yaw, rhs.yaw); } }; rclcpp::Node& node; + mutable std::mutex goal_mutex; // TF Query std::shared_ptr tf_buffer = std::make_shared(node.get_clock()); @@ -85,9 +119,20 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { std::uint64_t latest_request_id = 0; auto has_same_goal(const Target& goal) const -> bool { + auto lock = std::scoped_lock{goal_mutex}; return active_goal.has_value() && *active_goal == goal; } + auto set_active_goal(const Target& goal) -> void { + auto lock = std::scoped_lock{goal_mutex}; + active_goal = goal; + } + + auto reset_active_goal() -> void { + auto lock = std::scoped_lock{goal_mutex}; + active_goal.reset(); + } + auto ensure_server_ready() -> bool { if (client->action_server_is_ready()) return true; @@ -103,6 +148,8 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { navigation_goal_message.pose.header.stamp = node.now(); navigation_goal_message.pose.pose.position.x = goal.x; navigation_goal_message.pose.pose.position.y = goal.y; + navigation_goal_message.pose.pose.orientation = + std::isfinite(goal.yaw) ? make_yaw_quaternion(goal.yaw) : make_yaw_quaternion(0.0); } auto log_result(const Target& goal, rclcpp_action::ResultCode code) const -> void { @@ -130,7 +177,7 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { [request_id, this, goal](const std::shared_ptr& handle) { if (!handle) { if (request_id == latest_request_id) - active_goal.reset(); + reset_active_goal(); warn("navigate_to_pose goal rejected: x={}, y={}", goal.x, goal.y); return; @@ -149,7 +196,7 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { if (request_id != latest_request_id) return; - active_goal.reset(); + reset_active_goal(); current_goal_handle.reset(); log_result(goal, result.code); }; @@ -165,8 +212,11 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { return; auto& position = message->pose.position; - send_target(position.x, position.y); - info("forward {} -> ({:.1}, {:.1})", topic, position.x, position.y); + auto yaw = parse_pose_yaw(message->pose.orientation); + send_target(position.x, position.y, yaw); + info( + "forward {} -> ({:.1}, {:.1}, yaw={:.2f}rad)", topic, position.x, position.y, + yaw); }); } @@ -176,7 +226,7 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { ~Impl() { auto current_handle = std::exchange(current_goal_handle, std::shared_ptr{}); - active_goal.reset(); + reset_active_goal(); ++latest_request_id; if (current_handle) @@ -186,7 +236,11 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { auto get_logger() const -> rclcpp::Logger { return node.get_logger(); } auto send_target(double x, double y) -> void { - auto goal = Target{.x = x, .y = y}; + send_target(x, y, std::numeric_limits::quiet_NaN()); + } + + auto send_target(double x, double y, double yaw) -> void { + auto goal = Target{.x = x, .y = y, .yaw = yaw}; if (has_same_goal(goal)) return; @@ -196,7 +250,7 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { if (has_same_goal(goal)) return; - active_goal = goal; + set_active_goal(goal); auto request_id = ++latest_request_id; auto cancel_handle = std::exchange(current_goal_handle, std::shared_ptr{}); @@ -206,6 +260,15 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { send_target(goal, request_id); } + auto cancel_target() -> void { + auto cancel_handle = std::exchange(current_goal_handle, std::shared_ptr{}); + reset_active_goal(); + ++latest_request_id; + + if (cancel_handle) + client->async_cancel_goal(cancel_handle); + } + auto switch_topic_forward(bool enable) -> void { if (topic_forward_enabled == enable) return; @@ -228,18 +291,26 @@ struct Navigation::Impl : rmcs::navigation::NodeMixin { auto transform = tf_buffer->lookupTransform(kWorldFrame, kBaseFrame, tf2::TimePointZero); auto& translation = transform.transform.translation; - auto& rotation = transform.transform.rotation; - - auto sin_yaw = 2.0 * ((rotation.w * rotation.z) + (rotation.x * rotation.y)); - auto cos_yaw = 1.0 - (2.0 * ((rotation.y * rotation.y) + (rotation.z * rotation.z))); - auto yaw = std::atan2(sin_yaw, cos_yaw); + auto yaw = parse_pose_yaw(transform.transform.rotation); return {translation.x, translation.y, yaw}; - } catch (const tf2::TransformException& exception) { + } catch (const tf2::TransformException&) { constexpr auto kNan = std::numeric_limits::quiet_NaN(); return {kNan, kNan, kNan}; } } + + auto check_bottom_yaw() const -> double { return std::get<2>(check_position()); } + + auto check_active_goal() const -> std::tuple { + auto lock = std::scoped_lock{goal_mutex}; + if (!active_goal.has_value()) { + constexpr auto kNan = std::numeric_limits::quiet_NaN(); + return {kNan, kNan, kNan}; + } + + return {active_goal->x, active_goal->y, active_goal->yaw}; + } }; Navigation::Navigation(rclcpp::Node& node) noexcept @@ -248,6 +319,9 @@ Navigation::Navigation(rclcpp::Node& node) noexcept Navigation::~Navigation() noexcept = default; auto Navigation::send_target(double x, double y) -> void { pimpl->send_target(x, y); } +auto Navigation::send_target(double x, double y, double yaw) -> void { + pimpl->send_target(x, y, yaw); +} auto Navigation::switch_topic_forward(bool enable) -> void { pimpl->switch_topic_forward(enable); } @@ -255,4 +329,12 @@ auto Navigation::check_position() const -> std::tuple { return pimpl->check_position(); } +auto Navigation::check_bottom_yaw() const -> double { return pimpl->check_bottom_yaw(); } + +auto Navigation::check_active_goal() const -> std::tuple { + return pimpl->check_active_goal(); +} + +auto Navigation::cancel_target() -> void { pimpl->cancel_target(); } + } // namespace rmcs::navigation::details diff --git a/src/cxx/navigation.hh b/src/cxx/navigation.hh index bf0d1b5..dc914af 100644 --- a/src/cxx/navigation.hh +++ b/src/cxx/navigation.hh @@ -21,6 +21,7 @@ public: /// 用法示例： /// - navigation.send_target(1.5, -2.0); auto send_target(double x, double y) -> void; + auto send_target(double x, double y, double yaw) -> void; /// 查询当前位姿（world -> base_link）。 /// @@ -36,11 +37,31 @@ public: /// - auto [x, y, yaw] = navigation.check_position(); auto check_position() const -> std::tuple; + /// 查询底部云台轴当前朝向（world 坐标系 yaw）。 + /// + /// 行为说明： + /// - 若 TF 查询失败，会返回 nan。 + auto check_bottom_yaw() const -> double; + + /// 查询当前活跃导航目标点（world 坐标系）。 + /// + /// 返回值： + /// - tuple 第 1 项：goal x（米）； + /// - tuple 第 2 项：goal y（米）。 + /// - tuple 第 3 项：goal yaw（弧度，若无有效朝向则为 nan）。 + /// + /// 行为说明： + /// - 若当前无活跃 action 目标，返回 {nan, nan, nan}。 + auto check_active_goal() const -> std::tuple; + + /// 取消当前活跃导航目标。 + auto cancel_target() -> void; + /// 开关 goal topic 转发功能（可选）。 /// /// 行为说明： /// - enable=true 时，订阅 `/move_base_simple/goal` 和 `/goal_pose`； - /// - 收到 PoseStamped 后，会提取 `pose.position.{x,y}` 并调用 send_target； + /// - 收到 PoseStamped 后，会提取 `pose.position.{x,y}` 与 `pose.orientation yaw` 并调用 send_target； /// - enable=false 时，取消上述订阅并停止转发。 /// /// 用法示例： diff --git a/src/cxx/navigation_gimbal_controller.cc b/src/cxx/navigation_gimbal_controller.cc new file mode 100644 index 0000000..c8e001a --- /dev/null +++ b/src/cxx/navigation_gimbal_controller.cc @@ -0,0 +1,150 @@ +#include "navigation_gimbal_controller.hh" +#include "navigation.hh" +#include +#include +#include +#include + +namespace rmcs::navigation::details { + +NavigationGimbalController::NavigationGimbalController(rclcpp::Node& node, Navigation& navigation) + : node_{node} + , navigation_{navigation} {} + +auto NavigationGimbalController::init_output(rmcs_executor::Component& component) -> void { + component.register_output( + "/rmcs_navigation/gimbal_velocity", gimbal_speed_, Eigen::Vector2d::Zero()); +} + +auto NavigationGimbalController::init_params() -> void { + kp_ = node_.get_parameter_or("navigation_gimbal_yaw_kp", 0.2); + speed_max_ = node_.get_parameter_or("navigation_gimbal_yaw_speed_max", 0.5); + tolerance_ = node_.get_parameter_or( + "navigation_gimbal_yaw_tolerance", + node_.get_parameter_or("navigation_gimbal_yaw_tolerance_deg", 8.0) + * (std::numbers::pi_v / 180.0)); + smooth_alpha_ = node_.get_parameter_or("navigation_gimbal_yaw_smooth_alpha", 0.02); + acc_limit_ = node_.get_parameter_or("navigation_gimbal_yaw_acc_limit", 1.0); + test_mode_ = node_.get_parameter_or("navigation_gimbal_velocity_test_mode", false); + test_yaw_speed_ = + node_.get_parameter_or("navigation_gimbal_velocity_test_yaw_speed", 0.2); + test_pitch_amplitude_ = node_.get_parameter_or( + "navigation_gimbal_velocity_test_pitch_speed_amplitude", 0.3); + test_pitch_frequency_ = node_.get_parameter_or( + "navigation_gimbal_velocity_test_pitch_speed_frequency", 0.2); +} + +auto NavigationGimbalController::set_lua_target_yaw(double yaw) -> void { + auto lock = std::scoped_lock{lua_mutex_}; + if (std::isfinite(yaw)) { + lua_target_yaw_ = yaw; + lua_target_yaw_active_ = true; + return; + } + + lua_target_yaw_ = std::numeric_limits::quiet_NaN(); + lua_target_yaw_active_ = false; +} + +auto NavigationGimbalController::get_lua_target_yaw() const -> std::optional { + auto lock = std::scoped_lock{lua_mutex_}; + if (!lua_target_yaw_active_ || !std::isfinite(lua_target_yaw_)) + return std::nullopt; + + return lua_target_yaw_; +} + +auto NavigationGimbalController::smooth_yaw_speed(double target_speed) -> double { + constexpr double kControlDt = 1e-3; + + target_speed = std::clamp(target_speed, -speed_max_, speed_max_); + + const auto alpha = std::clamp(smooth_alpha_, 0.0, 1.0); + const auto prev = speed_command_; + auto filtered = prev + alpha * (target_speed - prev); + + if (acc_limit_ > 0.0) { + const auto max_step = acc_limit_ * kControlDt; + const auto delta = std::clamp(filtered - prev, -max_step, max_step); + filtered = prev + delta; + } + + if (std::abs(filtered) < 1e-4) + filtered = 0.0; + + speed_command_ = filtered; + return filtered; +} + +auto NavigationGimbalController::sync_to_navigation_goal() -> void { + const auto current_yaw = std::get<2>(navigation_.check_position()); + + auto apply_yaw_target = [&](double target_yaw) { + auto target_yaw_speed = 0.0; + if (std::isfinite(target_yaw) && std::isfinite(current_yaw)) { + const auto yaw_error = + std::remainder(target_yaw - current_yaw, 2.0 * std::numbers::pi_v); + if (std::abs(yaw_error) > tolerance_) + target_yaw_speed = kp_ * yaw_error; + } + *gimbal_speed_ = Eigen::Vector2d{smooth_yaw_speed(target_yaw_speed), 0.0}; + }; + + // Lua override always wins — does not pollute last_navigation_goal_yaw_ + if (auto override_yaw = get_lua_target_yaw(); override_yaw.has_value()) { + apply_yaw_target(*override_yaw); + return; + } + + // Compute target yaw from the active navigation goal (goal-yaw > position-delta) + constexpr auto kNan = std::numeric_limits::quiet_NaN(); + + const auto [goal_x, goal_y, goal_yaw] = navigation_.check_active_goal(); + const auto position = navigation_.check_position(); + const auto x = std::get<0>(position); + const auto y = std::get<1>(position); + + auto nav_target_yaw = kNan; + if (std::isfinite(goal_yaw)) + nav_target_yaw = goal_yaw; + else if (std::isfinite(goal_x) && std::isfinite(goal_y) && std::isfinite(x) && std::isfinite(y)) + nav_target_yaw = std::atan2(goal_y - y, goal_x - x); + + // Persist valid navigation target yaw so the gimbal can continue turning even + // after Nav2 considers the position reached and clears the active goal. + if (std::isfinite(nav_target_yaw)) + last_navigation_goal_yaw_ = nav_target_yaw; + + auto effective_yaw = nav_target_yaw; + if (!std::isfinite(effective_yaw)) + effective_yaw = last_navigation_goal_yaw_; + + apply_yaw_target(effective_yaw); + + // Once heading is aligned AND the navigation goal is gone, release the hold + if (!std::isfinite(nav_target_yaw) && std::isfinite(last_navigation_goal_yaw_) + && std::isfinite(current_yaw)) { + const auto yaw_error = std::remainder( + last_navigation_goal_yaw_ - current_yaw, 2.0 * std::numbers::pi_v); + if (std::abs(yaw_error) <= tolerance_) + last_navigation_goal_yaw_ = kNan; + } +} + +auto NavigationGimbalController::update_test_pattern() -> void { + const auto t = node_.now().seconds(); + const auto pitch_speed = test_pitch_amplitude_ + * std::sin(2.0 * std::numbers::pi_v * test_pitch_frequency_ * t); + + *gimbal_speed_ = Eigen::Vector2d{test_yaw_speed_, pitch_speed}; +} + +auto NavigationGimbalController::update() -> void { + if (test_mode_) { + update_test_pattern(); + } else { + sync_to_navigation_goal(); + } +} + +} // namespace rmcs::navigation::details diff --git a/src/cxx/navigation_gimbal_controller.hh b/src/cxx/navigation_gimbal_controller.hh new file mode 100644 index 0000000..acab050 --- /dev/null +++ b/src/cxx/navigation_gimbal_controller.hh @@ -0,0 +1,53 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace rmcs::navigation::details { + +class Navigation; + +class NavigationGimbalController { +public: + using Output = rmcs_executor::Component::OutputInterface; + + NavigationGimbalController(rclcpp::Node& node, Navigation& navigation); + + auto init_output(rmcs_executor::Component& component) -> void; + auto init_params() -> void; + auto set_lua_target_yaw(double yaw) -> void; + auto update() -> void; + +private: + auto get_lua_target_yaw() const -> std::optional; + auto smooth_yaw_speed(double target_speed) -> double; + auto sync_to_navigation_goal() -> void; + auto update_test_pattern() -> void; + + rclcpp::Node& node_; + Navigation& navigation_; + + Output gimbal_speed_; + + mutable std::mutex lua_mutex_; + double lua_target_yaw_ = std::numeric_limits::quiet_NaN(); + bool lua_target_yaw_active_ = false; + + double last_navigation_goal_yaw_ = std::numeric_limits::quiet_NaN(); + + double kp_ = 0.5; + double speed_max_ = 0.8; + double tolerance_ = std::numbers::pi_v / 18.0; + double smooth_alpha_ = 0.02; + double acc_limit_ = 2.0; + double speed_command_ = 0.0; + + bool test_mode_ = false; + double test_yaw_speed_ = 2.0; + double test_pitch_amplitude_ = 0.3; + double test_pitch_frequency_ = 0.2; +}; + +} // namespace rmcs::navigation::details diff --git a/src/lua/action.lua b/src/lua/action.lua index e9193b6..274a6d9 100644 --- a/src/lua/action.lua +++ b/src/lua/action.lua @@ -1,6 +1,7 @@ local util = require("util.math") local api = require("api") local request = require("util.scheduler").request +local blackboard = require("blackboard").singleton() local NaN = 0 / 0 @@ -77,6 +78,14 @@ function action:update_chassis_vel(x, y) api.update_chassis_vel(x, y) end +function action:set_local_obstacle(enable) + api.set_local_obstacle(enable) +end + +function action:cancel_target() + api.cancel_target() +end + function action:restart_navigation(config) return api.restart_navigation(config) end @@ -96,4 +105,84 @@ function action:navigate(position) self.target = position end +--- 直线导航到目标点，只走当前点到目标点的方向，忽略侧向纠偏。 +--- +--- 原理：每次重发时将目标点沿道路垂向平移，使得 Nav2 看到的 +--- 前进方向始终平行于起始方向，不会产生侧向速度分量。 +--- +--- @param target {x: number, y: number} +--- @param tolerance? number -- 沿道路方向的到达容差，默认 0.15 +--- @param timeout? number -- 超时秒数，默认 30 +--- @param speed? number -- 直线速度(m/s)，不传则用 Nav2 原速 +function action:navigate_straight(target, tolerance, timeout, speed) + if tolerance == nil then tolerance = 0.15 end + if timeout == nil then timeout = 30 end + + local start = { x = blackboard.user.x, y = blackboard.user.y } + + local dx = target.x - start.x + local dy = target.y - start.y + local length = math.sqrt(dx * dx + dy * dy) + + if length < 0.01 then + self:warn("navigate_straight: start and target are too close") + self.target = { x = NaN, y = NaN } + self:cancel_target() + return + end + + local ux = dx / length + local uy = dy / length + local nx = -uy + local ny = ux + + local elapsed = 0 + local interval = 0.5 + + while elapsed < timeout do + local user = blackboard.user + + if speed then + api.set_chassis_vel_override(speed * ux, speed * uy) + end + + local perp = (user.x - start.x) * nx + (user.y - start.y) * ny + self:navigate({ + x = target.x + perp * nx, + y = target.y + perp * ny, + }) + + request:sleep(interval) + elapsed = elapsed + interval + + local fresh = blackboard.user + local proj = (fresh.x - start.x) * ux + (fresh.y - start.y) * uy + if proj >= length - tolerance then + self.target = { x = NaN, y = NaN } + self:cancel_target() + if speed then + api.set_chassis_vel_override(0, 0) + request:sleep(0.3) + api.clear_chassis_vel_override() + end + return + end + end + + self.target = { x = NaN, y = NaN } + self:cancel_target() + if speed then + api.set_chassis_vel_override(0, 0) + request:sleep(0.3) + api.clear_chassis_vel_override() + end + + self:warn( + string.format( + "navigate_straight: timeout (%.1fs), target (%.2f, %.2f)", + timeout, target.x, target.y + ) + ) +end + return action diff --git a/src/lua/api.lua b/src/lua/api.lua index ab357ad..4905b86 100644 --- a/src/lua/api.lua +++ b/src/lua/api.lua @@ -17,6 +17,7 @@ local util = require("util.native") --- @field update_gimbal_dominator fun(name: string) --- @field update_chassis_mode fun(mode: string) --- @field update_chassis_vel fun(x: number, y: number) +--- @field set_local_obstacle fun(enable: boolean) --- local api = setmetatable({}, { __index = function(_, name) diff --git a/src/lua/blackboard.lua b/src/lua/blackboard.lua index dea3bcb..a75caf2 100644 --- a/src/lua/blackboard.lua +++ b/src/lua/blackboard.lua @@ -52,14 +52,16 @@ local function create_default_blackboard() assembly_zone = PointPair { { 0, 0 }, { 0, 0 } }, -- 特殊跨越地形坐标 - road_tunnel_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 公路隧道 + road_tunnel_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 公路隧道 road_tunnel_final = PointPair { { 0, 0 }, { 0, 0 } }, - one_step_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 一级台阶 + one_step_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 一级台阶 one_step_final = PointPair { { 0, 0 }, { 0, 0 } }, - two_step_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 二级台阶 + two_step_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 二级台阶 two_step_final = PointPair { { 0, 0 }, { 0, 0 } }, common_elevated_ground_begin = PointPair { { 0, 0 }, { 0, 0 } }, -- 普通高地（飞坡起点那个高地） common_elevated_ground_final = PointPair { { 0, 0 }, { 0, 0 } }, + rough_terrain_begin = PointPair { { 0, 0 }, { 0.0, 0 } }, -- 起伏路段 + rough_terrain_final = PointPair { { 2.5, 0 }, { 0, 0 } }, }, } @@ -67,6 +69,9 @@ local function create_default_blackboard() rswitch = function() return result.play.rswitch end, + lswitch = function() + return result.play.lswitch + end, } result.condition = { diff --git a/src/lua/endpoint/test.lua b/src/lua/endpoint/test.lua index f1019ee..43fc085 100644 --- a/src/lua/endpoint/test.lua +++ b/src/lua/endpoint/test.lua @@ -11,6 +11,8 @@ local Scheduler = require("util.scheduler") local scheduler = Scheduler.new() local request = Scheduler.request +local crossing_rough_terrain = require("task.crossing-rough-terrain") + local restart_navigation = function() action:info("导航即将重启") action:restart_navigation { @@ -21,6 +23,12 @@ local restart_navigation = function() } end +local run_rough_terrain = function() + scheduler:append_task(function() + crossing_rough_terrain(true) + end) +end + --- --- Export Context --- @@ -34,8 +42,12 @@ on_init = function() clock:reset(blackboard.meta.timestamp) action:switch_topic_forward(true) + action:bind(scheduler) + restart_navigation() - edges:on(blackboard.getter.rswitch, "UP", restart_navigation) + -- edges:on(blackboard.getter.rswitch, "UP", restart_navigation) + edges:on(blackboard.getter.rswitch, "UP", run_rough_terrain) + scheduler:append_task(function() while true do diff --git a/src/lua/task/crossing-rough-terrain.lua b/src/lua/task/crossing-rough-terrain.lua new file mode 100644 index 0000000..587065e --- /dev/null +++ b/src/lua/task/crossing-rough-terrain.lua @@ -0,0 +1,81 @@ +local blackboard = require("blackboard").singleton() +local action = require("action") + +--- 过起伏路段任务。 +--- +--- 流程： +--- 1. 切换底盘为 step_down 模式 +--- 2. 发送云台角度（垂直于路段方向） +--- 3. 关闭 local_map 避障 +--- 4. 直线导航到终点（只走前进方向） +--- 5. 恢复底盘为 auto 模式 +--- 6. 释放云台方向覆盖 +--- 7. 恢复 local_map 避障 +--- +--- @param forward_center boolean -- true = begin→final 方向, false = final→begin +--- @param disable_obstacle? boolean -- 是否关闭 local_map 避障，默认 true +return function(forward_center, disable_obstacle) + if disable_obstacle == nil then + disable_obstacle = true + end + + local user = blackboard.user + local rule = blackboard.rule + + local begin_ours = rule.rough_terrain_begin.ours + local begin_them = rule.rough_terrain_begin.them + local final_ours = rule.rough_terrain_final.ours + local final_them = rule.rough_terrain_final.them + + -- 离哪一个入口近就用哪个阵营的坐标 + local dist_ours = math.sqrt( + (user.x - begin_ours.x) ^ 2 + (user.y - begin_ours.y) ^ 2 + ) + local dist_them = math.sqrt( + (user.x - begin_them.x) ^ 2 + (user.y - begin_them.y) ^ 2 + ) + + local begin, final + if dist_ours <= dist_them then + begin = begin_ours + final = final_ours + else + begin = begin_them + final = final_them + end + + local from, to + if forward_center then + from = begin + to = final + else + from = final + to = begin + end + + -- 1. 切换底盘为 step_down 模式 + action:update_chassis_mode("step_down") + + -- 2. 设定云台朝向（垂直于路段方向） + local dx = to.x - from.x + local dy = to.y - from.y + local perpendicular_yaw = math.atan(dy, dx) + action:update_gimbal_direction(perpendicular_yaw) + + -- 3. 关闭 local_map 避障 + if disable_obstacle then + action:set_local_obstacle(false) + end + + -- 4. 直线导航到终点（只走前进方向） + action:navigate_straight(to, 0.3, 20, 1.8) + + -- 5. 恢复底盘为 auto 模式 + action:update_chassis_mode("auto") + + -- 6. 释放云台方向覆盖 + action:update_gimbal_direction(0 / 0) + if disable_obstacle then + action:set_local_obstacle(true) + end +end