From 6004f96fc39d4b287ab5c142adb1fc220c83a4b8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 12:56:12 -0800 Subject: [PATCH] Implement plan fetching with configurable key Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 32 ++++++++-------- .../src/motion_plan_cache.hpp | 37 ++++++++++++++++--- 2 files changed, 46 insertions(+), 23 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 7e0d3d8..39e0b78 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -74,7 +74,8 @@ MotionPlanCache::fetch_all_matching_plans( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double start_tolerance, double goal_tolerance, bool metadata_only) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection( "move_group_plan_cache", move_group_namespace); @@ -92,8 +93,7 @@ MotionPlanCache::fetch_all_matching_plans( return {}; } - return coll.queryList( - query, metadata_only, /* sort_by */ "execution_time_s", true); + return coll.queryList(query, metadata_only, sort_by, true); } MessageWithMetadata::ConstPtr @@ -101,14 +101,14 @@ MotionPlanCache::fetch_best_matching_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double start_tolerance, double goal_tolerance, bool metadata_only) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. auto matching_plans = this->fetch_all_matching_plans( move_group, move_group_namespace, - plan_request, start_tolerance, goal_tolerance, - true); + plan_request, start_tolerance, goal_tolerance, true, sort_by); if (matching_plans.empty()) { @@ -134,9 +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 overwrite) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -921,8 +919,8 @@ MotionPlanCache::extract_and_append_plan_goal_to_metadata( moveit_msgs::srv::GetCartesianPath::Request MotionPlanCache::construct_get_cartesian_plan_request( moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions) + const std::vector& waypoints, + double step, double jump_threshold, bool avoid_collisions) { moveit_msgs::srv::GetCartesianPath::Request out; @@ -958,7 +956,8 @@ MotionPlanCache::fetch_all_matching_cartesian_plans( const std::string& move_group_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, - double start_tolerance, double goal_tolerance, bool metadata_only) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); @@ -977,9 +976,7 @@ MotionPlanCache::fetch_all_matching_cartesian_plans( } query->appendGTE("fraction", min_fraction); - - return coll.queryList( - query, metadata_only, /* sort_by */ "execution_time_s", true); + return coll.queryList(query, metadata_only, sort_by, true); } MessageWithMetadata::ConstPtr @@ -988,14 +985,15 @@ MotionPlanCache::fetch_best_matching_cartesian_plan( const std::string& move_group_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, - double start_tolerance, double goal_tolerance, bool metadata_only) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. auto matching_plans = this->fetch_all_matching_cartesian_plans( move_group, move_group_namespace, plan_request, min_fraction, - start_tolerance, goal_tolerance, true); + start_tolerance, goal_tolerance, true, sort_by); if (matching_plans.empty()) { diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index e637e35..89e06ce 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -97,6 +97,9 @@ class MotionPlanCache // MOTION PLAN CACHING // =========================================================================== // TOP LEVEL OPS + + // Fetches all plans that fit within the requested tolerances for start and + // goal conditions, returning them as a vector, sorted by some db column. std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -106,8 +109,13 @@ class MotionPlanCache const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double start_tolerance, double goal_tolerance, bool metadata_only = false); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + // Fetches the best plan that fits within the requested tolerances for start + // and goal conditions, by some db column. warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr @@ -115,7 +123,10 @@ class MotionPlanCache const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - double start_tolerance, double goal_tolerance, bool metadata_only = false); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); bool put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, @@ -154,14 +165,20 @@ class MotionPlanCache // CARTESIAN PLAN CACHING // =========================================================================== // TOP LEVEL OPS + // This mimics the move group computeCartesianPath signature (without path // constraints). moveit_msgs::srv::GetCartesianPath::Request construct_get_cartesian_plan_request( moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions = true); + const std::vector& waypoints, + double step, + double jump_threshold, + bool avoid_collisions = true); + // Fetches all cartesian plans that fit within the requested tolerances for + // start and goal conditions, returning them as a vector, sorted by some db + // column. std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -172,8 +189,13 @@ class MotionPlanCache const std::string& move_group_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, - double start_tolerance, double goal_tolerance, bool metadata_only = false); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + // Fetches the best cartesian plan that fits within the requested tolerances + // for start and goal conditions, by some db column. warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr @@ -182,7 +204,10 @@ class MotionPlanCache const std::string& move_group_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, - double start_tolerance, double goal_tolerance, bool metadata_only = false); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); bool put_cartesian_plan( const moveit::planning_interface::MoveGroupInterface& move_group,