From 30141f6264089ca2b3e18867f0c8561cadaa4f16 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 21 Feb 2024 16:40:05 -0500 Subject: [PATCH 1/3] change min_distance of lidar speed controler in narrow BTs Signed-off-by: Daisuke Sato --- cabot/src/safety/lidar_speed_control_node.cpp | 3 ++ .../navigate_for_little_narrow.xml | 30 +++++++++++-------- .../behavior_trees/navigate_for_narrow.xml | 18 ++++++----- 3 files changed, 31 insertions(+), 20 deletions(-) diff --git a/cabot/src/safety/lidar_speed_control_node.cpp b/cabot/src/safety/lidar_speed_control_node.cpp index fb5a5d2a..71194852 100644 --- 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 index 49764730..ebeab1c6 100644 --- a/cabot_bt/behavior_trees/navigate_for_little_narrow.xml +++ b/cabot_bt/behavior_trees/navigate_for_little_narrow.xml @@ -51,19 +51,23 @@ - - - - - - - + + + + + + + + + diff --git a/cabot_bt/behavior_trees/navigate_for_narrow.xml b/cabot_bt/behavior_trees/navigate_for_narrow.xml index 8d3cc1a9..eb1ff4d1 100644 --- 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"/> - - - + + + + + From 5ecb26646eae200df54b669024467de84122cbdd Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 21 Feb 2024 16:40:55 -0500 Subject: [PATCH 2/3] bugfix: cabot planner dies when the goal is too close to the current position Signed-off-by: Daisuke Sato --- cabot_navigation2/plugins/cabot_planner_param.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cabot_navigation2/plugins/cabot_planner_param.cpp b/cabot_navigation2/plugins/cabot_planner_param.cpp index 937bc1e1..e34c9d92 100644 --- 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() - 1; + } /* if (start_index < (uint64_t)(5.0/options_.initial_node_interval)) { start_index = 0; From dd5dc18780ed25ad22da92a2193c80dfd6e0f38d Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Thu, 22 Feb 2024 14:39:26 -0500 Subject: [PATCH 3/3] bugfix: it can stack while navigation due to zero length plan Signed-off-by: Daisuke Sato --- cabot_navigation2/plugins/cabot_planner_param.cpp | 8 +++++--- cabot_ui/cabot_ui/navgoal.py | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/cabot_navigation2/plugins/cabot_planner_param.cpp b/cabot_navigation2/plugins/cabot_planner_param.cpp index e34c9d92..bcc842b5 100644 --- a/cabot_navigation2/plugins/cabot_planner_param.cpp +++ b/cabot_navigation2/plugins/cabot_planner_param.cpp @@ -150,7 +150,7 @@ void CaBotPlan::findIndex() } } if (nodes.size() < end_index) { - end_index = nodes.size() - 1; + end_index = nodes.size(); } /* if (start_index < (uint64_t)(5.0/options_.initial_node_interval)) { @@ -167,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; } @@ -567,13 +567,15 @@ std::vector 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 index 8639befd..85da9775 100644 --- 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):