Add overwrite_worse_plans param

Signed-off-by: methylDragon <[email protected]>
osrf · Sep 27, 2023 · b3b5656 · b3b5656
1 parent 737ca8c
commit b3b5656
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp
@@ -207,6 +207,14 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options)
     _only_use_cached_plans ? "True" : "False"
   );
 
+  _overwrite_worse_plans = this->declare_parameter(
+    "overwrite_worse_plans", true);
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Setting parameter overwrite_worse_plans to [%s]",
+    _overwrite_worse_plans ? "True" : "False"
+  );
+
   _cache_db_plugin = this->declare_parameter(
     "cache_db_plugin", "warehouse_ros_sqlite::DatabaseConnection");
   RCLCPP_INFO(
@@ -227,7 +235,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options)
   );
 
   _cache_exact_match_tolerance = this->declare_parameter(
-    "cache_exact_match_tolerance", 0.025);
+    "cache_exact_match_tolerance", 0.00001);
   RCLCPP_INFO(
     this->get_logger(),
     "Setting parameter cache_exact_match_tolerance to [%.2e]",
@@ -691,7 +699,7 @@ void MotionPlannerServer::plan_with_move_group(
             res->result.trajectory.joint_trajectory.points.back()
             .time_from_start
           ).seconds(),
-          true))
+          _overwrite_worse_plans))
       {
         RCLCPP_WARN(this->get_logger(), "Did not put plan into cache.");
       }

diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp
@@ -100,6 +100,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode
 
   bool _use_motion_plan_cache;
   bool _only_use_cached_plans;
+  bool _overwrite_worse_plans;
   std::string _cache_db_plugin;
   std::string _cache_db_host;
   int _cache_db_port;