diff --git a/docker/deploy/Dockerfile b/docker/deploy/Dockerfile index 96d8af3..bc005ea 100644 --- a/docker/deploy/Dockerfile +++ b/docker/deploy/Dockerfile @@ -154,9 +154,10 @@ USER root COPY ./settings/tf2_sensor_msgs.hpp /opt/ros/humble/include/tf2_sensor_msgs/tf2_sensor_msgs.hpp ############################################################################## -# Copy tmux config file +# Copy tmux config file and vimrc ############################################################################## COPY ./settings/.tmux.conf /home/${USERNAME}/.tmux.conf +COPY ./settings/.vimrc /home/${USERNAME}/.vimrc ############################################################################## # Copy scripts and set permissions diff --git a/docker/deploy/settings/.vimrc b/docker/deploy/settings/.vimrc new file mode 100644 index 0000000..e8fad1d --- /dev/null +++ b/docker/deploy/settings/.vimrc @@ -0,0 +1,65 @@ +" Comments in Vimscript start with a `"`. + +" If you open this file in Vim, it'll be syntax highlighted for you. + +" Vim is based on Vi. Setting `nocompatible` switches from the default +" Vi-compatibility mode and enables useful Vim functionality. This +" configuration option turns out not to be necessary for the file named +" '~/.vimrc', because Vim automatically enters nocompatible mode if that file +" is present. But we're including it here just in case this config file is +" loaded some other way (e.g. saved as `foo`, and then Vim started with +" `vim -u foo`). +set nocompatible + +" Turn on syntax highlighting. +syntax on + +" Disable the default Vim startup message. +set shortmess+=I + +" Show line numbers. +set number + +" This enables relative line numbering mode. With both number and +" relativenumber enabled, the current line shows the true line number, while +" all other lines (above and below) are numbered relative to the current line. +" This is useful because you can tell, at a glance, what count is needed to +" jump up or down to a particular line, by {count}k to go up or {count}j to go +" down. +set relativenumber + +" Always show the status line at the bottom, even if you only have one window open. +set laststatus=2 + +" The backspace key has slightly unintuitive behavior by default. For example, +" by default, you can't backspace before the insertion point set with 'i'. +" This configuration makes backspace behave more reasonably, in that you can +" backspace over anything. +set backspace=indent,eol,start + +" By default, Vim doesn't let you hide a buffer (i.e. have a buffer that isn't +" shown in any window) that has unsaved changes. This is to prevent you from " +" forgetting about unsaved changes and then quitting e.g. via `:qa!`. We find +" hidden buffers helpful enough to disable this protection. See `:help hidden` +" for more information on this. +set hidden + +" This setting makes search case-insensitive when all characters in the string +" being searched are lowercase. However, the search becomes case-sensitive if +" it contains any capital letters. This makes searching more convenient. +set ignorecase +set smartcase + +" Enable searching as you type, rather than waiting till you press enter. +set incsearch + +" Unbind some useless/annoying default key bindings. +nmap Q " 'Q' in normal mode enters Ex mode. You almost never want this. + +" Disable audible bell because it's annoying. +set noerrorbells visualbell t_vb= + +" Enable mouse support. You should avoid relying on this too much, but it can +" sometimes be convenient. +set mouse+=a + diff --git a/src/Navigation2/nav2_controller/src/controller_server.cpp b/src/Navigation2/nav2_controller/src/controller_server.cpp index 2acf92c..8e4c537 100644 --- a/src/Navigation2/nav2_controller/src/controller_server.cpp +++ b/src/Navigation2/nav2_controller/src/controller_server.cpp @@ -469,7 +469,6 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) void ControllerServer::computeAndPublishVelocity() { - static const std::string kRivalTimeoutSkipGoal = "TEB_RIVAL_STOP_TIMEOUT_SKIP_GOAL"; geometry_msgs::msg::PoseStamped pose; if (!getRobotPose(pose)) { @@ -492,24 +491,6 @@ void ControllerServer::computeAndPublishVelocity() goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); } catch (nav2_core::PlannerException & e) { - if (std::string(e.what()) == kRivalTimeoutSkipGoal) { - RCLCPP_WARN( - this->get_logger(), - "Rival stop timeout exceeded, marking current goal reached and advancing to next goal."); - cmd_vel_2d.twist.angular.x = 0; - cmd_vel_2d.twist.angular.y = 0; - cmd_vel_2d.twist.angular.z = 0; - cmd_vel_2d.twist.linear.x = 0; - cmd_vel_2d.twist.linear.y = 0; - cmd_vel_2d.twist.linear.z = 0; - cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); - cmd_vel_2d.header.stamp = now(); - end_pose_.header = pose.header; - end_pose_.pose = pose.pose; - current_path_.poses.clear(); - current_path_.poses.push_back(pose); - return; - } if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { RCLCPP_WARN(this->get_logger(), "%s", e.what()); cmd_vel_2d.twist.angular.x = 0; diff --git a/src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml b/src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml index a158490..2dee56e 100644 --- a/src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml +++ b/src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml @@ -18,11 +18,11 @@ - + - + - + diff --git a/src/custom_controller/CMakeLists.txt b/src/custom_controller/CMakeLists.txt index b3675a5..bde2f29 100644 --- a/src/custom_controller/CMakeLists.txt +++ b/src/custom_controller/CMakeLists.txt @@ -12,11 +12,14 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(yaml-cpp REQUIRED) @@ -27,10 +30,13 @@ set(dep_pkgs nav2_costmap_2d pluginlib nav_msgs + nav2_msgs geometry_msgs std_msgs + rclcpp_action tf2 - tf2_ros) + tf2_ros + tf2_geometry_msgs) include_directories(include) include_directories(${EIGEN3_INCLUDE_DIRS}) @@ -53,10 +59,13 @@ ament_target_dependencies(custom_controller nav2_costmap_2d pluginlib nav_msgs + nav2_msgs geometry_msgs std_msgs + rclcpp_action tf2 tf2_ros + tf2_geometry_msgs ) ament_target_dependencies(teb_controller rclcpp diff --git a/src/custom_controller/include/teb_controller/teb_controller.hpp b/src/custom_controller/include/teb_controller/teb_controller.hpp index 51b0b9b..2c2bca7 100644 --- a/src/custom_controller/include/teb_controller/teb_controller.hpp +++ b/src/custom_controller/include/teb_controller/teb_controller.hpp @@ -19,6 +19,9 @@ #include "nav_msgs/msg/path.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/bool.hpp" #include "tf2_ros/buffer.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -40,6 +43,8 @@ class TebController : public nav2_core::Controller public: TebController() = default; ~TebController() override = default; + using NavigateToPose = nav2_msgs::action::NavigateToPose; + using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle; void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -61,6 +66,22 @@ class TebController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; private: + struct RivalInfo + { + bool valid{false}; + double dx_world{0.0}; + double dy_world{0.0}; + double dx_body{0.0}; + double dy_body{0.0}; + double distance{std::numeric_limits::infinity()}; + }; + + enum class MotionMode + { + FollowPath, + RivalEscape + }; + // utils static double clamp(double v, double lo, double hi) { @@ -96,7 +117,31 @@ class TebController : public nav2_core::Controller double & tx, double & ty) const; void publishTebPath(); + void publishGoalReached() const; + bool sendEscapeGoal(const geometry_msgs::msg::PoseStamped & pose, const RivalInfo & rival); bool shouldTriggerReplan(bool raw_blocked, const rclcpp::Time & now); + RivalInfo getRivalInfo(const geometry_msgs::msg::PoseStamped & pose) const; + bool shouldEnterRivalEscape(const RivalInfo & rival, double cmd_vx, double cmd_vy) const; + bool shouldExitRivalEscape( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + bool blocked_and_close, + bool pose_collision) const; + bool buildRivalEscapeCommand( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + double current_speed, + geometry_msgs::msg::TwistStamped & cmd); + void applyRivalSlowdown(const RivalInfo & rival, double & vx, double & vy, double & w) const; + bool findRivalEscapeTarget( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + double & target_x, + double & target_y) const; + double distanceFromEscapeStart(const geometry_msgs::msg::PoseStamped & pose) const; + void beginRivalEscape(const geometry_msgs::msg::PoseStamped & pose); + void resetRivalEscapeState(); + void updateRivalStopDistance(); private: // ros @@ -107,6 +152,7 @@ class TebController : public nav2_core::Controller std::shared_ptr tf_; std::shared_ptr costmap_ros_; std::string name_; + std::string external_rival_data_path_; rclcpp::Subscription::SharedPtr global_costmap_sub_; rclcpp::Subscription::SharedPtr rival_pose_sub_; @@ -122,26 +168,29 @@ class TebController : public nav2_core::Controller // pubs rclcpp_lifecycle::LifecyclePublisher::SharedPtr teb_path_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr global_plan_pub_; + rclcpp::Publisher::SharedPtr goal_reached_pub_; + rclcpp_action::Client::SharedPtr navigate_to_pose_client_; // parameters (band) - double dt_ref_{0.1}; + double dt_ref_{0.05}; double resample_ds_{0.05}; - int iterations_{2}; + int iterations_{4}; // obstacle double min_obstacle_dist_{0.25}; - double w_smooth_{1.0}; - double w_obst_{2.0}; + double w_smooth_{0.0}; + double w_obst_{1.0}; double step_size_{0.05}; - double slowdown_obstacle_dist_{0.3}; + double slowdown_obstacle_dist_{0.7}; double stop_obstacle_dist_{0.15}; double obstacle_check_lookahead_{0.5}; - double obstacle_check_time_horizon_{1.0}; - double obstacle_cost_threshold_{nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE}; - bool enable_rival_slowdown_{true}; + double obstacle_check_time_horizon_{2.0}; + double obstacle_cost_threshold_{150.0}; double rival_slowdown_dist_{0.4}; double rival_min_speed_scale_{0.5}; double rival_close_distance_{0.5}; double rival_stop_distance_{0.35}; - double rival_stop_timeout_{0.0}; + double rival_stop_distance_prev_{0.35}; + double rival_escape_speed_{0.2}; + double rival_escape_distance_{0.18}; // tracking (holonomic) double lookahead_dist_{0.25}; @@ -149,8 +198,8 @@ class TebController : public nav2_core::Controller double k_w_{4.0}; // goal behavior - double goal_xy_stop_dist_{0.02}; - double goal_heading_switch_dist_{0.20}; + double goal_xy_stop_dist_{0.03}; + double goal_heading_switch_dist_{5.0}; // limits double max_v_{1.1}; @@ -170,19 +219,27 @@ class TebController : public nav2_core::Controller double last_w_{0.0}; // --- replan trigger params --- - double max_cost_threshold_{150.0}; - bool treat_no_info_as_obstacle_{false}; + double max_cost_threshold_{95.0}; + bool treat_no_info_as_obstacle_{true}; int cost_check_stride_{1}; double stop_v_eps_{0.05}; - double blocked_stop_clearance_{0.5}; - double replan_min_blocked_time_{0.3}; - double replan_cooldown_{0.6}; + double blocked_stop_clearance_{0.3}; + double replan_min_blocked_time_{0.2}; + double replan_cooldown_{0.01}; rclcpp::Time blocked_since_; rclcpp::Time last_replan_time_; - rclcpp::Time rival_stop_since_; + + MotionMode motion_mode_{MotionMode::FollowPath}; + geometry_msgs::msg::PoseStamped rival_escape_start_pose_; + bool has_rival_escape_start_{false}; + bool rival_escape_pending_stop_{false}; + bool rival_escape_goal_requested_{false}; + bool escape_navigation_active_{false}; + int rival_escape_stall_cycles_{0}; + int rival_escape_attempt_count_{0}; }; } // namespace teb_controller -#endif // TEB_CONTROLLER__TEB_CONTROLLER_HPP_ \ No newline at end of file +#endif // TEB_CONTROLLER__TEB_CONTROLLER_HPP_ diff --git a/src/custom_controller/package.xml b/src/custom_controller/package.xml index 9dcab5d..45ea358 100644 --- a/src/custom_controller/package.xml +++ b/src/custom_controller/package.xml @@ -11,11 +11,17 @@ geometry_msgs nav_msgs + nav2_core + nav2_msgs nav2_costmap_2d + pluginlib + rclcpp + rclcpp_action rclcpp_lifecycle std_msgs tf2 tf2_ros + tf2_geometry_msgs yaml-cpp ament_lint_auto diff --git a/src/custom_controller/src/teb_controller.cpp b/src/custom_controller/src/teb_controller.cpp index 678ce92..ff103da 100644 --- a/src/custom_controller/src/teb_controller.cpp +++ b/src/custom_controller/src/teb_controller.cpp @@ -3,6 +3,7 @@ #include #include +#include "yaml-cpp/yaml.h" #include "pluginlib/class_list_macros.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_core/exceptions.hpp" @@ -18,6 +19,14 @@ namespace teb_controller { +namespace +{ + +constexpr int kRivalEscapeStallCycleLimit = 5; +constexpr double kRivalStopDistanceMargin = 0.27; + +} // namespace + double TebController::yawFromQuat(const geometry_msgs::msg::Quaternion & q) { tf2::Quaternion tq; @@ -65,12 +74,13 @@ void TebController::configure( node->declare_parameter(name_ + ".obstacle_check_lookahead", obstacle_check_lookahead_); node->declare_parameter(name_ + ".obstacle_check_time_horizon", obstacle_check_time_horizon_); node->declare_parameter(name_ + ".obstacle_cost_threshold", obstacle_cost_threshold_); - node->declare_parameter(name_ + ".enable_rival_slowdown", enable_rival_slowdown_); node->declare_parameter(name_ + ".rival_slowdown_dist", rival_slowdown_dist_); node->declare_parameter(name_ + ".rival_min_speed_scale", rival_min_speed_scale_); node->declare_parameter(name_ + ".rival_close_distance", rival_close_distance_); node->declare_parameter(name_ + ".rival_stop_distance", rival_stop_distance_); - node->declare_parameter(name_ + ".rival_stop_timeout", rival_stop_timeout_); + node->declare_parameter(name_ + ".external_rival_data_path", external_rival_data_path_); + node->declare_parameter(name_ + ".rival_escape_speed", rival_escape_speed_); + node->declare_parameter(name_ + ".rival_escape_distance", rival_escape_distance_); node->declare_parameter(name_ + ".lookahead_dist", lookahead_dist_); node->declare_parameter(name_ + ".k_xy", k_xy_); @@ -106,12 +116,13 @@ void TebController::configure( node->get_parameter(name_ + ".obstacle_check_lookahead", obstacle_check_lookahead_); node->get_parameter(name_ + ".obstacle_check_time_horizon", obstacle_check_time_horizon_); node->get_parameter(name_ + ".obstacle_cost_threshold", obstacle_cost_threshold_); - node->get_parameter(name_ + ".enable_rival_slowdown", enable_rival_slowdown_); node->get_parameter(name_ + ".rival_slowdown_dist", rival_slowdown_dist_); node->get_parameter(name_ + ".rival_min_speed_scale", rival_min_speed_scale_); node->get_parameter(name_ + ".rival_close_distance", rival_close_distance_); node->get_parameter(name_ + ".rival_stop_distance", rival_stop_distance_); - node->get_parameter(name_ + ".rival_stop_timeout", rival_stop_timeout_); + node->get_parameter(name_ + ".external_rival_data_path", external_rival_data_path_); + node->get_parameter(name_ + ".rival_escape_speed", rival_escape_speed_); + node->get_parameter(name_ + ".rival_escape_distance", rival_escape_distance_); node->get_parameter(name_ + ".lookahead_dist", lookahead_dist_); node->get_parameter(name_ + ".k_xy", k_xy_); @@ -148,13 +159,11 @@ void TebController::configure( rival_min_speed_scale_ = clamp(rival_min_speed_scale_, 0.0, 1.0); rival_close_distance_ = std::max(0.0, rival_close_distance_); rival_stop_distance_ = std::max(0.0, rival_stop_distance_); - rival_stop_timeout_ = std::max(0.0, rival_stop_timeout_); - if (rival_slowdown_dist_ < rival_close_distance_) { - rival_slowdown_dist_ = rival_close_distance_; - } - if (rival_close_distance_ < rival_stop_distance_) { - rival_close_distance_ = rival_stop_distance_; - } + updateRivalStopDistance(); + rival_stop_distance_ = std::max(0.0, rival_stop_distance_); + rival_stop_distance_prev_ = rival_stop_distance_; + rival_escape_speed_ = std::max(0.01, rival_escape_speed_); + rival_escape_distance_ = std::max(0.02, rival_escape_distance_); max_cost_threshold_ = clamp(max_cost_threshold_, 0.0, 255.0); cost_check_stride_ = std::max(1, cost_check_stride_); @@ -167,6 +176,11 @@ void TebController::configure( teb_path_pub_ = node->create_publisher(name_ + "/teb_path", rclcpp::SystemDefaultsQoS()); // Mirror the custom_controller topic name for RViz global_plan_pub_ = node->create_publisher("received_global_plan", 5); + goal_reached_pub_ = node->create_publisher( + "goal_reached", + rclcpp::QoS(10).reliable().transient_local()); + navigate_to_pose_client_ = + rclcpp_action::create_client(node, "/navigate_to_pose"); global_costmap_sub_ = node->create_subscription( "/global_costmap/costmap", rclcpp::QoS(10), @@ -192,6 +206,8 @@ void TebController::cleanup() has_plan_ = false; teb_path_pub_.reset(); global_plan_pub_.reset(); + goal_reached_pub_.reset(); + navigate_to_pose_client_.reset(); rival_pose_sub_.reset(); global_costmap_sub_.reset(); has_rival_pose_ = false; @@ -200,8 +216,9 @@ void TebController::cleanup() last_stamp_ = rclcpp::Time(0, 0, clock_ ? clock_->get_clock_type() : RCL_ROS_TIME); blocked_since_ = rclcpp::Time(0, 0, clock_ ? clock_->get_clock_type() : RCL_ROS_TIME); last_replan_time_ = rclcpp::Time(0, 0, clock_ ? clock_->get_clock_type() : RCL_ROS_TIME); - rival_stop_since_ = rclcpp::Time(0, 0, clock_ ? clock_->get_clock_type() : RCL_ROS_TIME); last_vx_ = last_vy_ = last_w_ = 0.0; + escape_navigation_active_ = false; + resetRivalEscapeState(); } void TebController::activate() @@ -220,7 +237,8 @@ void TebController::setPlan(const nav_msgs::msg::Path & path) { std::scoped_lock lk(mtx_); global_plan_ = path; - rival_stop_since_ = rclcpp::Time(0, 0, clock_ ? clock_->get_clock_type() : RCL_ROS_TIME); + rival_escape_attempt_count_ = 0; + resetRivalEscapeState(); RCLCPP_INFO(logger_, "[%s] received global plan with %zu poses", name_.c_str(), path.poses.size()); if (global_plan_pub_ && global_plan_pub_->is_activated()) { @@ -527,6 +545,64 @@ void TebController::publishTebPath() } } +void TebController::publishGoalReached() const +{ + if (!goal_reached_pub_) { + return; + } + + std_msgs::msg::Bool msg; + msg.data = true; + goal_reached_pub_->publish(msg); +} + +bool TebController::sendEscapeGoal( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival) +{ + if (!navigate_to_pose_client_ || !navigate_to_pose_client_->action_server_is_ready()) { + return false; + } + + double target_x = 0.0; + double target_y = 0.0; + if (!findRivalEscapeTarget(pose, rival, target_x, target_y)) { + return false; + } + + NavigateToPose::Goal goal_msg; + goal_msg.pose.header.frame_id = global_plan_.header.frame_id.empty() ? "map" : global_plan_.header.frame_id; + goal_msg.pose.header.stamp = clock_->now(); + goal_msg.pose.pose.position.x = target_x; + goal_msg.pose.pose.position.y = target_y; + goal_msg.pose.pose.position.z = 0.0; + goal_msg.pose.pose.orientation = pose.pose.orientation; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + [this](std::shared_ptr goal_handle) { + std::scoped_lock lk(mtx_); + if (!goal_handle) { + escape_navigation_active_ = false; + rival_escape_goal_requested_ = false; + } + }; + send_goal_options.result_callback = + [this](const GoalHandleNavigateToPose::WrappedResult & result) { + std::scoped_lock lk(mtx_); + escape_navigation_active_ = false; + rival_escape_goal_requested_ = false; + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + publishGoalReached(); + } + }; + + escape_navigation_active_ = true; + rival_escape_goal_requested_ = true; + navigate_to_pose_client_->async_send_goal(goal_msg, send_goal_options); + return true; +} + bool TebController::shouldTriggerReplan(bool raw_blocked, const rclcpp::Time & now) { if (!raw_blocked) { @@ -553,12 +629,277 @@ bool TebController::shouldTriggerReplan(bool raw_blocked, const rclcpp::Time & n return true; } +TebController::RivalInfo TebController::getRivalInfo(const geometry_msgs::msg::PoseStamped & pose) const +{ + RivalInfo rival; + if (!has_rival_pose_) { + return rival; + } + + const double px = pose.pose.position.x; + const double py = pose.pose.position.y; + const double yaw = yawFromQuat(pose.pose.orientation); + const double c = std::cos(yaw); + const double s = std::sin(yaw); + + rival.valid = true; + rival.dx_world = latest_rival_pose_.pose.pose.position.x - px; + rival.dy_world = latest_rival_pose_.pose.pose.position.y - py; + rival.distance = std::hypot(rival.dx_world, rival.dy_world); + rival.dx_body = c * rival.dx_world + s * rival.dy_world; + rival.dy_body = -s * rival.dx_world + c * rival.dy_world; + return rival; +} + +bool TebController::shouldEnterRivalEscape( + const RivalInfo & rival, + double cmd_vx, + double cmd_vy) const +{ + if (!rival.valid || rival.distance > rival_stop_distance_) { + return false; + } + + const bool rival_in_front_half_plane = rival.dx_body >= -0.05; + const double approach_projection = cmd_vx * rival.dx_body + cmd_vy * rival.dy_body; + const bool pushing_toward_rival = approach_projection >= -0.01; + return rival_in_front_half_plane && pushing_toward_rival; +} + +bool TebController::shouldExitRivalEscape( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + bool blocked_and_close, + bool pose_collision) const +{ + const double release_distance = + rival_stop_distance_ + std::max(0.05, rival_escape_distance_ * 0.5); + + if (rival.valid && rival.distance <= release_distance) { + return false; + } + + if (blocked_and_close || pose_collision) { + return false; + } + + return distanceFromEscapeStart(pose) >= std::max(0.03, rival_escape_distance_ * 0.5); +} + +bool TebController::findRivalEscapeTarget( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + double & target_x, + double & target_y) const +{ + double away_x = -rival.dx_world; + double away_y = -rival.dy_world; + const double norm = std::hypot(away_x, away_y); + if (norm < 1e-6) { + const double yaw = yawFromQuat(pose.pose.orientation); + away_x = -std::cos(yaw); + away_y = -std::sin(yaw); + } else { + away_x /= norm; + away_y /= norm; + } + + const double px = pose.pose.position.x; + const double py = pose.pose.position.y; + const double step = std::max(0.02, rival_escape_distance_ / 10.0); + const double release_distance = + rival_stop_distance_ + std::max(0.05, rival_escape_distance_ * 0.5); + const double clearance_margin = std::max(0.03, rival_escape_distance_ * 0.25); + const double min_escape_distance = + std::max(step, release_distance - rival.distance + clearance_margin); + const double search_limit = + std::max(rival_escape_distance_, min_escape_distance + clearance_margin); + + for (double distance = min_escape_distance; distance <= search_limit + 1e-6; distance += step) { + const double candidate_x = px + away_x * distance; + const double candidate_y = py + away_y * distance; + const unsigned char cost = costAtGlobal(candidate_x, candidate_y); + if (!treat_no_info_as_obstacle_ && cost == nav2_costmap_2d::NO_INFORMATION) { + target_x = candidate_x; + target_y = candidate_y; + return true; + } + if (cost < obstacle_cost_threshold_) { + target_x = candidate_x; + target_y = candidate_y; + return true; + } + } + return false; +} + +double TebController::distanceFromEscapeStart(const geometry_msgs::msg::PoseStamped & pose) const +{ + if (!has_rival_escape_start_) { + return 0.0; + } + + const double dx = pose.pose.position.x - rival_escape_start_pose_.pose.position.x; + const double dy = pose.pose.position.y - rival_escape_start_pose_.pose.position.y; + return std::hypot(dx, dy); +} + +void TebController::beginRivalEscape(const geometry_msgs::msg::PoseStamped & pose) +{ + motion_mode_ = MotionMode::RivalEscape; + rival_escape_start_pose_ = pose; + has_rival_escape_start_ = true; + rival_escape_pending_stop_ = true; + rival_escape_goal_requested_ = false; + rival_escape_stall_cycles_ = 0; + rival_escape_attempt_count_++; + + RCLCPP_WARN( + logger_, + "[%s] rival escape #%d started from robot=(%.3f, %.3f)", + name_.c_str(), + rival_escape_attempt_count_, + pose.pose.position.x, + pose.pose.position.y); +} + +void TebController::resetRivalEscapeState() +{ + motion_mode_ = MotionMode::FollowPath; + rival_escape_start_pose_ = geometry_msgs::msg::PoseStamped{}; + has_rival_escape_start_ = false; + rival_escape_pending_stop_ = false; + rival_escape_goal_requested_ = false; + rival_escape_stall_cycles_ = 0; +} + +void TebController::updateRivalStopDistance() +{ + if (!external_rival_data_path_.empty()) { + try { + YAML::Node config = YAML::LoadFile(external_rival_data_path_); + if (config["nav_rival_parameters"] && + config["nav_rival_parameters"]["rival_inscribed_radius"]) + { + rival_stop_distance_ = + config["nav_rival_parameters"]["rival_inscribed_radius"].as() + + kRivalStopDistanceMargin; + rival_stop_distance_ = std::max(0.0, rival_stop_distance_); + if (rival_stop_distance_prev_ != rival_stop_distance_) { + RCLCPP_WARN( + logger_, + "[%s] rival_stop_distance updated to %f", + name_.c_str(), + rival_stop_distance_); + } + } else { + RCLCPP_WARN( + logger_, + "rival_inscribed_radius not found in YAML file, using default value"); + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, + "Failed to load YAML file: %s, using default value", + e.what()); + } + } + rival_stop_distance_prev_ = rival_stop_distance_; +} + +bool TebController::buildRivalEscapeCommand( + const geometry_msgs::msg::PoseStamped & pose, + const RivalInfo & rival, + double current_speed, + geometry_msgs::msg::TwistStamped & cmd) +{ + double target_x = 0.0; + double target_y = 0.0; + if (!findRivalEscapeTarget(pose, rival, target_x, target_y)) { + return false; + } + + const double progress = distanceFromEscapeStart(pose); + if (progress < 0.03 && current_speed <= stop_v_eps_) { + rival_escape_stall_cycles_++; + } else { + rival_escape_stall_cycles_ = 0; + } + + if (rival_escape_stall_cycles_ >= kRivalEscapeStallCycleLimit) { + return false; + } + + const double dx = target_x - pose.pose.position.x; + const double dy = target_y - pose.pose.position.y; + const double norm = std::hypot(dx, dy); + if (norm < 1e-6) { + return false; + } + const double dir_x = dx / norm; + const double dir_y = dy / norm; + const double yaw = yawFromQuat(pose.pose.orientation); + const double c = std::cos(yaw); + const double s = std::sin(yaw); + const double vel_x_world = dir_x * rival_escape_speed_; + const double vel_y_world = dir_y * rival_escape_speed_; + + cmd.twist.linear.x = c * vel_x_world + s * vel_y_world; + cmd.twist.linear.y = -s * vel_x_world + c * vel_y_world; + cmd.twist.angular.z = 0.0; + + RCLCPP_WARN( + logger_, + "[%s] rival escape #%d target=(%.3f, %.3f) cmd=(vx=%.3f, vy=%.3f, wz=%.3f) speed=%.3f", + name_.c_str(), + rival_escape_attempt_count_, + target_x, + target_y, + cmd.twist.linear.x, + cmd.twist.linear.y, + cmd.twist.angular.z, + std::hypot(cmd.twist.linear.x, cmd.twist.linear.y)); + return true; +} + +void TebController::applyRivalSlowdown( + const RivalInfo & rival, + double & vx, + double & vy, + double & w) const +{ + if (!rival.valid || rival.distance >= rival_slowdown_dist_) { + return; + } + + const double vmag = std::hypot(vx, vy); + if (vmag <= 1e-6) { + return; + } + + double scale = 1.0; + if (rival.distance <= rival_close_distance_) { + scale = rival_min_speed_scale_; + } else { + const double t = clamp( + (rival.distance - rival_close_distance_) / + std::max(1e-6, rival_slowdown_dist_ - rival_close_distance_), + 0.0, 1.0); + scale = rival_min_speed_scale_ + (1.0 - rival_min_speed_scale_) * t; + } + + vx *= scale; + vy *= scale; + w *= scale; +} + geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker) { std::scoped_lock lk(mtx_); + updateRivalStopDistance(); geometry_msgs::msg::TwistStamped cmd; const rclcpp::Time now = clock_->now(); @@ -595,34 +936,12 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( const unsigned char mc = maxCostOnBandGlobal(); const bool cost_bad = (mc >= (unsigned char)std::lround(max_cost_threshold_)); - // closest index on band size_t closest = 0; findClosestIndex(pose, closest); const double arc_window = std::max(lookahead_dist_, blocked_stop_clearance_ * 2.0); const double clearance = minObstacleDistanceOnBandGlobal(closest, arc_window, blocked_stop_clearance_); const bool blocked_and_close = cost_bad && (clearance <= blocked_stop_clearance_); - const double v_cur = std::hypot(velocity.linear.x, velocity.linear.y); - const bool slow_enough = (v_cur <= stop_v_eps_); - - if (blocked_and_close) { - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] blocked_and_close: max_cost=%u threshold=%.1f clearance=%.3f blocked_stop_clearance=%.3f speed=%.3f", - name_.c_str(), mc, max_cost_threshold_, clearance, blocked_stop_clearance_, v_cur); - cmd.twist.linear.x = 0.0; - cmd.twist.linear.y = 0.0; - cmd.twist.angular.z = 0.0; - - publishTebPath(); - if (slow_enough && shouldTriggerReplan(true, now)) { - RCLCPP_INFO(logger_, "[%s] throwing replan because path cost is too high and robot is nearly stopped", name_.c_str()); - throw nav2_core::PlannerException("TEB: max_cost exceeded and speed low -> replan"); - } - return cmd; - } - - // lookahead target along arc double tx = teb_band_.back().x; double ty = teb_band_.back().y; sampleLookaheadTargetArc(closest, lookahead_dist_, tx, ty); @@ -630,39 +949,31 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( const double px = pose.pose.position.x; const double py = pose.pose.position.y; const double yaw = yawFromQuat(pose.pose.orientation); - const double dx_w = tx - px; const double dy_w = ty - py; - // dist_to_target currently unused but kept for potential future tuning - // final goal info const double gx = global_plan_.poses.back().pose.position.x; const double gy = global_plan_.poses.back().pose.position.y; const double goal_dist = std::hypot(gx - px, gy - py); const double goal_yaw = yawFromQuat(global_plan_.poses.back().pose.orientation); - // translate world -> body const double c = std::cos(yaw); const double s = std::sin(yaw); - const double dx_b = c * dx_w + s * dy_w; + const double dx_b = c * dx_w + s * dy_w; const double dy_b = -s * dx_w + c * dy_w; - // Position -> (vx,vy) like your CustomController double vx = k_xy_ * dx_b; double vy = k_xy_ * dy_b; const unsigned char pose_cost = costAtGlobal(px, py); const bool pose_collision = (pose_cost >= obstacle_cost_threshold_); + const double v_cur = std::hypot(velocity.linear.x, velocity.linear.y); - // Near goal: stop translation to avoid overshoot if (goal_dist <= goal_xy_stop_dist_) { vx = 0.0; vy = 0.0; } - // Heading control: - // far: face the motion direction - // near: align with final goal yaw double target_yaw = std::atan2(dy_w, dx_w); if (goal_dist <= goal_heading_switch_dist_) { target_yaw = goal_yaw; @@ -670,7 +981,6 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( double heading_err = normAngle(target_yaw - yaw); double w = k_w_ * heading_err; - // limit translational speed magnitude double vmag = std::hypot(vx, vy); if (vmag > max_v_) { const double r = max_v_ / std::max(1e-9, vmag); @@ -680,7 +990,6 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( vmag = max_v_; } - // optional min_v_ (usually keep at 0) if (min_v_ > 0.0 && vmag > 1e-6 && vmag < min_v_ && goal_dist > goal_xy_stop_dist_) { const double r = min_v_ / vmag; vx *= r; @@ -688,142 +997,112 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( w *= r; } - if (enable_rival_slowdown_ && has_rival_pose_) { - const double rival_dx = latest_rival_pose_.pose.pose.position.x - px; - const double rival_dy = latest_rival_pose_.pose.pose.position.y - py; - const double rival_distance = std::hypot(rival_dx, rival_dy); - const double goal_rival_dx = latest_rival_pose_.pose.pose.position.x - gx; - const double goal_rival_dy = latest_rival_pose_.pose.pose.position.y - gy; - const double goal_rival_distance = std::hypot(goal_rival_dx, goal_rival_dy); - const double rival_dx_b = c * rival_dx + s * rival_dy; - const double rival_dy_b = -s * rival_dx + c * rival_dy; - const bool moving_away_from_rival = ((vx * rival_dx_b + vy * rival_dy_b) < 0.0); - const bool allow_escape_from_rival_stop = - (rival_distance <= rival_stop_distance_) && - (goal_rival_distance > rival_stop_distance_) && - moving_away_from_rival; - - if (allow_escape_from_rival_stop) { - rival_stop_since_ = rclcpp::Time(0, 0, now.get_clock_type()); - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] allowing motion inside rival stop zone: rival_distance=%.3f goal_rival_distance=%.3f stop_distance=%.3f moving_away=1", - name_.c_str(), rival_distance, goal_rival_distance, rival_stop_distance_); - } else if (rival_distance <= rival_stop_distance_) { - if (rival_stop_since_.nanoseconds() == 0) { - rival_stop_since_ = now; - } - const double rival_stop_elapsed = (now - rival_stop_since_).seconds(); - if (rival_stop_timeout_ > 0.0 && rival_stop_elapsed >= rival_stop_timeout_) { - RCLCPP_WARN( - logger_, - "[%s] rival stop timeout exceeded: rival_distance=%.3f stop_distance=%.3f elapsed=%.3f timeout=%.3f", - name_.c_str(), rival_distance, rival_stop_distance_, rival_stop_elapsed, rival_stop_timeout_); - throw nav2_core::PlannerException("TEB_RIVAL_STOP_TIMEOUT_SKIP_GOAL"); - } - vx = 0.0; - vy = 0.0; - w = 0.0; - vmag = 0.0; - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] rival stop active: rival_distance=%.3f stop_distance=%.3f elapsed=%.3f timeout=%.3f", - name_.c_str(), rival_distance, - rival_stop_distance_, rival_stop_elapsed, rival_stop_timeout_); - } else if (rival_distance < rival_slowdown_dist_ && vmag > 1e-6) { - rival_stop_since_ = rclcpp::Time(0, 0, now.get_clock_type()); - double rival_scale = 1.0; - if (rival_distance <= rival_close_distance_) { - rival_scale = rival_min_speed_scale_; - } else { - const double t = clamp( - (rival_distance - rival_close_distance_) / std::max(1e-6, rival_slowdown_dist_ - rival_close_distance_), 0.0, 1.0); - rival_scale = rival_min_speed_scale_ + (1.0 - rival_min_speed_scale_) * t; + const RivalInfo rival = getRivalInfo(pose); + applyRivalSlowdown(rival, vx, vy, w); + vmag = std::hypot(vx, vy); + if (!escape_navigation_active_ && + (motion_mode_ == MotionMode::RivalEscape || shouldEnterRivalEscape(rival, vx, vy))) { + if (motion_mode_ != MotionMode::RivalEscape) { + beginRivalEscape(pose); + } + + if (!shouldExitRivalEscape(pose, rival, blocked_and_close, pose_collision)) { + if (rival_escape_pending_stop_) { + cmd.twist.linear.x = 0.0; + cmd.twist.linear.y = 0.0; + cmd.twist.angular.z = 0.0; + if (v_cur <= stop_v_eps_) { + rival_escape_pending_stop_ = false; + } + publishTebPath(); + return cmd; } - const double limited_v = max_v_ * rival_scale; - if (vmag > limited_v) { - const double r = limited_v / std::max(1e-9, vmag); - vx *= r; - vy *= r; - vmag = limited_v; + + if (!rival_escape_goal_requested_ && !sendEscapeGoal(pose, rival)) { + resetRivalEscapeState(); + throw nav2_core::PlannerException("TEB: rival escape goal dispatch failed"); } - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] rival slowdown active: rival_distance=%.3f slowdown_dist=%.3f close_distance=%.3f stop_distance=%.3f speed_scale=%.3f limited_v=%.3f", - name_.c_str(), rival_distance, rival_slowdown_dist_, rival_close_distance_, - rival_stop_distance_, rival_scale, limited_v); - } else { - rival_stop_since_ = rclcpp::Time(0, 0, now.get_clock_type()); + cmd.twist.linear.x = 0.0; + cmd.twist.linear.y = 0.0; + cmd.twist.angular.z = 0.0; + publishTebPath(); + return cmd; } + + resetRivalEscapeState(); } else { - rival_stop_since_ = rclcpp::Time(0, 0, now.get_clock_type()); + resetRivalEscapeState(); + } + + if (escape_navigation_active_) { + w = 0.0; + } + + if (blocked_and_close) { + cmd.twist.linear.x = 0.0; + cmd.twist.linear.y = 0.0; + cmd.twist.angular.z = 0.0; + publishTebPath(); + if (v_cur <= stop_v_eps_ && shouldTriggerReplan(true, now)) { + throw nav2_core::PlannerException("TEB: max_cost exceeded and speed low -> replan"); + } + return cmd; } w = clamp(w, -max_w_, max_w_); - // Apply Nav2 speed limit interface (only scale translation) if (speed_limit_ > 0.0) { double vlim = max_v_; if (speed_limit_is_percentage_) { - vlim = max_v_ * clamp(speed_limit_, 0.0, 100.0) / 100.0; + vlim = max_v_ * clamp(speed_limit_, 0.0, 100.0) / 100.0; } else { - vlim = clamp(speed_limit_, 0.0, max_v_); + vlim = clamp(speed_limit_, 0.0, max_v_); } const double vmag2 = std::hypot(vx, vy); if (vmag2 > vlim) { - const double r = vlim / std::max(1e-9, vmag2); - vx *= r; - vy *= r; + const double r = vlim / std::max(1e-9, vmag2); + vx *= r; + vy *= r; } } - // Forward obstacle check arc based on commanded speed const double check_arc = std::max(obstacle_check_lookahead_, vmag * obstacle_check_time_horizon_); - // If any band point within check_arc is high-cost, mark path blocked bool path_blocked = false; double acc_arc = 0.0; - RCLCPP_INFO(logger_, "[TEB Controller]: Checking path blockage started in computeVelocityCommands."); for (size_t i = closest; i < teb_band_.size(); ++i) { const auto & st = teb_band_[i]; - const unsigned char c = costAtGlobal(st.x, st.y); - if (c >= obstacle_cost_threshold_) { + const unsigned char c_band = costAtGlobal(st.x, st.y); + if (c_band >= obstacle_cost_threshold_) { path_blocked = true; break; } if (i + 1 < teb_band_.size()) { acc_arc += std::hypot(teb_band_[i + 1].x - st.x, teb_band_[i + 1].y - st.y); - if (acc_arc >= check_arc) break; + if (acc_arc >= check_arc) { + break; + } } } - RCLCPP_INFO(logger_, "[TEB Controller]: Checking path blockage finished in computeVelocityCommands."); - // Slow down / stop based on obstacle distance along band or robot pose const double min_obst_path = minObstacleDistanceOnBandGlobal(closest, check_arc, slowdown_obstacle_dist_); const double obst_pose_dist = minObstacleDistanceGlobal(px, py, slowdown_obstacle_dist_); double obst_dist = std::min(min_obst_path, obst_pose_dist); - if (pose_collision) obst_dist = 0.0; + if (pose_collision) { + obst_dist = 0.0; + } bool request_replan = false; - RCLCPP_INFO(logger_, "[TEB CONTROLLER]: Check blocked started in computeVelocityCommands"); if (path_blocked || obst_dist <= stop_obstacle_dist_) { - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] request_replan path_blocked=%d obst_dist=%.3f stop_obstacle_dist=%.3f pose_collision=%d check_arc=%.3f closest=%zu", - name_.c_str(), path_blocked, obst_dist, stop_obstacle_dist_, pose_collision, check_arc, closest); vx = 0.0; vy = 0.0; vmag = 0.0; w = 0.0; request_replan = true; } else if (std::isfinite(obst_dist) && obst_dist <= slowdown_obstacle_dist_) { - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] slowing down for obstacle: obst_dist=%.3f slowdown_obstacle_dist=%.3f stop_obstacle_dist=%.3f", - name_.c_str(), obst_dist, slowdown_obstacle_dist_, stop_obstacle_dist_); const double alpha = (obst_dist - stop_obstacle_dist_) / std::max(1e-6, slowdown_obstacle_dist_ - stop_obstacle_dist_); const double scale = clamp(alpha, 0.0, 1.0); @@ -831,17 +1110,16 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( vy *= scale; vmag *= scale; } - RCLCPP_INFO(logger_, "[TEB CONTROLLER]: Check blocked finished in computeVelocityCommands"); - // Accel limiting on vx, vy, w + if (last_stamp_.nanoseconds() != 0) { const double dt = (now - last_stamp_).seconds(); if (dt > 1e-3) { - const double dvx = clamp(vx - last_vx_, -max_acc_v_ * dt, max_acc_v_ * dt); - const double dvy = clamp(vy - last_vy_, -max_acc_v_ * dt, max_acc_v_ * dt); - const double dw = clamp(w - last_w_, -max_acc_w_ * dt, max_acc_w_ * dt); - vx = last_vx_ + dvx; - vy = last_vy_ + dvy; - w = last_w_ + dw; + const double dvx = clamp(vx - last_vx_, -max_acc_v_ * dt, max_acc_v_ * dt); + const double dvy = clamp(vy - last_vy_, -max_acc_v_ * dt, max_acc_v_ * dt); + const double dw = clamp(w - last_w_, -max_acc_w_ * dt, max_acc_w_ * dt); + vx = last_vx_ + dvx; + vy = last_vy_ + dvy; + w = last_w_ + dw; } } last_stamp_ = now; @@ -849,7 +1127,6 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( last_vy_ = vy; last_w_ = w; - RCLCPP_INFO(logger_, "[TEB CONTROLLER]: Check replan started in computeVelocityCommands"); const bool replan_ready = shouldTriggerReplan(request_replan, now); if (request_replan) { cmd.twist.linear.x = 0.0; @@ -857,25 +1134,14 @@ geometry_msgs::msg::TwistStamped TebController::computeVelocityCommands( cmd.twist.angular.z = 0.0; publishTebPath(); if (replan_ready) { - RCLCPP_INFO( - logger_, - "[%s] throwing replan: request_replan=1 path_blocked=%d obst_dist=%.3f pose_cost=%u max_band_cost=%u", - name_.c_str(), path_blocked, obst_dist, pose_cost, mc); throw nav2_core::PlannerException("TEB: path blocked or obstacle too close -> replan"); } - RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "[%s] request_replan active but waiting for replan cooldown / blocked duration", name_.c_str()); return cmd; } - RCLCPP_INFO(logger_, "[TEB CONTROLLER]: Check replan finished in computeVelocityCommands"); - RCLCPP_INFO_THROTTLE( - logger_, *clock_, 1000, - "[%s] cmd vx=%.3f vy=%.3f wz=%.3f closest=%zu goal_dist=%.3f pose_cost=%u band_max_cost=%u obst_dist=%.3f", - name_.c_str(), vx, vy, w, closest, goal_dist, pose_cost, mc, obst_dist); + cmd.twist.linear.x = vx; cmd.twist.linear.y = vy; cmd.twist.angular.z = w; - - RCLCPP_INFO(logger_, "[TEB CONTROLLER]: publish teb path in computeVelocityCommands"); publishTebPath(); return cmd; } diff --git a/src/navigation2_run/params/nav2_params_11.yaml b/src/navigation2_run/params/nav2_params_11.yaml index 2536fdc..59224be 100644 --- a/src/navigation2_run/params/nav2_params_11.yaml +++ b/src/navigation2_run/params/nav2_params_11.yaml @@ -45,6 +45,8 @@ bt_navigator: - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node + - nav2_persistent_sequence_bt_node + - nav2_persistent_sequence_w_cont_on_fail_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_path_expiring_timer_condition @@ -145,15 +147,10 @@ controller_server: # Controller parameters CheckIn: # very slow, for checkin plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" min_obstacle_dist: 0.2 lookahead_dist: 0.2 - w_smooth: 1.0 - w_obst: 1.0 + w_smooth: 1.0 k_xy: 2.0 k_w: 2.0 max_v: 0.6 @@ -162,23 +159,12 @@ controller_server: max_w: 2.0 max_acc_w: 5.0 stop_v_eps: 0.04 - max_cost_threshold: 95.0 - blocked_stop_clearance: 0.25 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.01 - slowdown_obstacle_dist: 0.7 stop_obstacle_dist: 0.2 - obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 2.0 - obstacle_cost_threshold: 150.0 - enable_rival_slowdown: true + obstacle_check_lookahead: 0.4 rival_slowdown_dist: 1.0 rival_min_speed_scale: 0.5 rival_close_distance: 0.65 rival_stop_distance: 0.5 - rival_stop_timeout: 3.0 # Fast: # plugin: "custom_controller::CustomController" @@ -199,52 +185,31 @@ controller_server: Fast: plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" min_obstacle_dist: 0.2 lookahead_dist: 0.4 w_smooth: 1.0 - w_obst: 1.0 k_xy: 2.8 k_w: 3.2 - max_v: 1.2 - min_v: 0.0 + max_v: 1.2 + min_v: 0.0 max_acc_v: 3.6 max_w: 4.0 max_acc_w: 12.0 stop_v_eps: 0.1 - max_cost_threshold: 95.0 - blocked_stop_clearance: 0.25 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.01 - slowdown_obstacle_dist: 0.7 stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 2.0 - obstacle_cost_threshold: 150.0 - enable_rival_slowdown: true rival_slowdown_dist: 1.0 rival_min_speed_scale: 0.5 rival_close_distance: 0.65 rival_stop_distance: 0.5 - rival_stop_timeout: 3.0 Slow: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" min_obstacle_dist: 0.2 lookahead_dist: 0.3 w_smooth: 1.0 - w_obst: 1.0 k_xy: 2.0 k_w: 2.0 max_v: 1.0 @@ -253,85 +218,54 @@ controller_server: max_w: 2.5 max_acc_w: 5.0 stop_v_eps: 0.06 - max_cost_threshold: 95.0 - blocked_stop_clearance: 0.25 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.01 - slowdown_obstacle_dist: 0.7 stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 2.0 - obstacle_cost_threshold: 150.0 - enable_rival_slowdown: true rival_slowdown_dist: 1.0 rival_min_speed_scale: 0.8 rival_close_distance: 0.6 rival_stop_distance: 0.48 - rival_stop_timeout: 3.0 LinearBoost: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 - min_obstacle_dist: 0.25 + external_rival_data_path: "/home/share/data/rival_params.yaml" + min_obstacle_dist: 0.2 lookahead_dist: 0.8 w_smooth: 1.0 - w_obst: 1.0 k_xy: 0.4 k_w: 2.0 - max_v: 0.5 - min_v: 0.0 + max_v: 0.5 + min_v: 0.0 max_acc_v: 2.5 max_w: 1.0 - max_acc_w: 3.0 - max_cost_threshold: 97.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - stop_v_eps: 0.05 - replan_min_blocked_time: 0.05 - replan_cooldown: 0.05 - slowdown_obstacle_dist: 1.2 + max_acc_w: 3.0 + stop_v_eps: 0.05 stop_obstacle_dist: 0.6 - obstacle_check_lookahead: 0.7 - obstacle_check_time_horizon: 0.8 - obstacle_cost_threshold: 240.0 + obstacle_check_lookahead: 0.7 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.5 + rival_close_distance: 0.65 + rival_stop_distance: 0.5 AngularBoost: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 - min_obstacle_dist: 0.25 + external_rival_data_path: "/home/share/data/rival_params.yaml" lookahead_dist: 0.5 w_smooth: 1.0 - w_obst: 1.0 k_xy: 0.6 k_w: 2.0 - max_v: 0.2 - min_v: 0.0 + max_v: 0.2 + min_v: 0.0 max_acc_v: 1.0 max_w: 4.0 - max_acc_w: 15.0 - stop_v_eps: 0.05 + max_acc_w: 15.0 + stop_v_eps: 0.05 slowdown_obstacle_dist: 0.6 stop_obstacle_dist: 0.25 - max_cost_threshold: 97.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: false - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.20 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 0.6 - obstacle_cost_threshold: 252.0 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.5 + rival_close_distance: 0.65 + rival_stop_distance: 0.5 local_costmap: local_costmap: @@ -366,7 +300,7 @@ global_costmap: trinary_costmap: false lethal_cost_threshold: 10 unknown_cost_value: 255 - plugins: ["static_layer", "inflation_layer", "rival_layer", "object_layer", "keepout_layer"] + plugins: ["static_layer", "inflation_layer", "rival_layer", "object_layer", "keepout_layer", "sima_layer"] # plugins: ["static_layer", "inflation_layer", "object_layer", "keepout_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" @@ -375,6 +309,40 @@ global_costmap: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 5.0 inflation_radius: 0.21 + sima_layer: + plugin: "Sima_costmap_plugin::SimaLayer" + enabled: True + auto_reset_with_timeout: true + model_size: 16 + x_cov_threshold: 0.012 + y_cov_threshold: 0.012 + R_sq_threshold: 0.88 + reset_timeout_threshold: 15 + sima_ids: [1, 2, 3, 4, 11, 12, 13, 14] + robot_inscribed_radius: 0.0 + rival_inscribed_radius: 0.2 + halted_inflation_radius: 0.2 + wandering_inflation_radius: 0.2 + moving_inflation_radius: 0.2 + unknown_inflation_radius: 0.05 + halted_cost_scaling_factor: 5.0 + wandering_cost_scaling_factor: 5.0 + moving_cost_scaling_factor: 5.0 + unknown_cost_scaling_factor: 15.0 + max_extend_length: 0.06 + cov_range_max: 0.02 + cov_range_min: 0.006 + vel_range_max: 0.4 + vel_range_min: 0.0 + inscribed_radius_rate: 0.98 + inflation_radius_rate: 5.0 + debug_mode: 0 + use_statistic_method: false + offset_vel_factor_weight_statistic: 0.25 + expand_vel_factor_weight_statistic: 0.25 + offset_vel_factor_weight_localization: 0.25 + expand_vel_factor_weight_localization: 0.25 + safe_distance: 0.12 rival_layer: plugin: "custom_path_costmap_plugin::RivalLayer" enabled: True @@ -615,11 +583,13 @@ docking_server: external_detection_rotation_pitch: 0.0 external_detection_rotation_roll: 0.0 filter_coef: 1.0 # 1.0->no filter. if filer_coef too small, cause dragging since this filter is not very good + pose_match_max_delta_ms: 50 + pose_sync_timer_period_ms: 100 # the threshold for CameraOnBoard to see ArUco camera_aruco_max: 0.42 camera_aruco_min: 0.19 # Optional dock pose history CSV logging (empty path disables) - dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dock_pose_history.csv" + dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dph.csv" dock_pose_history_suffix_datetime: true docks: ['home_dock'] @@ -645,6 +615,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Gentle: max_linear_vel: 0.15 @@ -659,24 +631,28 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Rush: - max_linear_vel: 0.1 + max_linear_vel: 0.4 min_linear_vel: 0.03 linear_kp_accel_vel: 0.4 linear_ki_accel_vel: 0.2 linear_kp_decel_dis: 0.06 max_speed_diff: 0.4 angular_kp: 0.2 - deceleration_distance: 0.03 + deceleration_distance: 0.10 reserved_distance: 0.0075 external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Cam: - max_linear_vel: 0.14 # when cam has low update rate ( < 10 ) - # max_linear_vel: 0.25 # when cam has high update rate ( > 10 ) + # max_linear_vel: 0.14 # when cam has low update rate ( < 10 ) + max_linear_vel: 0.22 # when cam has high update rate ( > 10 ) min_linear_vel: 0.03 linear_kp_accel_vel: 0.7 linear_ki_accel_vel: 0.3 @@ -688,6 +664,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 CamFront: max_linear_vel: 0.23 @@ -702,6 +680,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 final_pose_bridge: diff --git a/src/navigation2_run/params/nav2_params_13.yaml b/src/navigation2_run/params/nav2_params_13.yaml index 45b175d..fc4df5e 100644 --- a/src/navigation2_run/params/nav2_params_13.yaml +++ b/src/navigation2_run/params/nav2_params_13.yaml @@ -45,6 +45,8 @@ bt_navigator: - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node + - nav2_persistent_sequence_bt_node + - nav2_persistent_sequence_w_cont_on_fail_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_path_expiring_timer_condition @@ -145,34 +147,23 @@ controller_server: # Controller parameters DidilongController: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 - min_obstacle_dist: 0.25 - lookahead_dist: 0.7 - w_smooth: 1.0 - w_obst: 1.0 - k_xy: 0.35 - k_w: 2.0 - max_v: 0.4 + external_rival_data_path: "/home/share/data/rival_params.yaml" + min_obstacle_dist: 0.25 + lookahead_dist: 0.35 + k_xy: 3.0 + k_w: 2.0 + max_v: 0.7 min_v: 0.0 - max_acc_v: 1.2 - max_w: 0.75 - max_acc_w: 1.5 - max_cost_threshold: 97.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - stop_v_eps: 0.05 - replan_min_blocked_time: 0.05 - replan_cooldown: 0.05 - slowdown_obstacle_dist: 1.2 - stop_obstacle_dist: 0.6 - obstacle_check_lookahead: 0.7 - obstacle_check_time_horizon: 0.8 - obstacle_cost_threshold: 252.0 + max_acc_v: 1.6 + max_w: 2.0 + max_acc_w: 6.0 + stop_v_eps: 0.05 + stop_obstacle_dist: 0.2 + obstacle_check_lookahead: 0.4 + rival_slowdown_dist: 1.2 + rival_min_speed_scale: 0.75 + rival_close_distance: 0.8 + rival_stop_distance: 0.6 # Fast: # plugin: "custom_controller::CustomController" @@ -193,52 +184,29 @@ controller_server: Fast: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" min_obstacle_dist: 0.25 lookahead_dist: 0.4 - w_smooth: 0.0 - w_obst: 1.0 k_xy: 5.0 k_w: 3.0 - max_v: 1.5 - min_v: 0.0 + max_v: 1.5 + min_v: 0.0 max_acc_v: 4.5 max_w: 3.0 max_acc_w: 9.0 stop_v_eps: 0.1 - max_cost_threshold: 95.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.01 - slowdown_obstacle_dist: 0.7 stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 2.0 - obstacle_cost_threshold: 150.0 - enable_rival_slowdown: true rival_slowdown_dist: 1.0 rival_min_speed_scale: 0.55 rival_close_distance: 0.65 rival_stop_distance: 0.55 - rival_stop_timeout: 3.0 Slow: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" min_obstacle_dist: 0.25 - lookahead_dist: 0.35 - w_smooth: 0.0 - w_obst: 1.0 + lookahead_dist: 0.35 k_xy: 3.0 k_w: 2.0 max_v: 1.0 @@ -247,85 +215,52 @@ controller_server: max_w: 2.5 max_acc_w: 7.5 stop_v_eps: 0.05 - max_cost_threshold: 95.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.01 - slowdown_obstacle_dist: 0.7 stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 2.0 - obstacle_cost_threshold: 150.0 - enable_rival_slowdown: true rival_slowdown_dist: 1.0 rival_min_speed_scale: 0.75 rival_close_distance: 0.6 - rival_stop_distance: 0.48 - rival_stop_timeout: 3.0 + rival_stop_distance: 0.5 LinearBoost: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 - min_obstacle_dist: 0.25 - lookahead_dist: 0.8 - w_smooth: 1.0 - w_obst: 1.0 + external_rival_data_path: "/home/share/data/rival_params.yaml" + min_obstacle_dist: 0.25 + lookahead_dist: 0.8 k_xy: 0.4 k_w: 2.0 - max_v: 0.5 - min_v: 0.0 + max_v: 0.5 + min_v: 0.0 max_acc_v: 2.5 max_w: 1.0 - max_acc_w: 3.0 - max_cost_threshold: 97.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: true - cost_check_stride: 1 - stop_v_eps: 0.05 - replan_min_blocked_time: 0.05 - replan_cooldown: 0.05 - slowdown_obstacle_dist: 1.2 - stop_obstacle_dist: 0.6 + max_acc_w: 3.0 + stop_v_eps: 0.05 + stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.7 - obstacle_check_time_horizon: 0.8 - obstacle_cost_threshold: 240.0 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.55 + rival_close_distance: 0.65 + rival_stop_distance: 0.55 AngularBoost: # First version plugin: "teb_controller::TebController" - dt_ref: 0.05 - resample_ds: 0.05 - iterations: 4 - goal_xy_stop_dist: 0.03 - goal_heading_switch_dist: 5.0 - min_obstacle_dist: 0.25 + external_rival_data_path: "/home/share/data/rival_params.yaml" + min_obstacle_dist: 0.25 lookahead_dist: 0.5 - w_smooth: 1.0 - w_obst: 1.0 k_xy: 0.6 k_w: 2.0 - max_v: 0.2 - min_v: 0.0 + max_v: 0.2 + min_v: 0.0 max_acc_v: 1.0 max_w: 4.0 max_acc_w: 15.0 - stop_v_eps: 0.05 - slowdown_obstacle_dist: 0.6 - stop_obstacle_dist: 0.25 - max_cost_threshold: 97.0 - blocked_stop_clearance: 0.3 - treat_no_info_as_obstacle: false - cost_check_stride: 1 - replan_min_blocked_time: 0.20 - replan_cooldown: 0.20 + stop_v_eps: 0.05 + stop_obstacle_dist: 0.2 obstacle_check_lookahead: 0.4 - obstacle_check_time_horizon: 0.6 - obstacle_cost_threshold: 252.0 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.55 + rival_close_distance: 0.65 + rival_stop_distance: 0.55 local_costmap: local_costmap: @@ -496,47 +431,47 @@ planner_server: use_omnidirectional_robot: true planner_plugins: ["GridBased"] # navfn planner setting - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.02 - use_astar: True - allow_unknown: false - # ============================ - # A* / propagation parameters - # ============================ - heuristic_scale: 1.0 # Weight of heuristic term in A* (higher = more greedy toward goal) - priority_increment_scale: 2.0 # Scale factor for A* priority threshold increment (larger = faster but less optimal) - # ============================ - # Obstacle-aware bias - # ============================ - obstacle_bias_scale: 0.4 # Strength of extra cost added for high-cost cells to avoid obstacles/inflation (0.15 ~ 0.5) - obstacle_bias_offset: 10.0 # Cost offset before obstacle bias is applied (ignore small cost variations) (3.0 ~ 10.0) - # ============================ - # Path extraction / fallback - # ============================ - plateau_stagnation_steps: 3 # Number of stagnant steps to trigger plateau handling - min_gradient_norm: 0.9 # Minimum gradient norm to continue path extraction without fallback - potential_epsilon: 1.0e-3 # Minimum potential decrease to reset stagnation counter - - # smac planner setting # GridBased: - # plugin: "nav2_smac_planner/SmacPlanner2D" + # plugin: "nav2_navfn_planner/NavfnPlanner" # tolerance: 0.02 # use_astar: True # allow_unknown: false - # # useful parameters - # cost_travel_multiplier: 10.0 # larger values -> more obstacle avoidance - # distance_heuristic_weight: 1.0 # 1.0: normal A*, >1.0: heavily weighted to distance, <1.0: heavily weighted to cost - # cost_penalty: 1.0 # how much to penalize high cost cells (exponential) - # # no need to change - # downsample_costmap: false # set to true to downsample costmap for planning - # downsampling_factor: 1 # factor by which to downsample costmap (example 2 -> half size in each dimension) - # smooth_path: false # we prefer doing this by smoother - # max_iterations: 200000 - # max_on_approach_iterations: 1000 - # straight_line_max_skip_points: 500 - # straight_line_resample_points: 500 - # straight_line_resample_spacing: 0.01 + # # ============================ + # # A* / propagation parameters + # # ============================ + # heuristic_scale: 1.0 # Weight of heuristic term in A* (higher = more greedy toward goal) + # priority_increment_scale: 2.0 # Scale factor for A* priority threshold increment (larger = faster but less optimal) + # # ============================ + # # Obstacle-aware bias + # # ============================ + # obstacle_bias_scale: 0.4 # Strength of extra cost added for high-cost cells to avoid obstacles/inflation (0.15 ~ 0.5) + # obstacle_bias_offset: 10.0 # Cost offset before obstacle bias is applied (ignore small cost variations) (3.0 ~ 10.0) + # # ============================ + # # Path extraction / fallback + # # ============================ + # plateau_stagnation_steps: 3 # Number of stagnant steps to trigger plateau handling + # min_gradient_norm: 0.9 # Minimum gradient norm to continue path extraction without fallback + # potential_epsilon: 1.0e-3 # Minimum potential decrease to reset stagnation counter + + # smac planner setting + GridBased: + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.02 + use_astar: True + allow_unknown: false + # useful parameters + cost_travel_multiplier: 10.0 # larger values -> more obstacle avoidance + distance_heuristic_weight: 1.0 # 1.0: normal A*, >1.0: heavily weighted to distance, <1.0: heavily weighted to cost + cost_penalty: 1.0 # how much to penalize high cost cells (exponential) + # no need to change + downsample_costmap: false # set to true to downsample costmap for planning + downsampling_factor: 1 # factor by which to downsample costmap (example 2 -> half size in each dimension) + smooth_path: false # we prefer doing this by smoother + max_iterations: 200000 + max_on_approach_iterations: 1000 + straight_line_max_skip_points: 500 + straight_line_resample_points: 500 + straight_line_resample_spacing: 0.01 smoother_server: ros__parameters: @@ -643,11 +578,13 @@ docking_server: external_detection_rotation_pitch: 0.0 external_detection_rotation_roll: 0.0 filter_coef: 1.0 # 1.0->no filter. if filer_coef too small, cause dragging since this filter is not very good + pose_match_max_delta_ms: 50 + pose_sync_timer_period_ms: 100 # the threshold for CameraOnBoard to see ArUco - camera_aruco_max: 0.42 + camera_aruco_max: 0.42 # not used camera_aruco_min: 0.195 # Optional dock pose history CSV logging (empty path disables) - dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dock_pose_history.csv" + dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dph.csv" dock_pose_history_suffix_datetime: true docks: ['home_dock'] @@ -673,6 +610,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Gentle: max_linear_vel: 0.02 @@ -687,6 +626,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Rush: max_linear_vel: 0.3 @@ -701,9 +642,11 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 Cam: - max_linear_vel: 0.2 + max_linear_vel: 0.25 min_linear_vel: 0.025 linear_kp_accel_vel: 0.4 linear_ki_accel_vel: 0.2 @@ -715,6 +658,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 CamFront: max_linear_vel: 0.15 @@ -729,6 +674,8 @@ docking_server: external_rival_data_path: "/home/share/data/rival_params.yaml" stop_degree: 120.0 rival_radius: 0.44 + omni_docking_angle_threshold: 15.0 + omni_docking_dist_bound: 0.05 final_pose_bridge: ros__parameters: @@ -738,4 +685,4 @@ final_pose_bridge: system_check: ros__parameters: costmap_tolerance: 90 - external_rival_data_path: "/home/share/data/rival_params.yaml" \ No newline at end of file + external_rival_data_path: "/home/share/data/rival_params.yaml" diff --git a/src/navigation2_run/params/nav2_params_14.yaml b/src/navigation2_run/params/nav2_params_14.yaml index 36b8654..8642414 100644 --- a/src/navigation2_run/params/nav2_params_14.yaml +++ b/src/navigation2_run/params/nav2_params_14.yaml @@ -584,11 +584,13 @@ docking_server: external_detection_rotation_pitch: 0.0 external_detection_rotation_roll: 0.0 filter_coef: 1.0 + pose_match_max_delta_ms: 100 + pose_sync_timer_period_ms: 100 # the threshold for CameraOnBoard to see ArUco camera_aruco_max: 0.7 camera_aruco_min: 0.25 # Optional dock pose history CSV logging (empty path disables) - dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dock_pose_history.csv" + dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dph.csv" dock_pose_history_suffix_datetime: true docks: ['home_dock'] diff --git a/src/navigation2_run/params/nav2_params_default.yaml b/src/navigation2_run/params/nav2_params_default.yaml index cb4d747..11f5f02 100644 --- a/src/navigation2_run/params/nav2_params_default.yaml +++ b/src/navigation2_run/params/nav2_params_default.yaml @@ -45,6 +45,8 @@ bt_navigator: - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node + - nav2_persistent_sequence_bt_node + - nav2_persistent_sequence_w_cont_on_fail_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_path_expiring_timer_condition @@ -213,6 +215,11 @@ controller_server: obstacle_check_lookahead: 0.7 obstacle_check_time_horizon: 0.8 obstacle_cost_threshold: 240.0 + obstacle_cost_threshold: 150.0 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.75 + rival_close_distance: 0.6 + rival_stop_distance: 0.48 Slow: plugin: "teb_controller::TebController" @@ -245,6 +252,11 @@ controller_server: obstacle_check_lookahead: 0.4 obstacle_check_time_horizon: 0.6 obstacle_cost_threshold: 252.0 + obstacle_cost_threshold: 150.0 + rival_slowdown_dist: 1.0 + rival_min_speed_scale: 0.75 + rival_close_distance: 0.6 + rival_stop_distance: 0.48 LinearBoost: plugin: "teb_controller::TebController" @@ -635,12 +647,14 @@ docking_server: external_detection_rotation_pitch: 0.0 external_detection_rotation_roll: 0.0 filter_coef: 1.0 # 1.0->no filter. if filer_coef too small, cause dragging since this filter is not very good + pose_match_max_delta_ms: 100 + pose_sync_timer_period_ms: 100 # the threshold for CameraOnBoard to see ArUco camera_aruco_max: 0.3 camera_aruco_min: 0.19 # Optional dock pose history CSV logging (empty path disables) # dock_pose_history_file_path: "" - dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dock_pose_history.csv" + dock_pose_history_file_path: "/home/user/Eurobot-2026-Navigation2-ws/src/opennav_docking/opennav_docking/dock_records/dph.csv" dock_pose_history_suffix_datetime: true docks: ["home_dock"] diff --git a/src/opennav_docking/opennav_docking/dock_records/scripts/plot_dock_pose_history.py b/src/opennav_docking/opennav_docking/dock_records/scripts/plot_dock_pose_history.py index fddd6a9..fe3e27c 100644 --- a/src/opennav_docking/opennav_docking/dock_records/scripts/plot_dock_pose_history.py +++ b/src/opennav_docking/opennav_docking/dock_records/scripts/plot_dock_pose_history.py @@ -28,7 +28,7 @@ def resolve_csv_path(csv_or_dir: Path | None) -> Path: raise ValueError(f"Not a file or directory: {target}") candidates = sorted( - target.glob("dock_pose_history*.csv"), + target.glob("dph*.csv"), key=lambda p: p.stat().st_mtime, reverse=True, ) @@ -110,12 +110,20 @@ def pick(*candidates: str) -> str | None: "Expected columns like: time_sec,frame_id,x,y,z,yaw_rad" ) + # drop out last row that is usually corrupted + if len(t) > 1: + t = t[:-1] + x = x[:-1] + y = y[:-1] + z = z[:-1] + yaw = yaw[:-1] + t0 = t[0] t = [v - t0 for v in t] return frame_id or "unknown", t, x, y, z, yaw -def segment_goal_sets(t_vals, x_vals, y_vals, gap_threshold_sec=1.0, jump_threshold_m=0.40): +def segment_goal_sets(t_vals, x_vals, y_vals, gap_threshold_sec=3.0, jump_threshold_m=0.40): """ Split samples into goal sets. Primary rule: large timestamp gap. @@ -133,9 +141,11 @@ def segment_goal_sets(t_vals, x_vals, y_vals, gap_threshold_sec=1.0, jump_thresh new_segment = False if not all_zero_time and (t_vals[i] - t_vals[i - 1] > gap_threshold_sec): + print("new cluster by time") new_segment = True elif all_zero_time: jump = math.hypot(x_vals[i] - x_vals[i - 1], y_vals[i] - y_vals[i - 1]) + print("new cluster by dist") if jump > jump_threshold_m: new_segment = True diff --git a/src/opennav_docking/opennav_docking/include/opennav_docking/controller.hpp b/src/opennav_docking/opennav_docking/include/opennav_docking/controller.hpp index b2b9052..ee95238 100644 --- a/src/opennav_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/src/opennav_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -33,6 +33,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2/utils.h" #include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/int16.hpp" #include @@ -118,7 +119,9 @@ class Controller void updateParams(); rclcpp::Subscription::SharedPtr dock_controller_selector_sub_; rclcpp::Subscription::SharedPtr controller_function_sub_; + rclcpp::Subscription::SharedPtr dock_side_sub_; std::string controller_function_; + int cam_side_{1}; // 0:+y, 1:+x, 2:-y, 3:-x // Parameters from the config file double max_linear_vel_, min_linear_vel_; @@ -133,6 +136,8 @@ class Controller double angular_kp_; double look_ahead_distance_; double final_goal_angle_; + double omni_docking_angle_threshold_; // Angle threshold for omni-directional docking strategy + double omni_docking_dist_bound_; // Distance bound to relax omni docking angle threshold // see if need to stop double stop_degree_; @@ -181,6 +186,16 @@ class Controller void ConstantVelocity(double & vel, const double & remaining_distance, VelocityState & state); void Deceleration(double & vel, const double & remaining_distance, VelocityState & state); + // Omni-directional docking helper functions + /** + * @brief Compute velocity command for omni-directional robot docking + * @param target Target pose in base_link frame + * @param cmd Output velocity command + * @return true if command is valid + */ + bool computeOmniVelocityCommand( + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Twist & cmd); + VelocityState state_x_; VelocityState state_y_; diff --git a/src/opennav_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/src/opennav_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index f9ced5c..a0fbf91 100644 --- a/src/opennav_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/src/opennav_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -110,6 +112,12 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock protected: void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state); + void detectedDockPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr pose); + void finalPoseNavCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void poseSyncTimerCallback(); + bool processMatchedPose( + const geometry_msgs::msg::PoseStamped & detected_pose, + const geometry_msgs::msg::PoseStamped & matched_final_pose); void resetDockPoseSubscription(); void recordDockPoseHistory(const geometry_msgs::msg::PoseStamped & dock_pose); double computeExternalDockingDist(const double z ); @@ -134,6 +142,11 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock // If subscribed to a detected pose topic, will contain latest message geometry_msgs::msg::PoseStamped detected_dock_pose_; geometry_msgs::msg::PoseStamped detected_dock_pose_prev_; + std::deque detected_dock_pose_queue_; + std::deque final_pose_queue_; + geometry_msgs::msg::PoseStamped processed_dock_pose_cached_; + bool has_processed_dock_pose_cached_{false}; + std::mutex pose_sync_mutex_; // This is the actual dock pose once it has the specified translation/rotation applied // If not subscribed to a topic, this is simply the database dock pose geometry_msgs::msg::PoseStamped dock_pose_; @@ -156,6 +169,14 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock tf2::Quaternion external_detection_rotation_; double external_detection_translation_x_; double external_detection_translation_y_; + int pose_match_max_delta_ms_; + int pose_sync_timer_period_ms_; + rclcpp::TimerBase::SharedPtr pose_sync_timer_; + + static constexpr int kPoseSyncTimerPeriodMinMs = 80; // avoid mis-used param of Timer's period + static constexpr int kPoseSyncTimerPeriodMaxMs = 500; + static constexpr size_t kPoseQueueCap = 100; // max queue size + static constexpr double kPoseQueueMaxAgeSec = 1.0; // stale data retention // Filtering of detected poses std::shared_ptr filter_; diff --git a/src/opennav_docking/opennav_docking/src/controller.cpp b/src/opennav_docking/opennav_docking/src/controller.cpp index e352324..d2babf2 100644 --- a/src/opennav_docking/opennav_docking/src/controller.cpp +++ b/src/opennav_docking/opennav_docking/src/controller.cpp @@ -25,6 +25,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/odometry.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/int16.hpp" using std::hypot; using std::min; @@ -97,6 +98,18 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) [this](const std_msgs::msg::String::SharedPtr msg) { controller_function_ = msg->data; }); + + // Subscribe to dock side selector: 0:+y, 1:+x, 2:-y, 3:-x + dock_side_sub_ = node->create_subscription( + "/robot/dock_side", + rclcpp::QoS(10), + [this](const std_msgs::msg::Int16::SharedPtr msg) { + if (msg->data >= 0 && msg->data <= 3) { + cam_side_ = msg->data; + } else { + RCLCPP_WARN(logger_, "Invalid /robot/dock_side=%d, keeping previous=%d", msg->data, cam_side_); + } + }); } RobotState::RobotState(double x, double y, double theta) { @@ -155,29 +168,122 @@ void Controller::velocityInit(const geometry_msgs::msg::Pose & target) { } bool Controller::computeVelocityCommand( - const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Twist & cmd, bool /*backward*/) { + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Twist & cmd, bool /*backward*/) +{ + // try new method of controller: eliminate perpendicular error first + if ( controller_function_ == "Forklift" ) { + return computeOmniVelocityCommand(target, cmd); + } + else { + // legacy function down below + + double global_distance = sqrt(pow(target.position.x, 2) + pow(target.position.y, 2)); // Target is in base_link frame + double global_angle = tf2::getYaw(target.orientation); + // ? Why + // local_goal_ = globalTolocal(robot_pose_, local_goal_); + double local_angle = atan2(target.position.y, target.position.x); + + publishLocalGoal(); + + if(controller_function_ == "NonStop") { + cmd.linear.x = ExtractVelocity(cmd.linear.x, global_distance, state_x_) * cos(local_angle); + cmd.linear.y = ExtractVelocity(cmd.linear.y, global_distance, state_y_) * sin(local_angle); + cmd.angular.z = getGoalAngle(global_angle); + } else { + cmd.linear.x = ExtractVelocity(cmd.linear.x, global_distance, state_x_) * cos(local_angle); + cmd.linear.y = ExtractVelocity(cmd.linear.y, global_distance, state_y_) * sin(local_angle); + cmd.angular.z = getGoalAngle(global_angle); + } + + return true; + } +} + +bool Controller::computeOmniVelocityCommand( + const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Twist & cmd) { + + double x_local = target.position.x; // Target x in base_link frame + double y_local = target.position.y; // Target y in base_link frame + double yaw_target = tf2::getYaw(target.orientation); - double global_distance = sqrt(pow(target.position.x, 2) + pow(target.position.y, 2)); // Target is in base_link frame - double global_angle = tf2::getYaw(target.orientation); - // ? Why - // local_goal_ = globalTolocal(robot_pose_, local_goal_); - double local_angle = atan2(target.position.y, target.position.x); + double distance = sqrt(pow(x_local, 2) + pow(y_local, 2)); + double target_angle = atan2(y_local, x_local); + RCLCPP_INFO_THROTTLE( + logger_, *clock_, 1000, + "OmniDock input: side=%d, target(x=%.3f,y=%.3f,yaw=%.3f), cmd_in(vx=%.3f,vy=%.3f,w=%.3f), dist=%.3f, target_angle=%.3f", + cam_side_, x_local, y_local, yaw_target, cmd.linear.x, cmd.linear.y, cmd.angular.z, distance, target_angle); + publishLocalGoal(); - if(controller_function_ == "NonStop") { - cmd.linear.x = ExtractVelocity(cmd.linear.x, global_distance, state_x_) * cos(local_angle); - cmd.linear.y = ExtractVelocity(cmd.linear.y, global_distance, state_y_) * sin(local_angle); - cmd.angular.z = getGoalAngle(global_angle); + // Docking direction from /robot/dock_side in robot frame: + // 0:+y, 1:+x, 2:-y, 3:-x + double dock_dir_x = 1.0; + double dock_dir_y = 0.0; + switch (cam_side_) { + case 0: dock_dir_x = 0.0; dock_dir_y = 1.0; break; + case 1: dock_dir_x = 1.0; dock_dir_y = 0.0; break; + case 2: dock_dir_x = 0.0; dock_dir_y = -1.0; break; + case 3: dock_dir_x = -1.0; dock_dir_y = 0.0; break; + default: break; + } + + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "OmniDock basis: dock_dir=(%.1f,%.1f)", dock_dir_x, dock_dir_y); + + // Perpendicular unit vector to dock direction (+pi/2 of dock_dir) + const double perp_x = -dock_dir_y; + const double perp_y = dock_dir_x; + + // Project target into along-dock and perpendicular components + const double along_offset = x_local * dock_dir_x + y_local * dock_dir_y; + const double perp_offset = x_local * perp_x + y_local * perp_y; + + // Angle between target direction and dock direction + const double dock_dir_angle = std::atan2(dock_dir_y, dock_dir_x); + const double angle_diff = angles::shortest_angular_distance(dock_dir_angle, target_angle); + + // Calculate angle threshold from parameter (radians) + double angle_threshold = omni_docking_angle_threshold_; + if (distance < omni_docking_dist_bound_) { + angle_threshold *= 2.0; + } + + RCLCPP_INFO_THROTTLE( + logger_, *clock_, 1000, + "OmniDock projection: along=%.3f, perp=%.3f, dist=%.3f, angle_diff=%.3f, threshold=%.3f, dist_bound=%.3f", + along_offset, perp_offset, distance, angle_diff, angle_threshold, omni_docking_dist_bound_); + + if (std::abs(angle_diff) < angle_threshold) { + // Direct approach: move toward goal while maintaining velocity profile + const double current_speed = std::hypot(cmd.linear.x, cmd.linear.y); + double v_magnitude = ExtractVelocity(current_speed, distance, state_x_); + cmd.linear.x = v_magnitude * cos(target_angle); + cmd.linear.y = v_magnitude * sin(target_angle); + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, + "Omni docking: DIRECT approach, angle_diff=%.3f, v_x=%.3f, v_y=%.3f", + angle_diff, cmd.linear.x, cmd.linear.y); } else { - cmd.linear.x = ExtractVelocity(cmd.linear.x, global_distance, state_x_) * cos(local_angle); - cmd.linear.y = ExtractVelocity(cmd.linear.y, global_distance, state_y_) * sin(local_angle); - cmd.angular.z = getGoalAngle(global_angle); + // Eliminate perpendicular component first, in the correct dock-side frame + const double perp_distance = std::fabs(perp_offset); + const double current_perp_speed = std::fabs(cmd.linear.x * perp_x + cmd.linear.y * perp_y); + const double v_perp = ExtractVelocity(current_perp_speed, perp_distance, state_y_); + const double signed_v_perp = (perp_offset >= 0.0) ? v_perp : -v_perp; // a scalar to times into perp vector + + cmd.linear.x = signed_v_perp * perp_x; + cmd.linear.y = signed_v_perp * perp_y; + RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, + "Omni docking: PERP elimination side=%d, along=%.3f, perp=%.3f, v_x=%.3f, v_y=%.3f", + cam_side_, along_offset, perp_offset, cmd.linear.x, cmd.linear.y); } + // Angular velocity: align with target orientation + cmd.angular.z = getGoalAngle(yaw_target); + + return true; } + bool Controller::computeIfNeedStop(const geometry_msgs::msg::Pose & target) { // Calculate the vector from the robot to the target double target_dx = target.position.x - robot_pose_.x_; @@ -338,6 +444,8 @@ void Controller::declareAllControlParams() {"stop_degree", rclcpp::ParameterValue(45.0)}, {"rival_radius", rclcpp::ParameterValue(0.44)}, {"max_speed_diff", rclcpp::ParameterValue(0.05)}, + {"omni_docking_angle_threshold", rclcpp::ParameterValue(15.0)}, // in degrees + {"omni_docking_dist_bound", rclcpp::ParameterValue(0.05)}, }; for (const auto& profile : profiles_) @@ -368,6 +476,10 @@ void Controller::updateParams() { node_->get_parameter(param_name_ + ".stop_degree", stop_degree_); node_->get_parameter(param_name_ + ".rival_radius", rival_radius_); node_->get_parameter(param_name_ + ".max_speed_diff", max_speed_diff_); + double omni_docking_angle_threshold_deg; + node_->get_parameter(param_name_ + ".omni_docking_angle_threshold", omni_docking_angle_threshold_deg); + omni_docking_angle_threshold_ = angles::from_degrees(omni_docking_angle_threshold_deg); + node_->get_parameter(param_name_ + ".omni_docking_dist_bound", omni_docking_dist_bound_); std::string external_rival_data_path; node_->get_parameter(param_name_ + ".external_rival_data_path", external_rival_data_path); if(!external_rival_data_path.empty()) { diff --git a/src/opennav_docking/opennav_docking/src/nav_type_selector.cpp b/src/opennav_docking/opennav_docking/src/nav_type_selector.cpp index 17d94dc..4db1967 100644 --- a/src/opennav_docking/opennav_docking/src/nav_type_selector.cpp +++ b/src/opennav_docking/opennav_docking/src/nav_type_selector.cpp @@ -63,8 +63,8 @@ void NavTypeSelector::setType(std::string const & mode, char & offset_direction, // Determine the controller function if(strstr(mode.c_str(), "delaySpin") != nullptr) { controller_function_msg_.data = "DelaySpin"; - } else if(strstr(mode.c_str(), "didilong") != nullptr) { - controller_function_msg_.data = "Didilong"; + } else if(strstr(mode.c_str(), "forklift") != nullptr) { + controller_function_msg_.data = "Forklift"; } else if(strstr(mode.c_str(), "nonStop") != nullptr) { controller_function_msg_.data = "NonStop"; } else { diff --git a/src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp b/src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp index 630f8dd..6dac4f6 100644 --- a/src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp @@ -13,9 +13,12 @@ // limitations under the License. #include +#include #include +#include #include #include +#include #include "nav2_util/node_utils.hpp" #include "opennav_docking/simple_charging_dock.hpp" @@ -103,6 +106,10 @@ void SimpleChargingDock::configure( node_, name + ".dock_pose_history_file_path", rclcpp::ParameterValue("")); nav2_util::declare_parameter_if_not_declared( node_, name + ".dock_pose_history_suffix_datetime", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".pose_match_max_delta_ms", rclcpp::ParameterValue(100)); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".pose_sync_timer_period_ms", rclcpp::ParameterValue(20)); node_->get_parameter(name + ".use_battery_status", use_battery_status_); // node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); @@ -130,6 +137,13 @@ void SimpleChargingDock::configure( node_->get_parameter(name + ".base_frame", base_frame_); node_->get_parameter(name + ".dock_pose_history_file_path", dock_pose_history_file_path_); node_->get_parameter(name + ".dock_pose_history_suffix_datetime", dock_pose_history_suffix_datetime_); + node_->get_parameter(name + ".pose_match_max_delta_ms", pose_match_max_delta_ms_); + node_->get_parameter(name + ".pose_sync_timer_period_ms", pose_sync_timer_period_ms_); + + pose_sync_timer_period_ms_ = std::clamp( + pose_sync_timer_period_ms_, + kPoseSyncTimerPeriodMinMs, + kPoseSyncTimerPeriodMaxMs); if (dock_pose_history_file_.is_open()) { dock_pose_history_file_.flush(); @@ -226,13 +240,7 @@ void SimpleChargingDock::configure( dock_pose_sub_ = node_->create_subscription( "detected_dock_pose", qos, - [this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) { - use_external_detection_pose_ = true; - if ( dock_w_cam_ ) { - detected_dock_pose_ = *pose; - detected_dock_pose_prev_ = detected_dock_pose_; - } - }); + std::bind(&SimpleChargingDock::detectedDockPoseCallback, this, std::placeholders::_1)); // Subscribe to dock controller selector dock_controller_selector_sub_ = node_->create_subscription( @@ -249,6 +257,10 @@ void SimpleChargingDock::configure( else { dock_w_cam_ = false; use_external_detection_pose_ = false; + std::scoped_lock lock(pose_sync_mutex_); + detected_dock_pose_queue_.clear(); + final_pose_queue_.clear(); + has_processed_dock_pose_cached_ = false; } RCLCPP_INFO(node_->get_logger(), "Dock controller type changed to: %s, dock_w_cam_: %s", msg->data.c_str(), dock_w_cam_ ? "true" : "false"); @@ -275,9 +287,11 @@ void SimpleChargingDock::configure( // Subscribe to final_pose_nav topic final_pose_nav_sub_ = node_->create_subscription( "/final_pose", 10, - [this](const nav_msgs::msg::Odometry::SharedPtr msg) { - final_pose_nav_ = *msg; - }); + std::bind(&SimpleChargingDock::finalPoseNavCallback, this, std::placeholders::_1)); + + pose_sync_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(pose_sync_timer_period_ms_), + std::bind(&SimpleChargingDock::poseSyncTimerCallback, this)); // Subscribe to dock_side topic dock_side_sub_ = node_->create_subscription( @@ -290,8 +304,249 @@ void SimpleChargingDock::configure( } +void SimpleChargingDock::detectedDockPoseCallback( + const geometry_msgs::msg::PoseStamped::SharedPtr pose) +{ + if (!pose) { + return; + } + + if (!dock_w_cam_) { + return; + } + + use_external_detection_pose_ = true; + + std::scoped_lock lock(pose_sync_mutex_); + if (!detected_dock_pose_queue_.empty()) { + const auto last_stamp = rclcpp::Time(detected_dock_pose_queue_.back().header.stamp); + const auto curr_stamp = rclcpp::Time(pose->header.stamp); + if (curr_stamp <= last_stamp) { + return; + } + } + + detected_dock_pose_ = *pose; + detected_dock_pose_queue_.push_back(*pose); + while (detected_dock_pose_queue_.size() > kPoseQueueCap) { + detected_dock_pose_queue_.pop_front(); + } +} + + +void SimpleChargingDock::finalPoseNavCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + if (!msg) { + return; + } + + final_pose_nav_ = *msg; + + geometry_msgs::msg::PoseStamped final_pose; + final_pose.header = msg->header; + final_pose.pose = msg->pose.pose; + + std::scoped_lock lock(pose_sync_mutex_); + if (!final_pose_queue_.empty()) { + const auto last_stamp = rclcpp::Time(final_pose_queue_.back().header.stamp); + const auto curr_stamp = rclcpp::Time(final_pose.header.stamp); + if (curr_stamp <= last_stamp) { + return; + } + } + + final_pose_queue_.push_back(final_pose); + while (final_pose_queue_.size() > kPoseQueueCap) { + final_pose_queue_.pop_front(); + } +} + + +bool SimpleChargingDock::processMatchedPose( + const geometry_msgs::msg::PoseStamped & detected_pose, + const geometry_msgs::msg::PoseStamped & matched_final_pose) +{ + if (matched_final_pose.header.frame_id.empty()) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: matched final pose has empty frame_id"); + return false; + } + + geometry_msgs::msg::PoseStamped detected = detected_pose; + + if (cam_side_ == 0) { + detected.pose.position.y -= fabs(dock_offset_z_); + } else if (cam_side_ == 1) { + detected.pose.position.x -= fabs(dock_offset_z_); + } else if (cam_side_ == 2) { + detected.pose.position.y += fabs(dock_offset_z_); + } else if (cam_side_ == 3) { + detected.pose.position.x += fabs(dock_offset_z_); + } + + const auto & target_frame = matched_final_pose.header.frame_id; + if (detected.header.frame_id != target_frame) { + try { + if (!tf2_buffer_->canTransform( + target_frame, detected.header.frame_id, + detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) + { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: cannot transform detected pose from %s to %s at stamp=%.3f", + detected.header.frame_id.c_str(), + target_frame.c_str(), + rclcpp::Time(detected.header.stamp).seconds()); + return false; + } + tf2_buffer_->transform(detected, detected, target_frame); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: TF transform failed from %s to %s: %s", + detected.header.frame_id.c_str(), + target_frame.c_str(), + ex.what()); + return false; + } + } + + detected = filter_->update(detected); + filtered_dock_pose_pub_->publish(detected); + + geometry_msgs::msg::PoseStamped processed; + processed.header = detected.header; + processed.header.stamp = matched_final_pose.header.stamp; + processed.pose.position = detected.pose.position; + + if (!ignore_detected_orientation_) { + geometry_msgs::msg::PoseStamped just_orientation; + just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation = detected.pose.orientation; + tf2::doTransform(just_orientation, just_orientation, transform); + + tf2::Quaternion orientation; + orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); + processed.pose.orientation = tf2::toMsg(orientation); + } else { + processed.pose.orientation = matched_final_pose.pose.orientation; + } + + { + std::scoped_lock lock(pose_sync_mutex_); + processed_dock_pose_cached_ = processed; + has_processed_dock_pose_cached_ = true; + } + + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "pose_sync: cached refined dock pose (detected stamp=%.3f, matched final stamp=%.3f, frame=%s)", + rclcpp::Time(detected_pose.header.stamp).seconds(), + rclcpp::Time(matched_final_pose.header.stamp).seconds(), + matched_final_pose.header.frame_id.c_str()); + + return true; +} + + +void SimpleChargingDock::poseSyncTimerCallback() +{ + if (!dock_w_cam_) { + return; + } + + geometry_msgs::msg::PoseStamped newest_detected; + geometry_msgs::msg::PoseStamped matched_final; + + { + std::scoped_lock lock(pose_sync_mutex_); + + if (detected_dock_pose_queue_.empty() || final_pose_queue_.empty()) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: waiting for data (detected queue=%zu, final queue=%zu)", + detected_dock_pose_queue_.size(), final_pose_queue_.size()); + return; + } + + const auto now = node_->now(); + const auto max_age = rclcpp::Duration::from_seconds(kPoseQueueMaxAgeSec); + while (!detected_dock_pose_queue_.empty() && + (now - rclcpp::Time(detected_dock_pose_queue_.front().header.stamp)) > max_age) + { + detected_dock_pose_queue_.pop_front(); + } + while (!final_pose_queue_.empty() && + (now - rclcpp::Time(final_pose_queue_.front().header.stamp)) > max_age) + { + final_pose_queue_.pop_front(); + } + + if (detected_dock_pose_queue_.empty() || final_pose_queue_.empty()) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: data became stale after pruning (detected queue=%zu, final queue=%zu)", + detected_dock_pose_queue_.size(), final_pose_queue_.size()); + return; + } + + newest_detected = detected_dock_pose_queue_.back(); + const auto detected_stamp_ns = rclcpp::Time(newest_detected.header.stamp).nanoseconds(); + const int64_t max_delta_ns = static_cast(pose_match_max_delta_ms_) * 1000000LL; + + size_t best_idx = final_pose_queue_.size(); + int64_t best_delta_ns = std::numeric_limits::max(); + for (size_t i = 0; i < final_pose_queue_.size(); ++i) { + const int64_t final_stamp_ns = rclcpp::Time(final_pose_queue_[i].header.stamp).nanoseconds(); + const int64_t delta_ns = std::llabs(final_stamp_ns - detected_stamp_ns); + if (delta_ns < best_delta_ns) { + best_delta_ns = delta_ns; + best_idx = i; + } + } + + if (best_idx == final_pose_queue_.size() || best_delta_ns > max_delta_ns) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: no timestamp match within %d ms (best delta=%.1f ms, detected stamp=%.3f)", + pose_match_max_delta_ms_, + static_cast(best_delta_ns) / 1.0e6, + rclcpp::Time(newest_detected.header.stamp).seconds()); + return; + } + + matched_final = final_pose_queue_[best_idx]; + for (size_t i = 0; i <= best_idx && !final_pose_queue_.empty(); ++i) { + final_pose_queue_.pop_front(); + } + } + + if (!processMatchedPose(newest_detected, matched_final)) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "pose_sync: matched pair processing failed (detected frame=%s, final frame=%s)", + newest_detected.header.frame_id.c_str(), + matched_final.header.frame_id.c_str()); + } +} + + void SimpleChargingDock::cleanup() { + if (pose_sync_timer_) { + pose_sync_timer_->cancel(); + pose_sync_timer_.reset(); + } + + { + std::scoped_lock lock(pose_sync_mutex_); + detected_dock_pose_queue_.clear(); + final_pose_queue_.clear(); + has_processed_dock_pose_cached_ = false; + } + if (dock_pose_history_file_.is_open()) { dock_pose_history_file_.flush(); dock_pose_history_file_.close(); @@ -332,9 +587,13 @@ void SimpleChargingDock::recordDockPoseHistory(const geometry_msgs::msg::PoseSta geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose( const geometry_msgs::msg::Pose & pose, const std::string & frame, const std::string & dock_type) { + { + std::scoped_lock lock(pose_sync_mutex_); + detected_dock_pose_queue_.clear(); + final_pose_queue_.clear(); + has_processed_dock_pose_cached_ = false; + } - // Keep data durable if docking is interrupted unexpectedly. - dock_pose_history_file_.flush(); // reset_flag_ = false; // reset_timer_flag_ = false; if (dock_type.find("cam") != std::string::npos) { @@ -405,6 +664,42 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) if ( !dock_w_cam_ ) { use_external_detection_pose_ = false; } + + if (dock_w_cam_) { + geometry_msgs::msg::PoseStamped cached_pose; + bool has_cached = false; + { + std::scoped_lock lock(pose_sync_mutex_); + has_cached = has_processed_dock_pose_cached_; + if (has_cached) { + cached_pose = processed_dock_pose_cached_; + } + } + + if (has_cached && (node_->now() - cached_pose.header.stamp) <= + rclcpp::Duration::from_seconds(external_detection_timeout_)) + { + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "getRefinedPose: using synced detected dock pose (stamp=%.3f, frame=%s)", + rclcpp::Time(cached_pose.header.stamp).seconds(), + cached_pose.header.frame_id.c_str()); + + dock_pose_ = cached_pose; + detected_dock_pose_prev_ = cached_pose; + dock_pose_pub_->publish(dock_pose_); + pose = dock_pose_; + recordDockPoseHistory(dock_pose_); + return true; + } + + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 2000, + "getRefinedPose: no fresh synced detected pose available; fallback to previous/static pose"); + + use_external_detection_pose_ = false; + } + // ** If using not detection, set the dock pose to the static fixed-frame version if (!use_external_detection_pose_) { if(detected_dock_pose_prev_.header.frame_id.empty()) { @@ -418,102 +713,7 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) return true; } - // If using detections, get current detections, transform to frame, and apply offsets - geometry_msgs::msg::PoseStamped detected = detected_dock_pose_; - - RCLCPP_INFO(node_->get_logger(), "raw Dock pose: frame=%s, pos=(%.3f, %.3f, %.3f), yaw=%.3f", - detected.header.frame_id.c_str(), - detected.pose.position.x, - detected.pose.position.y, - detected.pose.position.z, - tf2::getYaw(detected.pose.orientation)); - - // ** Validate that external pose is new enough - auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); - if (node_->now() - detected.header.stamp > timeout) { - RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); - use_external_detection_pose_ = false; // experimental - dock_pose_pub_->publish(detected_dock_pose_prev_); - dock_pose_ = detected_dock_pose_prev_; - recordDockPoseHistory(dock_pose_); - return true; - } - - // Apply z-offset to move dock_pose away from detected pose before transform - // Use stored dock_offset_z_ value from original goal - if ( cam_side_ == 0 ) { // dock toward +y - detected.pose.position.y -= fabs(dock_offset_z_); - } - else if ( cam_side_ == 1 ) { // +x - detected.pose.position.x -= fabs(dock_offset_z_); - } - else if ( cam_side_ == 2 ) { // -y - detected.pose.position.y += fabs(dock_offset_z_); - } - else if ( cam_side_ == 3 ) { // -x - detected.pose.position.x += fabs(dock_offset_z_); - } - - // Transform detected pose into fixed frame. Note that the argument pose - // is the output of detection, but also acts as the initial estimate - // and contains the frame_id of docking - if (detected.header.frame_id != pose.header.frame_id) { - try { - if (!tf2_buffer_->canTransform( - pose.header.frame_id, detected.header.frame_id, - detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) - { - RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); - return false; - } - tf2_buffer_->transform(detected, detected, pose.header.frame_id); - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); - return false; - } - } - - // Filter the detected pose - detected = filter_->update(detected); - filtered_dock_pose_pub_->publish(detected); - - // Construct dock_pose_ header and position - - dock_pose_.header = detected.header; - dock_pose_.pose.position = detected.pose.position; - - // Process orientation unless flag is set to ignore it - if (!ignore_detected_orientation_) { - // Rotate the just the orientation, then remove roll/pitch - geometry_msgs::msg::PoseStamped just_orientation; - just_orientation.pose.orientation = tf2::toMsg(external_detection_rotation_); - geometry_msgs::msg::TransformStamped transform; - transform.transform.rotation = detected.pose.orientation; - tf2::doTransform(just_orientation, just_orientation, transform); - - tf2::Quaternion orientation; - orientation.setEuler(0.0, 0.0, tf2::getYaw(just_orientation.pose.orientation)); - dock_pose_.pose.orientation = tf2::toMsg(orientation); - } else { - // Keep the original orientation from the initial pose estimate - dock_pose_.pose.orientation = pose.pose.orientation; - } - - - - // Publish & return dock pose for debugging purposes - RCLCPP_INFO(node_->get_logger(), "transformed Dock pose: frame=%s, pos=(%.3f, %.3f, %.3f), yaw=%.3f", - dock_pose_.header.frame_id.c_str(), - dock_pose_.pose.position.x, - dock_pose_.pose.position.y, - dock_pose_.pose.position.z, - tf2::getYaw(dock_pose_.pose.orientation)); - dock_pose_pub_->publish(dock_pose_); - pose = dock_pose_; - recordDockPoseHistory(dock_pose_); - use_external_detection_pose_ = false; - detected_dock_pose_prev_ = dock_pose_; // Update with processed pose - return true; + return false; } bool SimpleChargingDock::isDocked()