merge NarrowGoal into NavGoal

Signed-off-by: Daisuke Sato <[email protected]>
CMU-cabot · Mar 19, 2024 · 5b8761a · 5b8761a
1 parent 2f52b2f
commit 5b8761a
Show file tree

Hide file tree

Showing 7 changed files with 349 additions and 271 deletions.
diff --git a/cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml b/cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml
diff --git a/cabot_bt/behavior_trees/navigation.xml b/cabot_bt/behavior_trees/navigation.xml
diff --git a/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp b/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp
@@ -63,6 +63,7 @@ struct CaBotPlannerOptions
   int kdtree_max_results = 50;
   int min_iteration_count = 500;
   int max_iteration_count = 1000;
+  bool ignore_obstacles = false;
 
   // private params
   float iteration_scale_min = 0.0001;

diff --git a/cabot_navigation2/plugins/cabot_planner.cpp b/cabot_navigation2/plugins/cabot_planner.cpp
@@ -174,6 +174,11 @@ void CaBotPlanner::configure(
     rclcpp::ParameterValue(defaultValue.max_iteration_count));
   node->get_parameter(name + ".max_iteration_count", options_.max_iteration_count);
 
+  declare_parameter_if_not_declared(
+    node, name + ".ignore_obstacles",
+    rclcpp::ParameterValue(defaultValue.ignore_obstacles));
+  node->get_parameter(name + ".ignore_obstacles", options_.ignore_obstacles);
+
   declare_parameter_if_not_declared(node, name + ".static_layer_name", rclcpp::ParameterValue("static_layer"));
   node->get_parameter(name + ".static_layer_name", static_layer_name_);
 
@@ -410,6 +415,9 @@ rcl_interfaces::msg::SetParametersResult CaBotPlanner::param_set_callback(const
     if (param.get_name() == name_ + ".max_iteration_count") {
       options_.max_iteration_count = param.as_int();
     }
+    if (param.get_name() == name_ + ".ignore_obstacles") {
+      options_.ignore_obstacles = param.as_bool();
+    }
 
     // private
     if (param.get_name() == name_ + ".private.iteration_scale_min") {
@@ -532,8 +540,14 @@ nav_msgs::msg::Path CaBotPlanner::createPlan(CaBotPlannerParam & param)
 
   int total_count = 0;
   DetourMode modes[3] = {DetourMode::RIGHT, DetourMode::LEFT, DetourMode::IGNORE};
+  int i = 0;
+  if (param.options.ignore_obstacles) {
+    plans[0].okay = false;
+    plans[1].okay = false;
+    i = 2;
+  }
   rclcpp::Rate r(1);
-  for (int i = 0; i < 3; i++) {
+  for (; i < 3; i++) {
     CaBotPlan & plan = plans[i];
     plan.detour_mode = modes[i];
 

diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py
@@ -28,6 +28,7 @@
 # -*- coding: utf-8 -*-
 import sys
 import traceback
+import enum
 import copy
 import math
 import json
@@ -381,6 +382,20 @@ def copy(self):
         return copy.deepcopy(self)
 
 
+class NavigationMode(enum.Enum):
+    Standard = 0
+    Narrow = 1
+    Tight = 2
+
+    @classmethod
+    def get_mode(cls, width):
+        if width < 1.0:
+            return NavigationMode.Tight
+        if width < 1.2:
+            return NavigationMode.Narrow
+        return NavigationMode.Standard
+
+
 class Link(Object):
     """Link class"""
     ROUTE_TYPE_WALKWAY = 1
@@ -391,8 +406,6 @@ class Link(Object):
     ROUTE_TYPE_STAIRS = 6
     ROUTE_TYPE_SLOPE = 7
     ROUTE_TYPE_UNKNOWN = 99
-    ROUTE_NARROW_PATH_THRESHOLD = 1.2
-    ROUTE_NARROW_ANNOUNCE_THRESHOLD = 1.0
 
     @classmethod
     def marshal(cls, dic):
@@ -445,14 +458,8 @@ def is_leaf(self):
         return self.start_node.is_leaf or self.end_node.is_leaf
 
     @property
-    def is_narrow(self):
-        """wheather this links is narrow or not"""
-        return self.properties.hulop_road_width < Link.ROUTE_NARROW_PATH_THRESHOLD
-
-    @property
-    def need_narrow_announce(self):
-        """wheather this links is narrow or not"""
-        return self.properties.hulop_road_width < Link.ROUTE_NARROW_ANNOUNCE_THRESHOLD
+    def navigation_mode(self):
+        return NavigationMode.get_mode(self.properties.hulop_road_width)
 
     @property
     def length(self):