From 33dafaeb2b01ff18d3e3490d8831ca34d2aeb993 Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 15 Nov 2022 23:28:59 +0000 Subject: [PATCH 1/5] Add allow unknown parameter to theta star planner --- .../include/nav2_theta_star_planner/theta_star.hpp | 7 +++++-- nav2_theta_star_planner/src/theta_star.cpp | 1 + nav2_theta_star_planner/src/theta_star_planner.cpp | 6 ++++++ 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index dacd8a57bc3..84adfea22b4 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -25,6 +25,7 @@ const double INF_COST = DBL_MAX; const int LETHAL_COST = 252; +const int UNKNOWN_COST = 255; struct coordsM { @@ -70,6 +71,8 @@ class ThetaStar double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; + /// parameter to set weather the planner can plan through unknown space + bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; @@ -91,7 +94,7 @@ class ThetaStar */ inline bool isSafe(const int & cx, const int & cy) const { - return costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; } /** @@ -185,7 +188,7 @@ class ThetaStar bool isSafe(const int & cx, const int & cy, double & cost) const { double curr_cost = getCost(cx, cy); - if (curr_cost < LETHAL_COST) { + if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; } else { diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index e0267b34c0e..baddc754d7a 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -23,6 +23,7 @@ ThetaStar::ThetaStar() w_euc_cost_(2.0), w_heuristic_cost_(1.0), how_many_corners_(8), + allow_unknown_(true), size_x_(0), size_y_(0), index_generated_(0) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 9f4930da726..c509afb065c 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -45,6 +45,10 @@ void ThetaStarPlanner::configure( RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); } + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + ".allow_unknown", planner_->allow_unknown_); + nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); @@ -237,6 +241,8 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param } else if (type == ParameterType::PARAMETER_BOOL) { if (name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); + } else if (name == name_ + ".allow_unknown") { + planner_->allow_unknown_ = parameter.as_bool(); } } } From 33dfb23126c562eecef45e5c1a452abf51b36d3d Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 16 Nov 2022 00:51:41 +0000 Subject: [PATCH 2/5] Add allow unknown parameter to tests --- nav2_theta_star_planner/test/test_theta_star.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 44b37f4ea77..8346e8c868b 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -212,7 +212,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) {rclcpp::Parameter("test.how_many_corners", 8), rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), - rclcpp::Parameter("test.use_final_approach_orientation", false)}); + rclcpp::Parameter("test.use_final_approach_orientation", false) + rclcpp::Parameter("test.allow_unknown", false)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -224,6 +225,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) 1.0); EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), From 0638bed96d22e4e1423235ba50a6c9161e76c6a5 Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 16 Nov 2022 01:01:04 +0000 Subject: [PATCH 3/5] missing comma --- nav2_theta_star_planner/test/test_theta_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 8346e8c868b..c2360d9f871 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -212,7 +212,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) {rclcpp::Parameter("test.how_many_corners", 8), rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), - rclcpp::Parameter("test.use_final_approach_orientation", false) + rclcpp::Parameter("test.use_final_approach_orientation", false), rclcpp::Parameter("test.allow_unknown", false)}); rclcpp::spin_until_future_complete( From f4a16da4288eca02a2379e5bf8e1a1c005894222 Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 16 Nov 2022 14:38:14 +0000 Subject: [PATCH 4/5] Change cost of unknown tiles --- .../include/nav2_theta_star_planner/theta_star.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 84adfea22b4..1a8c572d73c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,8 +24,9 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; -const int LETHAL_COST = 252; const int UNKNOWN_COST = 255; +const int OBS_COST = 254; +const int LETHAL_COST = 252; struct coordsM { @@ -189,6 +190,9 @@ class ThetaStar { double curr_cost = getCost(cx, cy); if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { + if(costmap_->getCost(cx, cy) == UNKNOWN_COST) { + curr_cost = OBS_COST - 1; + } cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; } else { From 198078d97f4311befd5f3e2d4b947ac61b48ff3c Mon Sep 17 00:00:00 2001 From: pepisg Date: Wed, 16 Nov 2022 20:48:37 +0000 Subject: [PATCH 5/5] Uncrustify --- .../include/nav2_theta_star_planner/theta_star.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 1a8c572d73c..d1ddf7354ce 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -95,7 +95,9 @@ class ThetaStar */ inline bool isSafe(const int & cx, const int & cy) const { - return (costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost( + cx, + cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; } /** @@ -190,7 +192,7 @@ class ThetaStar { double curr_cost = getCost(cx, cy); if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { - if(costmap_->getCost(cx, cy) == UNKNOWN_COST) { + if (costmap_->getCost(cx, cy) == UNKNOWN_COST) { curr_cost = OBS_COST - 1; } cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;