moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
A cache insertion policy that always decides to insert and never decides to prune for cartesian path requests. More...
#include <always_insert_never_prune_policy.hpp>
Public Member Functions | |
CartesianAlwaysInsertNeverPrunePolicy () | |
std::string | getName () const override |
Gets the name of the cache insert policy. | |
moveit::core::MoveItErrorCode | checkCacheInsertInputs (const moveit::planning_interface::MoveGroupInterface &move_group, const warehouse_ros::MessageCollection< moveit_msgs::msg::RobotTrajectory > &coll, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value) override |
Checks inputs to the cache insert call to see if we should abort instead. | |
std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr > | fetchMatchingEntries (const moveit::planning_interface::MoveGroupInterface &move_group, const warehouse_ros::MessageCollection< moveit_msgs::msg::RobotTrajectory > &coll, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value, double exact_match_precision) override |
Fetches all "matching" cache entries for comparison for pruning. | |
bool | shouldPruneMatchingEntry (const moveit::planning_interface::MoveGroupInterface &move_group, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value, const warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr &matching_entry, std::string *reason=nullptr) override |
bool | shouldInsert (const moveit::planning_interface::MoveGroupInterface &move_group, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value, std::string *reason=nullptr) override |
Returns whether the insertion candidate should be inserted into the cache. | |
moveit::core::MoveItErrorCode | appendInsertMetadata (warehouse_ros::Metadata &metadata, const moveit::planning_interface::MoveGroupInterface &move_group, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value) override |
Appends the insert metadata with the features supported by the policy. | |
void | reset () override |
Resets the state of the policy. | |
Public Member Functions inherited from moveit_ros::trajectory_cache::CacheInsertPolicyInterface< moveit_msgs::srv::GetCartesianPath::Request, moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory > | |
virtual | ~CacheInsertPolicyInterface ()=default |
virtual bool | shouldPruneMatchingEntry (const moveit::planning_interface::MoveGroupInterface &move_group, const moveit_msgs::srv::GetCartesianPath::Request &key, const moveit_msgs::srv::GetCartesianPath::Response &value, const typename warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr &matching_entry, std::string *reason)=0 |
Returns whether a matched cache entry should be pruned. | |
Static Public Member Functions | |
static std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > | getSupportedFeatures (double start_tolerance, double goal_tolerance, double min_fraction) |
Configures and returns a vector of feature extractors that can be used with this policy. | |
A cache insertion policy that always decides to insert and never decides to prune for cartesian path requests.
Supported Metadata and Features ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Appends the following additional metadata, which can be used for querying and sorting:
NOTE: Planning time is not available. If you want to use it, add it as an additional MetadataOnly feature in the cache insert call.
Compatible with the get cartesian path request features:
Matches, Pruning, and Insertion ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ A matching cache entry is one that exactly matches on every one of the features above, and fraction.
The sort order is ordered on execution_time_s in ascending order (so loweest execution time first). This policy never indicates that pruning should happen, and always indicates that insertion should happen.
Definition at line 165 of file always_insert_never_prune_policy.hpp.
moveit_ros::trajectory_cache::CartesianAlwaysInsertNeverPrunePolicy::CartesianAlwaysInsertNeverPrunePolicy | ( | ) |
Definition at line 222 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Appends the insert metadata with the features supported by the policy.
See notes in docstrings regarding the feature contract.
[in,out] | metadata. | The metadata to add features to. |
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | key. | The object used to key the insertion candidate with. |
[in] | value. | The object that the TrajectoryCache was passed to insert. |
Definition at line 346 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Checks inputs to the cache insert call to see if we should abort instead.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | coll. | The cache database to fetch messages from. |
[in] | key. | The object used to key the insertion candidate with. |
[in] | value. | The object that the TrajectoryCache was passed to insert. |
Definition at line 258 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Fetches all "matching" cache entries for comparison for pruning.
This method should be assumed to only return the message metadata without the underlying message data.
The policy should also make the decision about how to sort them. The order in which cache entries are presented to the shouldPruneMatchingEntry will be the order of cache entries returned by this function.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | coll. | The cache database to fetch messages from. |
[in] | key. | The object used to key the insertion candidate with. |
[in] | value. | The object that the TrajectoryCache was passed to insert. |
[in] | exact_match_precision. | Tolerance for float precision comparison for what counts as an exact match. |
Definition at line 306 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Gets the name of the cache insert policy.
Definition at line 253 of file always_insert_never_prune_policy.cpp.
|
static |
Configures and returns a vector of feature extractors that can be used with this policy.
Definition at line 231 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Resets the state of the policy.
Definition at line 366 of file always_insert_never_prune_policy.cpp.
|
overridevirtual |
Returns whether the insertion candidate should be inserted into the cache.
NOTE: The TrajectoryCache class executes the insert, but this class informs it on whether the insert should happen or not.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | key. | The object used to key the insertion candidate with. |
[in] | value. | The object that the TrajectoryCache was passed to insert. |
[out] | reason. | The reason for the returned result. |
Definition at line 334 of file always_insert_never_prune_policy.cpp.
|
override |
Definition at line 322 of file always_insert_never_prune_policy.cpp.