From e0d4cbcd898b048ea125a1e30e25c194e29c3887 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 23:27:17 -0700 Subject: [PATCH] Rename cache methods to prepare for cartesian caching Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 31 ++++++++++--------- .../src/motion_plan_cache.hpp | 19 +++++++----- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index a63d148..113f397 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -36,8 +36,6 @@ using warehouse_ros::Query; MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { - // TODO: methyldragon - - // Declare or pass in these in the motion_planner_server instead? if (!node_->has_parameter("warehouse_plugin")) { node_->declare_parameter( @@ -66,7 +64,10 @@ void MotionPlanCache::init( db_->connect(); } -// TOP LEVEL OPS =============================================================== +// ============================================================================= +// MOTION PLAN CACHING +// ============================================================================= +// MOTION PLAN CACHING: TOP LEVEL OPS std::vector::ConstPtr> MotionPlanCache::fetch_all_matching_plans( const moveit::planning_interface::MoveGroupInterface& move_group, @@ -79,9 +80,9 @@ MotionPlanCache::fetch_all_matching_plans( Query::Ptr query = coll.createQuery(); - bool start_ok = this->extract_and_append_start_to_query( + bool start_ok = this->extract_and_append_plan_start_to_query( *query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extract_and_append_goal_to_query( + bool goal_ok = this->extract_and_append_plan_goal_to_query( *query, move_group, plan_request, goal_tolerance); if (!start_ok || !goal_ok) @@ -170,10 +171,10 @@ MotionPlanCache::put_plan( // overwrite. Query::Ptr exact_query = coll.createQuery(); - bool start_query_ok = this->extract_and_append_start_to_query( + bool start_query_ok = this->extract_and_append_plan_start_to_query( *exact_query, move_group, plan_request, exact_match_precision_); - bool goal_query_ok = this->extract_and_append_goal_to_query( + bool goal_query_ok = this->extract_and_append_plan_goal_to_query( *exact_query, move_group, plan_request, exact_match_precision_); @@ -223,9 +224,9 @@ MotionPlanCache::put_plan( { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extract_and_append_start_to_metadata( + bool start_meta_ok = this->extract_and_append_plan_start_to_metadata( *insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extract_and_append_goal_to_metadata( + bool goal_meta_ok = this->extract_and_append_plan_goal_to_metadata( *insert_metadata, move_group, plan_request); insert_metadata->append("execution_time_s", execution_time_s); insert_metadata->append("planning_time_s", planning_time_s); @@ -256,9 +257,9 @@ MotionPlanCache::put_plan( return false; } -// QUERY CONSTRUCTION ========================================================== +// MOTION PLAN CACHING: QUERY CONSTRUCTION bool -MotionPlanCache::extract_and_append_start_to_query( +MotionPlanCache::extract_and_append_plan_start_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -364,7 +365,7 @@ MotionPlanCache::extract_and_append_start_to_query( } bool -MotionPlanCache::extract_and_append_goal_to_query( +MotionPlanCache::extract_and_append_plan_goal_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -605,9 +606,9 @@ MotionPlanCache::extract_and_append_goal_to_query( return true; } -// METADATA CONSTRUCTION ======================================================= +// MOTION PLAN CACHING: METADATA CONSTRUCTION bool -MotionPlanCache::extract_and_append_start_to_metadata( +MotionPlanCache::extract_and_append_plan_start_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) @@ -706,7 +707,7 @@ MotionPlanCache::extract_and_append_start_to_metadata( } bool -MotionPlanCache::extract_and_append_goal_to_metadata( +MotionPlanCache::extract_and_append_plan_goal_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index f3f3f3c..c958757 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -50,6 +50,8 @@ namespace motion_planner { * how long they took to execute. This allows for the lookup and reuse of the * best performing plans found so far. * + * WARNING: This cache does NOT yet support collision detection! + * * Relevant ROS Parameters: * - `warehouse_plugin`: What database to use * - `warehouse_host`: Where the database should be created @@ -82,7 +84,10 @@ class MotionPlanCache uint32_t db_port = 0, double exact_match_precision = 1e-6); - // TOP LEVEL OPS ============================================================= + // =========================================================================== + // MOTION PLAN CACHING + // =========================================================================== + // TOP LEVEL OPS std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -112,26 +117,26 @@ class MotionPlanCache double planning_time_s, bool overwrite = true); - // QUERY CONSTRUCTION ======================================================== - bool extract_and_append_start_to_query( + // QUERY CONSTRUCTION + bool extract_and_append_plan_start_to_query( warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance); - bool extract_and_append_goal_to_query( + bool extract_and_append_plan_goal_to_query( warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance); - // METADATA CONSTRUCTION ===================================================== - bool extract_and_append_start_to_metadata( + // METADATA CONSTRUCTION + bool extract_and_append_plan_start_to_metadata( warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request); - bool extract_and_append_goal_to_metadata( + bool extract_and_append_plan_goal_to_metadata( warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request);