Add allow_unknown parameter to theta star planner (#3286)

* Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify
ros-navigation · Dec 21, 2022 · fa44682 · fa44682
1 parent 16c9b12
commit fa44682
Show file tree

Hide file tree

Showing 4 changed files with 64 additions and 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
@@ -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_);
@@ -215,6 +219,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
@@ -185,3 +185,49 @@ TEST(ThetaStarPlanner, test_theta_star_planner) {
   life_node.reset();
   costmap_ros.reset();
 }
+
+TEST(ThetaStarPlanner, test_theta_star_reconfigure)
+{
+  rclcpp_lifecycle::LifecycleNode::SharedPtr life_node =
+    std::make_shared<rclcpp_lifecycle::LifecycleNode>("ThetaStarPlannerTest");
+
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
+    std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+
+  auto planner = std::make_unique<nav2_theta_star_planner::ThetaStarPlanner>();
+  try {
+    // Expect to throw due to invalid prims file in param
+    planner->configure(life_node, "test", nullptr, costmap_ros);
+  } catch (...) {
+  }
+  planner->activate();
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    life_node->get_node_base_interface(), life_node->get_node_topics_interface(),
+    life_node->get_node_graph_interface(),
+    life_node->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {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.allow_unknown", false)});
+
+  rclcpp::spin_until_future_complete(
+    life_node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(life_node->get_parameter("test.how_many_corners").as_int(), 8);
+  EXPECT_EQ(
+    life_node->get_parameter("test.w_euc_cost").as_double(),
+    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(),
+    results);
+}