Fix/controller escape

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

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 <Nop> " '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
+

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;

src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml

@@ -18,11 +18,11 @@
             </RetryUntilSuccessful>
             <ReactiveFallback name="RecoveryFallback">
               <GoalUpdated/>
-              <RoundRobin name="RecoveryActions">
+              <PersistentSequenceWContOnFail current_child_idx="{current_child_idx}">
                 <Shrink shrink_to="0.05"/>
-                <Wait wait_duration="0.5"/>
+                <!-- <Wait wait_duration="0.1"/> -->
                 <Escape run_dist="5" time_allowance="5" is_recovery="1"/>
-              </RoundRobin>
+              </PersistentSequenceWContOnFail>
             </ReactiveFallback>
           </RecoveryNode>
         </ResetRecovery>

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

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<NavigateToPose>;
 
     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<double>::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<tf2_ros::Buffer> tf_;
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
     std::string name_;
+    std::string external_rival_data_path_;
     rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr global_costmap_sub_;
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr rival_pose_sub_;
 
@@ -122,35 +168,38 @@ class TebController : public nav2_core::Controller
     // pubs
     rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr teb_path_pub_;
     rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr global_plan_pub_;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr goal_reached_pub_;
+    rclcpp_action::Client<NavigateToPose>::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};
     double k_xy_{2.5};
     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_
+#endif  // TEB_CONTROLLER__TEB_CONTROLLER_HPP_

src/custom_controller/package.xml

@@ -11,11 +11,17 @@
 
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_msgs</depend>
   <depend>nav2_costmap_2d</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>yaml-cpp</depend>
 
   <test_depend>ament_lint_auto</test_depend>