From b3b565632529adbf9e474101bb409f6c9764cf33 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 21:36:05 -0700 Subject: [PATCH] Add overwrite_worse_plans param Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 12 ++++++++++-- nexus_motion_planner/src/motion_planner_server.hpp | 1 + 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index b54442a..8d67901 100644 --- 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 index 5ad8564..24e319a 100644 --- 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;