Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify
StratomInc · Feb 23, 2023 · 432d90c · 432d90c
1 parent 75e7019
commit 432d90c
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 3 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
@@ -24,6 +24,8 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 const double INF_COST = DBL_MAX;
+const int UNKNOWN_COST = 255;
+const int OBS_COST = 254;
 const int LETHAL_COST = 252;
 
 struct coordsM
@@ -70,6 +72,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 +95,9 @@ 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 +191,10 @@ 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) {
+      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 {

diff --git 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
@@ -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<rclcpp::Parameter> 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();
       }
     }
   }

diff --git 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(),