Merge branch 'fix-narrow-path-navigation' into main

Signed-off-by: Daisuke Sato <[email protected]>
CMU-cabot · Feb 22, 2024 · 640f41a · 640f41a
2 parents 7a3c1d4 + dd5dc18
commit 640f41a
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 23 deletions.
diff --git a/cabot/src/safety/lidar_speed_control_node.cpp b/cabot/src/safety/lidar_speed_control_node.cpp
@@ -111,6 +111,9 @@ class LiDARSpeedControlNode : public rclcpp::Node
       if (param.get_name() == "check_blind_space") {
         check_blind_space_ = param.as_bool();
       }
+      if (param.get_name() == "min_distance") {
+        min_distance_ = param.as_double();
+      }
     }
     results->successful = true;
     return *results;

diff --git a/cabot_bt/behavior_trees/navigate_for_little_narrow.xml b/cabot_bt/behavior_trees/navigate_for_little_narrow.xml
@@ -51,19 +51,23 @@
         <ChangeParam node_name="/planner_server"
                      param_name="CaBot.path_adjusted_center"
                      param_value="0.0" store="params"  name="ChangeParamPathAdjustedCenter3"/>
-
-	    <ChangeParam node_name="/controller_server"
-					 param_name="FollowPath.max_vel_x"
-					 param_value="0.5" store="params" name="ChangeParamSpeedDown"/>
-
-	    <ChangeParam node_name="/global_costmap/global_costmap"
-					 param_name="people_obstacle_layer.people_enabled"
-					 param_value="false" store="params" name="DisablePeopleObstacle"/>
-
-		<ChangeParam node_name="/footprint_publisher"
-					 param_name="footprint_mode"
-					 param_value="3" store="params" name="ChangeParamToNormal"/>
-
+
+        <ChangeParam node_name="/controller_server"
+                     param_name="FollowPath.max_vel_x"
+                     param_value="0.5" store="params" name="ChangeParamSpeedDown"/>
+
+        <ChangeParam node_name="/global_costmap/global_costmap"
+                     param_name="people_obstacle_layer.people_enabled"
+                     param_value="false" store="params" name="DisablePeopleObstacle"/>
+
+        <ChangeParam node_name="/footprint_publisher"
+                     param_name="footprint_mode"
+                     param_value="3" store="params" name="ChangeParamToNormal"/>
+
+        <ChangeParam node_name="/cabot/lidar_speed_control_node"
+                     param_name="min_distance"
+                     param_value="0.25" store="params" name="ChangeLidarSpeedControlMinDistance"/>
+
         <!-- clear global costmap -->
         <RecoveryNode number_of_retries="999999999" name="InitialComputePathRepeat">
           <Sequence name="ComputePathSequence">

diff --git a/cabot_bt/behavior_trees/navigate_for_narrow.xml b/cabot_bt/behavior_trees/navigate_for_narrow.xml
@@ -52,13 +52,17 @@
                      param_name="CaBot.path_adjusted_center"
                      param_value="0.0" store="params"  name="ChangeParamPathAdjustedCenter3"/>
 
-		<ChangeParam node_name="/controller_server"
-			 param_name="FollowPath.max_vel_x"
-			 param_value="0.5" store="params" name="ChangeParamSpeedDown"/>
-
-	    <ChangeParam node_name="/global_costmap/global_costmap"
-			 param_name="people_obstacle_layer.people_enabled"
-			 param_value="false" store="params" name="DisablePeopleObstacle"/>
+        <ChangeParam node_name="/controller_server"
+                     param_name="FollowPath.max_vel_x"
+                     param_value="0.5" store="params" name="ChangeParamSpeedDown"/>
+
+        <ChangeParam node_name="/global_costmap/global_costmap"
+                     param_name="people_obstacle_layer.people_enabled"
+                     param_value="false" store="params" name="DisablePeopleObstacle"/>
+
+        <ChangeParam node_name="/cabot/lidar_speed_control_node"
+                     param_name="min_distance"
+                     param_value="0.25" store="params" name="ChangeLidarSpeedControlMinDistance"/>
 
         <!-- clear global costmap -->
         <RecoveryNode number_of_retries="999999999" name="InitialComputePathRepeat">

diff --git a/cabot_navigation2/plugins/cabot_planner_param.cpp b/cabot_navigation2/plugins/cabot_planner_param.cpp
@@ -149,6 +149,9 @@ void CaBotPlan::findIndex()
       }
     }
   }
+  if (nodes.size() < end_index) {
+    end_index = nodes.size();
+  }
   /*
   if (start_index < (uint64_t)(5.0/options_.initial_node_interval)) {
     start_index = 0;
@@ -164,7 +167,7 @@ void CaBotPlan::adjustNodeInterval()
   bool changed = false;
   int devide_link_cell_interval_threshold = param.options.initial_node_interval * 2.0 / param.resolution;
   int shrink_link_cell_interval_threshold = param.options.initial_node_interval / 2.0 / param.resolution;
-  for (uint64_t i = 0; i < nodes.size() - 1; i++) {
+  for (uint64_t i = 0; i < nodes.size() - 2; i++) {
     if (nodes_backup.size() * 2 < nodes.size()) {
       break;
     }
@@ -564,13 +567,15 @@ std::vector<Node> CaBotPlannerParam::getNodes() const
   auto p1 = path.poses[0].pose.position;
   float mx = 0, my = 0;
   for (uint64_t i = 1; i < path.poses.size(); i++) {
+    worldToMap(p0.x, p0.y, mx,  my);
+    nodes.push_back(Node(mx, my));
     p1 = path.poses[i].pose.position;
 
     auto dist = std::hypot(p0.x - p1.x, p0.y - p1.y);
     int N = std::round(dist / initial_node_interval);
     if (N == 0) {continue;}
 
-    for (int j = 0; j <= N; j++) {
+    for (int j = 1; j <= N; j++) {
       // deal with the last pose
       if (j == N && i < path.poses.size() - 1) {
         continue;

diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py
@@ -353,7 +353,7 @@ def convert(g, a=anchor):
             path.poses.append(pose)
 
     CaBotRclpyUtil.info(F"path {path}")
-    return (path, path.poses[-1])
+    return (path, path.poses[-1] if len(path.poses) > 0 else None)
 
 
 class Goal(geoutil.TargetPlace):