Fix API - planners and cost calculators

suchetanrs · suchetanrs · commit a474d239087f · 2025-10-11T23:11:31.000+13:00
diff --git a/roadmap_explorer/CMakeLists.txt b/roadmap_explorer/CMakeLists.txt
@@ -169,13 +169,13 @@ add_subdirectory(comparision_scripts)
 ########################### INSTALLATION ###########################
 
 install(TARGETS
-  # roadmap_exploration_bt
   ${PROJECT_NAME}_frontier
   ${PROJECT_NAME}_utils
   roadmap_explorer_frontier_search_plugins
   roadmap_explorer_information_gain_plugins
+  roadmap_explorer_planner_plugins
+  # roadmap_exploration_bt
   # roadmap_explorer_bt_plugins
-  # roadmap_explorer_planner_plugins
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -195,13 +195,13 @@ DESTINATION share/${PROJECT_NAME}
 
 ament_export_include_directories(include ${EIGEN3_INCLUDE_DIR})
 ament_export_libraries(
-# roadmap_exploration_bt 
 ${PROJECT_NAME}_frontier 
 ${PROJECT_NAME}_utils 
 roadmap_explorer_frontier_search_plugins 
 roadmap_explorer_information_gain_plugins
+roadmap_explorer_planner_plugins 
+# roadmap_exploration_bt 
 # roadmap_explorer_bt_plugins 
-# roadmap_explorer_planner_plugins 
 )
 ament_export_dependencies(rosidl_default_runtime ${dependencies})
 
diff --git a/roadmap_explorer/include/roadmap_explorer/CostAssigner.hpp b/roadmap_explorer/include/roadmap_explorer/CostAssigner.hpp
@@ -69,8 +69,6 @@ class CostAssigner
 
   void recomputeNormalizationFactors(FrontierPtr & frontier);
 
-  std::vector<FrontierPtr> findDuplicates(const std::vector<FrontierPtr> & vec);
-
   void initializePlugins(const std::string & informationGainPlugin, const std::string & plannerPlugin);
 
   void processFrontier(FrontierPtr & frontier,
diff --git a/roadmap_explorer/include/roadmap_explorer/frontier_search/BaseFrontierSearch.hpp b/roadmap_explorer/include/roadmap_explorer/frontier_search/BaseFrontierSearch.hpp
@@ -48,7 +48,7 @@ class FrontierSearchBase
    * @param costmap Pointer to the costmap for frontier detection.
    * @param node Shared pointer to the lifecycle node for parameter management
    */
-  virtual void configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
+  virtual void configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
 
   /**
    * @brief Reset the frontier search state
diff --git a/roadmap_explorer/include/roadmap_explorer/frontier_search/PluginBFSearch.hpp b/roadmap_explorer/include/roadmap_explorer/frontier_search/PluginBFSearch.hpp
@@ -33,7 +33,7 @@ class FrontierBFSearch : public FrontierSearchBase
 
   ~FrontierBFSearch() override;
 
-  void configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+  void configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   void reset() override;
 
@@ -72,6 +72,8 @@ class FrontierBFSearch : public FrontierSearchBase
     return (int)value < lethal_threshold_;
   }
 
+  std::vector<FrontierPtr> findDuplicates(const std::vector<FrontierPtr> & vec);
+
   // Constants for geometric calculations
   static constexpr double DIAGONAL_FACTOR = 1.414;  // sqrt(2) approximation
   static constexpr double CENTROID_OFFSET_MULTIPLIER = 2.0;
diff --git a/roadmap_explorer/include/roadmap_explorer/information_gain/BaseInformationGain.hpp b/roadmap_explorer/include/roadmap_explorer/information_gain/BaseInformationGain.hpp
@@ -41,7 +41,7 @@ class BaseInformationGain
    * @param explore_costmap_ros Shared pointer to the costmap ROS wrapper
    */
   virtual void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
 
   /**
    * @brief Reset the information gain calculator state
diff --git a/roadmap_explorer/include/roadmap_explorer/information_gain/CountBasedGain.hpp b/roadmap_explorer/include/roadmap_explorer/information_gain/CountBasedGain.hpp
@@ -45,7 +45,7 @@ class CountBasedGain : public BaseInformationGain
    * @param explore_costmap_ros Shared pointer to the costmap ROS wrapper
    */
   void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   /**
    * @brief Reset the calculator state
diff --git a/roadmap_explorer/include/roadmap_explorer/planners/BasePlanner.hpp b/roadmap_explorer/include/roadmap_explorer/planners/BasePlanner.hpp
@@ -41,7 +41,7 @@ class BasePlanner
    * @param explore_costmap_ros Shared pointer to the costmap ROS wrapper
    */
   virtual void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) = 0;
 
   /**
    * @brief Reset the planner state
diff --git a/roadmap_explorer/include/roadmap_explorer/planners/EuclideanPlugin.hpp b/roadmap_explorer/include/roadmap_explorer/planners/EuclideanPlugin.hpp
@@ -16,7 +16,7 @@ class EuclideanDistancePlugin : public BasePlanner
   ~EuclideanDistancePlugin() override = default;
 
   void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   void reset() override;
 
diff --git a/roadmap_explorer/include/roadmap_explorer/planners/FrontierRoadmapPlugin.hpp b/roadmap_explorer/include/roadmap_explorer/planners/FrontierRoadmapPlugin.hpp
@@ -18,7 +18,7 @@ class PluginFrontierRoadmap : public BasePlanner
   ~PluginFrontierRoadmap() override = default;
 
   void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   void reset() override;
 
diff --git a/roadmap_explorer/include/roadmap_explorer/planners/NavFnPlugin.hpp b/roadmap_explorer/include/roadmap_explorer/planners/NavFnPlugin.hpp
@@ -18,7 +18,7 @@ class PluginNavFn : public BasePlanner
   ~PluginNavFn() override = default;
 
   void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   void reset() override;
 
diff --git a/roadmap_explorer/include/roadmap_explorer/planners/RandomDistancePlugin.hpp b/roadmap_explorer/include/roadmap_explorer/planners/RandomDistancePlugin.hpp
@@ -16,7 +16,7 @@ class RandomDistancePlugin : public BasePlanner
   ~RandomDistancePlugin() override = default;
 
   void configure(
-    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node) override;
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node) override;
 
   void reset() override;
 
diff --git a/roadmap_explorer/src/CostAssigner.cpp b/roadmap_explorer/src/CostAssigner.cpp
@@ -111,11 +111,6 @@ bool CostAssigner::assignCosts(
     LOG_ERROR("FrontierPtr cannot be selected, no polygon.");
     return false;
   }
-  
-  // Check for duplicates
-  if (findDuplicates(frontier_list).size() > 0) {
-    throw RoadmapExplorerException("Duplicate frontiers found.");
-  }
 
   // Initialize plugins once for all frontiers
   initializePlugins(informationGainPlugin, plannerPlugin);
@@ -130,25 +125,6 @@ bool CostAssigner::assignCosts(
   return true;
 }
 
-std::vector<FrontierPtr> CostAssigner::findDuplicates(const std::vector<FrontierPtr> & vec)
-{
-  std::vector<FrontierPtr> duplicates;
-
-  // Iterate through the vector
-  for (size_t i = 0; i < vec.size(); ++i) {
-    // Compare the current element with all subsequent elements
-    for (size_t j = i + 1; j < vec.size(); ++j) {
-      if (vec[i] == vec[j]) {
-        // If a duplicate is found, add it to the duplicates vector
-        duplicates.push_back(vec[i]);
-        break;             // Break the inner loop to avoid adding the same duplicate multiple times
-      }
-    }
-  }
-
-  return duplicates;
-}
-
 void CostAssigner::setFrontierBlacklist(std::vector<FrontierPtr> & blacklist)
 {
   std::lock_guard<std::mutex> lock(blacklist_mutex_);
diff --git a/roadmap_explorer/src/Parameters.cpp b/roadmap_explorer/src/Parameters.cpp
@@ -31,22 +31,6 @@ void ParameterHandler::makeParameters(std::shared_ptr<nav2_util::LifecycleNode>
     "frontierSearch.max_permissable_frontier_search_distance").as_double();
 
   // --- costCalculator ---
-  nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.closeness_rejection_threshold", rclcpp::ParameterValue(
-      0.5));
-  nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.planner_allow_unknown", rclcpp::ParameterValue(
-      true));
-  nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.max_planning_distance_roadmap", rclcpp::ParameterValue(
-      6.0));
-
-  parameter_map_["costCalculator.closeness_rejection_threshold"] = node->get_parameter(
-    "costCalculator.closeness_rejection_threshold").as_double();
-  parameter_map_["costCalculator.planner_allow_unknown"] = node->get_parameter(
-    "costCalculator.planner_allow_unknown").as_bool();
-  parameter_map_["costCalculator.max_planning_distance_roadmap"] = node->get_parameter(
-    "costCalculator.max_planning_distance_roadmap").as_double();
 
   // --- costAssigner ---
   nav2_util::declare_parameter_if_not_declared(
diff --git a/roadmap_explorer/src/frontier_search/PluginBFSearch.cpp b/roadmap_explorer/src/frontier_search/PluginBFSearch.cpp
@@ -18,29 +18,29 @@ FrontierBFSearch::~FrontierBFSearch()
   LOG_INFO("FrontierBFSearch::~FrontierBFSearch()");
 }
 
-void FrontierBFSearch::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node)
+void FrontierBFSearch::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node)
 {
   if (explore_costmap_ros == nullptr) {
     throw std::runtime_error("Given input costmap is null");
   }
   costmap_ = explore_costmap_ros->getCostmap();
 
   nav2_util::declare_parameter_if_not_declared(
-    node, "frontierSearch.min_frontier_cluster_size", rclcpp::ParameterValue(
+    node, name + ".min_frontier_cluster_size", rclcpp::ParameterValue(
       1.0));
   nav2_util::declare_parameter_if_not_declared(
-    node, "frontierSearch.max_frontier_cluster_size", rclcpp::ParameterValue(
+    node, name + ".max_frontier_cluster_size", rclcpp::ParameterValue(
       20.0));
   nav2_util::declare_parameter_if_not_declared(
-    node, "frontierSearch.lethal_threshold", rclcpp::ParameterValue(
+    node, name + ".lethal_threshold", rclcpp::ParameterValue(
       160));
 
   min_frontier_cluster_size_ = node->get_parameter(
-    "frontierSearch.min_frontier_cluster_size").as_double();
+    name + ".min_frontier_cluster_size").as_double();
   max_frontier_cluster_size_ = node->get_parameter(
-    "frontierSearch.max_frontier_cluster_size").as_double();
+    name + ".max_frontier_cluster_size").as_double();
   lethal_threshold_ = node->get_parameter(
-    "frontierSearch.lethal_threshold").as_int();
+    name + ".lethal_threshold").as_int();
 }
 
 void FrontierBFSearch::reset()
@@ -131,6 +131,11 @@ FrontierSearchResult FrontierBFSearch::searchFrom(
     }
   }
   output_frontier_list = frontier_list;
+  
+  // Check for duplicates
+  if (findDuplicates(frontier_list).size() > 0) {
+    throw RoadmapExplorerException("Duplicate frontiers found.");
+  }
   return FrontierSearchResult::SUCCESSFUL_SEARCH;
 }
 
@@ -338,6 +343,25 @@ std::pair<double, double> FrontierBFSearch::getCentroidOfCells(
   return std::make_pair(centerX, centerY);
 }
 
+std::vector<FrontierPtr> FrontierBFSearch::findDuplicates(const std::vector<FrontierPtr> & vec)
+{
+  std::vector<FrontierPtr> duplicates;
+
+  // Iterate through the vector
+  for (size_t i = 0; i < vec.size(); ++i) {
+    // Compare the current element with all subsequent elements
+    for (size_t j = i + 1; j < vec.size(); ++j) {
+      if (vec[i] == vec[j]) {
+        // If a duplicate is found, add it to the duplicates vector
+        duplicates.push_back(vec[i]);
+        break;             // Break the inner loop to avoid adding the same duplicate multiple times
+      }
+    }
+  }
+
+  return duplicates;
+}
+
 }
 
 #include <pluginlib/class_list_macros.hpp>
diff --git a/roadmap_explorer/src/information_gain/CountBasedGain.cpp b/roadmap_explorer/src/information_gain/CountBasedGain.cpp
@@ -3,34 +3,34 @@
 namespace roadmap_explorer
 {
 
-void CountBasedGain::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node)
+void CountBasedGain::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node)
 {
   LOG_INFO("CountBasedGain::configure");
   exploration_costmap_ = explore_costmap_ros->getCostmap();
   explore_costmap_ros_ = explore_costmap_ros;
   arrival_info_limits_set_ = false;
 
   nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.max_camera_depth", rclcpp::ParameterValue(
+    node, name + ".max_camera_depth", rclcpp::ParameterValue(
       2.0));
   nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.delta_theta", rclcpp::ParameterValue(
+    node, name + ".delta_theta", rclcpp::ParameterValue(
       0.10));
   nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.camera_fov", rclcpp::ParameterValue(
+    node, name + ".camera_fov", rclcpp::ParameterValue(
       1.04));
   nav2_util::declare_parameter_if_not_declared(
-    node, "costCalculator.factor_of_max_is_min", rclcpp::ParameterValue(
+    node, name + ".factor_of_max_is_min", rclcpp::ParameterValue(
       0.70));
 
   MAX_CAMERA_DEPTH = node->get_parameter(
-    "costCalculator.max_camera_depth").as_double();
+    name + ".max_camera_depth").as_double();
   DELTA_THETA =
-    node->get_parameter("costCalculator.delta_theta").as_double();
+    node->get_parameter(name + ".delta_theta").as_double();
   CAMERA_FOV =
-    node->get_parameter("costCalculator.camera_fov").as_double();
+    node->get_parameter(name + ".camera_fov").as_double();
   factor_of_max_is_min = node->get_parameter(
-    "costCalculator.factor_of_max_is_min").as_double();
+    name + ".factor_of_max_is_min").as_double();
 
   setArrivalInformationLimits();
 }
diff --git a/roadmap_explorer/src/planners/EuclideanPlugin.cpp b/roadmap_explorer/src/planners/EuclideanPlugin.cpp
@@ -2,11 +2,27 @@
 
 namespace roadmap_explorer
 {
-void EuclideanDistancePlugin::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node)
+void EuclideanDistancePlugin::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node)
 {
   LOG_INFO("EuclideanDistancePlugin::configure");
   exploration_costmap_ = explore_costmap_ros->getCostmap();
-  (void)node;
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + "closeness_rejection_threshold", rclcpp::ParameterValue(
+      0.5));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + "planner_allow_unknown", rclcpp::ParameterValue(
+      true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + "max_planning_distance_roadmap", rclcpp::ParameterValue(
+      6.0));
+
+  closeness_rejection_threshold_ = node->get_parameter(
+    name + "closeness_rejection_threshold").as_double();
+  planner_allow_unknown_ = node->get_parameter(
+    name + "planner_allow_unknown").as_bool();
+  max_planning_distance_ = node->get_parameter(
+    name + "max_planning_distance_roadmap").as_double();
 }
 
 void EuclideanDistancePlugin::reset()
diff --git a/roadmap_explorer/src/planners/FrontierRoadmapPlugin.cpp b/roadmap_explorer/src/planners/FrontierRoadmapPlugin.cpp
@@ -2,10 +2,26 @@
 
 namespace roadmap_explorer
 {
-void PluginFrontierRoadmap::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::shared_ptr<nav2_util::LifecycleNode> node)
+void PluginFrontierRoadmap::configure(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> explore_costmap_ros, std::string name, std::shared_ptr<nav2_util::LifecycleNode> node)
 {
   exploration_costmap_ = explore_costmap_ros->getCostmap();
-  (void)node;
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".closeness_rejection_threshold", rclcpp::ParameterValue(
+      0.5));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".planner_allow_unknown", rclcpp::ParameterValue(
+      true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_planning_distance_roadmap", rclcpp::ParameterValue(
+      6.0));
+
+  closeness_rejection_threshold_ = node->get_parameter(
+    name + ".closeness_rejection_threshold").as_double();
+  planner_allow_unknown_ = node->get_parameter(
+    name + ".planner_allow_unknown").as_bool();
+  max_planning_distance_ = node->get_parameter(
+    name + ".max_planning_distance_roadmap").as_double();
 }
 
 void PluginFrontierRoadmap::reset()
@@ -49,7 +65,7 @@ void PluginFrontierRoadmap::setPlanForFrontier(
 
   // check if the set euclidean distance is greater than max planning distance, if yes, set to a large value and return
   if (goal_point_w->getPathLengthInM() >
-    parameterInstance.getValue<double>("costCalculator.max_planning_distance_roadmap"))
+    max_planning_distance_)
   {
     goal_point_w->setAchievability(true);
     goal_point_w->setPathLength(goal_point_w->getPathLength() * 5.0);
diff --git a/roadmap_explorer/src/planners/NavFnPlugin.cpp b/roadmap_explorer/src/planners/NavFnPlugin.cpp
diff --git a/roadmap_explorer/src/planners/RandomDistancePlugin.cpp b/roadmap_explorer/src/planners/RandomDistancePlugin.cpp
