Skip to content

Commit

Permalink
Rename cache methods to prepare for cartesian caching
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
@methylDragon
methylDragon committed Nov 20, 2023
1 parent 52db5b5 commit e0d4cbc
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 22 deletions.
31 changes: 16 additions & 15 deletions nexus_motion_planner/src/motion_plan_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>(
Expand Down Expand Up @@ -66,7 +64,10 @@ void MotionPlanCache::init(
db_->connect();
}

// TOP LEVEL OPS ===============================================================
// =============================================================================
// MOTION PLAN CACHING
// =============================================================================
// MOTION PLAN CACHING: TOP LEVEL OPS
std::vector<MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
MotionPlanCache::fetch_all_matching_plans(
const moveit::planning_interface::MoveGroupInterface& move_group,
Expand All @@ -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)
Expand Down Expand Up @@ -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_);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down
19 changes: 12 additions & 7 deletions nexus_motion_planner/src/motion_plan_cache.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit e0d4cbc

Please sign in to comment.