Skip to content

Commit

Permalink
Formatting pass
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
methylDragon committed Jul 25, 2024
1 parent fae241e commit 998377e
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 66 deletions.
4 changes: 2 additions & 2 deletions moveit_ros/trajectory_cache/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ else
traj_cache->putTrajectory(
*interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory),
rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(),
res->result.planning_time, /*overwrite=*/true);
res->result.planning_time, /*delete_worse_trajectories=*/true);
}
```
Expand Down Expand Up @@ -131,4 +131,4 @@ Since this is an initial release, the following features are unsupported because
- Including: path, constraint regions, everything related to collision.
- The fuzzy lookup can't be configured on a per-joint basis.
- Alternate ordinal lookup metrics for the cache
- Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153)
- Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153)
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,8 @@ class TrajectoryCache
* \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 higher priority in the sorting order specified by `sort_by` 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 @@ -271,7 +272,8 @@ class TrajectoryCache
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 ascending = true);
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 @@ -291,14 +293,15 @@ class TrajectoryCache
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", bool ascending = true);
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 higher priority in the sorting order specified by `sort_by` than exactly matching cartesian
* trajectories.
* 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.
* The tolerance for this depends on the `exact_match_tolerance` arg passed in init().
Expand Down Expand Up @@ -373,7 +376,7 @@ class TrajectoryCache
*
* These parameters will be used key the cache element.
*
* \param[out] metadata. The metadata to add paramters to.
* \param[out] metadata. The metadata to add parameters to.
* \param[in] move_group. The manipulator move group, used to get its state.
* \param[in] plan_request. The motion plan request to key the cache with.
* \returns true if successfully added to. If false, the metadata might have been partially modified and should not be
Expand All @@ -389,7 +392,7 @@ class TrajectoryCache
*
* These parameters will be used key the cache element.
*
* \param[out] metadata. The metadata to add paramters to.
* \param[out] metadata. The metadata to add parameters to.
* \param[in] move_group. The manipulator move group, used to get its state.
* \param[in] plan_request. The motion plan request to key the cache with.
* \returns true if successfully added to. If false, the metadata might have been partially modified and should not be
Expand Down Expand Up @@ -448,7 +451,7 @@ class TrajectoryCache
*
* These parameters will be used key the cache element.
*
* \param[out] metadata. The metadata to add paramters to.
* \param[out] metadata. The metadata to add parameters to.
* \param[in] move_group. The manipulator move group, used to get its state.
* \param[in] plan_request. The cartesian plan request to key the cache with.
* \returns true if successfully added to. If false, the metadata might have been partially modified and should not be
Expand All @@ -465,7 +468,7 @@ class TrajectoryCache
*
* These parameters will be used key the cache element.
*
* \param[out] metadata. The metadata to add paramters to.
* \param[out] metadata. The metadata to add parameters to.
* \param[in] move_group. The manipulator move group, used to get its state.
* \param[in] plan_request. The cartesian plan request to key the cache with.
* \returns true if successfully added to. If false, the metadata might have been partially modified and should not be
Expand Down
Loading

0 comments on commit 998377e

Please sign in to comment.