Skip to content

Commit

Permalink
Implement plan fetching with configurable key
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 0e84dd4 commit 6004f96
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 23 deletions.
32 changes: 15 additions & 17 deletions nexus_motion_planner/src/motion_plan_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<moveit_msgs::msg::RobotTrajectory>(
"move_group_plan_cache", move_group_namespace);
Expand All @@ -92,23 +93,22 @@ 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<moveit_msgs::msg::RobotTrajectory>::ConstPtr
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())
{
Expand All @@ -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())
Expand Down Expand Up @@ -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<geometry_msgs::msg::Pose>& waypoints, double step,
double jump_threshold, bool avoid_collisions)
const std::vector<geometry_msgs::msg::Pose>& waypoints,
double step, double jump_threshold, bool avoid_collisions)
{
moveit_msgs::srv::GetCartesianPath::Request out;

Expand Down Expand Up @@ -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<moveit_msgs::msg::RobotTrajectory>(
"move_group_cartesian_plan_cache", move_group_namespace);
Expand All @@ -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<moveit_msgs::msg::RobotTrajectory>::ConstPtr
Expand All @@ -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())
{
Expand Down
37 changes: 31 additions & 6 deletions nexus_motion_planner/src/motion_plan_cache.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -106,16 +109,24 @@ 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
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 = 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,
Expand Down Expand Up @@ -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<geometry_msgs::msg::Pose>& waypoints, double step,
double jump_threshold, bool avoid_collisions = true);
const std::vector<geometry_msgs::msg::Pose>& 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
Expand All @@ -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
Expand All @@ -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,
Expand Down

0 comments on commit 6004f96

Please sign in to comment.