Skip to content

Commit

Permalink
Add ability to sort in descending order
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
@methylDragon
methylDragon committed Jul 24, 2024
1 parent 49db826 commit 16f95ff
Show file tree
Hide file tree
Showing 2 changed files with 18 additions and 14 deletions.
16 changes: 10 additions & 6 deletions moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,14 +171,15 @@ class TrajectoryCache
* \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters.
* \param[in] metadata_only. If true, returns only the cache entry metadata.
* \param[in] sort_by. The cache column to sort by, defaults to execution time.
* \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order.
* \returns A vector of cache hits, sorted by the `sort_by` param.
*/
std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
fetchAllMatchingTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group,
const std::string& cache_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance,
double goal_tolerance, bool metadata_only = false,
const std::string& sort_by = "execution_time_s");
const std::string& sort_by = "execution_time_s", bool ascending = true);

/**
* \brief Fetch the best trajectory that fits within the requested tolerances for start and goal conditions, by some
Expand All @@ -191,18 +192,19 @@ class TrajectoryCache
* \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters.
* \param[in] metadata_only. If true, returns only the cache entry metadata.
* \param[in] sort_by. The cache column to sort by, defaults to execution time.
* \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order.
* \returns The best cache hit, with respect to the `sort_by` param.
*/
warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr fetchBestMatchingTrajectory(
const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance,
bool metadata_only = false, const std::string& sort_by = "execution_time_s");
bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true);

/**
* \brief Put a trajectory into the database if it is the best matching trajectory seen so far.
*
* Trajectories are matched based off their start and goal states.
* And are considered "better" if they have a smaller planned execution time than exactly matching trajectories.
* And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching trajectories.
*
* A trajectory is "exactly matching" if its start and goal are close enough to another trajectory.
* The tolerance for this depends on the `exact_match_tolerance` arg passed in init().
Expand Down Expand Up @@ -261,14 +263,15 @@ class TrajectoryCache
* \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters.
* \param[in] metadata_only. If true, returns only the cache entry metadata.
* \param[in] sort_by. The cache column to sort by, defaults to execution time.
* \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order.
* \returns A vector of cache hits, sorted by the `sort_by` param.
*/
std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
fetchAllMatchingCartesianTrajectories(const moveit::planning_interface::MoveGroupInterface& move_group,
const std::string& cache_namespace,
const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
double min_fraction, double start_tolerance, double goal_tolerance,
bool metadata_only = false, const std::string& sort_by = "execution_time_s");
bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true);

/**
* \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions,
Expand All @@ -282,18 +285,19 @@ class TrajectoryCache
* \param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters.
* \param[in] metadata_only. If true, returns only the cache entry metadata.
* \param[in] sort_by. The cache column to sort by, defaults to execution time.
* \param[in] ascending. If true, sorts in ascending order. If false, sorts in descending order.
* \returns The best cache hit, with respect to the `sort_by` param.
*/
warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr fetchBestMatchingCartesianTrajectory(
const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance,
double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s");
double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true);

/**
* \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far.
*
* Cartesian trajectories are matched based off their start and goal states.
* And are considered "better" if they have a smaller planned execution time than exactly matching cartesian
* And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching cartesian
* trajectories.
*
* A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory.
Expand Down
16 changes: 8 additions & 8 deletions moveit_ros/trajectory_cache/src/trajectory_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface::
const std::string& cache_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
double start_tolerance, double goal_tolerance, bool metadata_only,
const std::string& sort_by)
const std::string& sort_by, bool ascending)
{
auto coll = db_->openCollection<moveit_msgs::msg::RobotTrajectory>("move_group_trajectory_cache", cache_namespace);

Expand All @@ -122,18 +122,18 @@ TrajectoryCache::fetchAllMatchingTrajectories(const moveit::planning_interface::
return {};
}

return coll.queryList(query, metadata_only, sort_by, true);
return coll.queryList(query, metadata_only, sort_by, ascending);
}

MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory(
const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance,
bool metadata_only, const std::string& sort_by)
bool metadata_only, const std::string& sort_by, bool ascending)
{
// First find all matching, but metadata only.
// Then use the ID metadata of the best plan to pull the actual message.
auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request,
start_tolerance, goal_tolerance, true, sort_by);
start_tolerance, goal_tolerance, true, sort_by, ascending);

if (matching_trajectories.empty())
{
Expand Down Expand Up @@ -288,7 +288,7 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in
const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
double min_fraction, double start_tolerance,
double goal_tolerance, bool metadata_only,
const std::string& sort_by)
const std::string& sort_by, bool ascending)
{
auto coll =
db_->openCollection<moveit_msgs::msg::RobotTrajectory>("move_group_cartesian_trajectory_cache", cache_namespace);
Expand All @@ -306,18 +306,18 @@ TrajectoryCache::fetchAllMatchingCartesianTrajectories(const moveit::planning_in
}

query->appendGTE("fraction", min_fraction);
return coll.queryList(query, metadata_only, sort_by, true);
return coll.queryList(query, metadata_only, sort_by, ascending);
}

MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory(
const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance,
double goal_tolerance, bool metadata_only, const std::string& sort_by)
double goal_tolerance, bool metadata_only, const std::string& sort_by, bool ascending)
{
// First find all matching, but metadata only.
// Then use the ID metadata of the best plan to pull the actual message.
auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories(
move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by);
move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by, ascending);

if (matching_trajectories.empty())
{
Expand Down

0 comments on commit 16f95ff

Please sign in to comment.