From d1847baa85525f2c4bdabcf5eed631246e4c1acf Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 14:59:29 -0800 Subject: [PATCH] Rename overwrite to delete_worse_plans Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 10 ++++------ .../src/motion_plan_cache.hpp | 19 +++++++++++++++++-- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 10d2188..8470182 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -134,7 +134,7 @@ MotionPlanCache::put_plan( const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& plan, - double execution_time_s, double planning_time_s, bool overwrite) + double execution_time_s, double planning_time_s, bool delete_worse_plans) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -190,8 +190,7 @@ MotionPlanCache::put_plan( { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - // Delete "worse" plans if overwrite requested. - if (overwrite) + if (delete_worse_plans) { for (auto& match : exact_matches) { @@ -1020,7 +1019,7 @@ MotionPlanCache::put_cartesian_plan( double execution_time_s, double planning_time_s, double fraction, - bool overwrite) + bool delete_worse_plans) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -1068,8 +1067,7 @@ MotionPlanCache::put_cartesian_plan( { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - // Delete "worse" plans if overwrite requested. - if (overwrite) + if (delete_worse_plans) { for (auto& match : exact_matches) { diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index 89e06ce..fc7bbff 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -128,6 +128,13 @@ class MotionPlanCache bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + // Put a plan into the database if it is the best matching plan seen so far. + // + // Plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching plans. + // + // Optionally deletes all worse plans by default to prune the cache. bool put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, @@ -135,7 +142,7 @@ class MotionPlanCache const moveit_msgs::msg::RobotTrajectory& plan, double execution_time_s, double planning_time_s, - bool overwrite = true); + bool delete_worse_plans = true); // QUERY CONSTRUCTION bool extract_and_append_plan_start_to_query( @@ -209,6 +216,14 @@ class MotionPlanCache bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + // Put a cartesian plan into the database if it is the best matching cartesian + // plan seen so far. + // + // Cartesian plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching cartesian plans. + // + // Optionally deletes all worse cartesian plans by default to prune the cache. bool put_cartesian_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, @@ -217,7 +232,7 @@ class MotionPlanCache double execution_time_s, double planning_time_s, double fraction, - bool overwrite = true); + bool delete_worse_plans = true); // QUERY CONSTRUCTION bool extract_and_append_cartesian_plan_start_to_query(