From 1feb211a8f712436ae065fedca08f9eef2459c66 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sat, 3 Aug 2024 03:11:23 -0700 Subject: [PATCH] Add CacheInsertPolicyInterface and AlwaysInsertNeverPrunePolicy Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 14 + .../always_insert_never_prune_policy.hpp | 119 +++++++++ .../cache_insert_policy_interface.hpp | 205 +++++++++++++++ .../always_insert_never_prune_policy.cpp | 248 ++++++++++++++++++ .../trajectory_cache/test/CMakeLists.txt | 19 +- ...ert_never_prune_policy_with_move_group.cpp | 220 ++++++++++++++++ .../test/gtest_with_move_group.py | 2 +- 7 files changed, 823 insertions(+), 4 deletions(-) create mode 100644 moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp create mode 100644 moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp create mode 100644 moveit_ros/trajectory_cache/src/cache_insert_policies/always_insert_never_prune_policy.cpp create mode 100644 moveit_ros/trajectory_cache/test/cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 4105d26444d..98a48f9601b 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -34,6 +34,7 @@ set(TRAJECTORY_CACHE_DEPENDENCIES set(TRAJECTORY_CACHE_LIBRARIES moveit_ros_trajectory_cache_utils_lib moveit_ros_trajectory_cache_features_lib + moveit_ros_trajectory_cache_cache_insert_policies_lib moveit_ros_trajectory_cache_lib) # Utils library @@ -60,6 +61,19 @@ target_include_directories( ament_target_dependencies(moveit_ros_trajectory_cache_features_lib ${TRAJECTORY_CACHE_DEPENDENCIES}) +# Cache insert policies library +add_library(moveit_ros_trajectory_cache_cache_insert_policies_lib + src/cache_insert_policies/always_insert_never_prune_policy.cpp) +generate_export_header(moveit_ros_trajectory_cache_cache_insert_policies_lib) +target_link_libraries(moveit_ros_trajectory_cache_cache_insert_policies_lib + moveit_ros_trajectory_cache_utils_lib) +target_include_directories( + moveit_ros_trajectory_cache_cache_insert_policies_lib + PUBLIC $ + $) +ament_target_dependencies(moveit_ros_trajectory_cache_cache_insert_policies_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) + # Trajectory cache library add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) generate_export_header(moveit_ros_trajectory_cache_lib) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp new file mode 100644 index 00000000000..4fb8862ccf8 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp @@ -0,0 +1,119 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief A cache insertion policy that always decides to insert and never decides to prune for motion plan requests. + + * This is mainly for explanatory purposes. + * @see CacheInsertPolicyInterface + * + * @author methylDragon + */ + +#pragma once + +#include + +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan ==== + +/** @class AlwaysInsertNeverPrunePolicy + * + * @brief A cache insertion policy that always decides to insert and never decides to prune for motion plan requests. + * + * Supported Metadata and Features + * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + * Appends the following additional metadata, which can be used for querying and sorting: + * - execution_time_s + * - planning_time_s + * + * Usable with the motion plan request features: + * - WorkspaceFeatures + * - StartStateJointStateFeatures + * - MaxSpeedAndAccelerationFeatures + * - GoalConstraintsFeatures + * - PathConstraintsFeatures + * - TrajectoryConstraintsFeatures + * + * @see motion_plan_request_features.hpp + * @see FeaturesInterface + * + * Matches, Pruning, and Insertion + * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + * A matching cache entry is one that exactly matches on every one of the features above. + * + * 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. + */ +class AlwaysInsertNeverPrunePolicy final + : public CacheInsertPolicyInterface +{ +public: + /** @brief Configures and returns a vector of feature extractors that can be used with this policy. */ + static std::vector>> + getSupportedFeatures(double start_tolerance, double goal_tolerance); + + AlwaysInsertNeverPrunePolicy(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value) override; + + std::vector::ConstPtr> + fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value, + double exact_match_precision) override; + + bool prunePredicate( + const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value, + const warehouse_ros::MessageWithMetadata::ConstPtr& matched_entry) override; + + bool insertPredicate(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value) override; + + moveit::core::MoveItErrorCode + appendInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value) override; + + void reset() override; + +private: + const std::string name_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp new file mode 100644 index 00000000000..de1996c7a69 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp @@ -0,0 +1,205 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* Author: methylDragon */ + +#pragma once + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +/** @class CacheInsertPolicyInterface + * @headerfile cache_insert_policy_interface.hpp + * "moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp" + * + * @brief Abstract class for injecting logic for determining when to prune and insert a cache entry, and what metadata + * to attach to the cache entry. + * + * @tparam KeyT. The object to extract features from which to key the cache. Typically the plan request. + * @tparam ValueT. The object that the TrajectoryCache was passed to insert. Typically the plan response. + * @tparam CacheEntryT. The actual type of the cache entry. + * + * Notably, implementations of this interface are used to determine all the logic leading up to, but not including, the + * insert call itself. + * + * Additionally, the choice of which implementation to use will constraint the set of FeaturesInterface + * implementations that can be used to fetch cache entries with, with a caveat mentioned later. + * + * Users may implement this interface to add additional cache insertion functionality beyond the ones provided by this + * package (e.g., prioritizing minimum jerk, or minimal path length), and the set of metadata considered when inserting + * the cache entry. + * + * @see TrajectoryCache + * @see FeaturesInterface + * + * Usage + * ^^^^^ + * Each policy makes certain decisions about what pieces of metadata to attach to the cache + * entry for later fetching. + * + * As such, an implementation will necessarily constrain the set of FeaturesInterface implementations + * that can be used to fetch entries from the cache that were tagged by a policy. + * + * There is no way to meaningfully express this coupling at compile time, so users must ensure that they read the + * documentation of each implementation of the interface to determine what FeaturesInterface + * implementations can be used with cache entries processed by the implementation. + * + * Two mitigations may be applied to alleviate this: + * 1. Implementations of this interface may expose helper methods that can be called at runtime that take in arbitrary + * configuration parameters to them initialize the features that are used to fetch cache entries at point of use. + * + * 2. The TrajectoryCache class' insertion methods allow the bubbling down of additional user-specified features to + * be appended by the TrajectoryCache independent of a policy, which can be used to + * support features that are not explicitly supported by the policy. + * + * Care must be taken to ensure that there are no overlaps when doing so, though, because it could potentially + * result in duplicate metadata entries otherwise. + * + * Consequently, a set of FeaturesInterface implementations can be used to fetch a cache entry if that + * set is a subset of the union of: + * - The `additional_features` passed to `TrajectoryCache` + * - The features used by a policy to append metadata to the cache entry + * + * Pruning and Insertion + * ^^^^^^^^^^^^^^^^^^^^^ + * Pruning is a two step process: + * 1. Fetch all "matching" cache entries, as defined by the policy + * 2. From the fetched "matching" entries, subject each to the `prunePredicate` method, again defined by the + * policy. + * + * This allows a user to define a policy to match and prune on any arbitrary properties of + * the cache entries and insertion candidate. + * + * Similarly, logic can be injected to determine when the insertion candidate should be inserted. + * + * NOTE: The TrajectoryCache class also has some top-level logic to preserve cache entries that would have been pruned. + * + * Stateful Policies + * ^^^^^^^^^^^^^^^^^ + * Finally, as there can be information that must be preserved across the multiple steps of the match-prune-insert + * process, this interface assumes stateful operation within a single insert call. + * + * For example, preserving information in state will be required to propagate aggregated or rollup information about the + * entire set of matched cache entries to future calls. + * + * The TrajectoryCache will call `reset()` on the policy at the end of the insert call. + * + * Call Flow + * ^^^^^^^^^ + * The TrajectoryCache will call the following interface methods in the following order: + * 1. sanitize, once + * 2. fetchMatchingEntries, once + * 3. prunePredicate, once per fetched entry from fetchMatchingEntries + * 4. insertPredicate, once + * 5. appendInsertMetadata, once, if insertPredicate returns true + * 6. reset, once, at the end. + */ +template +class CacheInsertPolicyInterface +{ +public: + virtual ~CacheInsertPolicyInterface() = default; + + /** @brief Gets the name of the cache insert policy. */ + virtual std::string getName() const = 0; + + /** @brief Checks inputs to the cache insert call to see if we should abort instead. + + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] coll. The cache database to fetch messages from. + * @param[in] key. The object used to key the insertion candidate with. + * @param[in] value. The object that the TrajectoryCache was passed to insert. + * @returns moveit::core::MoveItErrorCode::SUCCESS if the cache insert should proceed. Otherwise, will return a + different error code with the reason why it should not. + */ + virtual moveit::core::MoveItErrorCode + checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, const KeyT& key, + const ValueT& value) = 0; + + /** @brief Fetches all "matching" cache entries for comparison for pruning. + * + * The policy should also make the decision about how to sort them. + * The order in which cache entries are presented to the prunePredicate will be the order of cache entries returned by + * this function. + * + * @see CacheInsertPolicyInterface#prunePredicate + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] coll. The cache database to fetch messages from. + * @param[in] key. The object used to key the insertion candidate with. + * @param[in] value. The object that the TrajectoryCache was passed to insert. + * @param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * @returns A vector of matching cache entries for use by the other methods. + */ + virtual std::vector::ConstPtr> + fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, const KeyT& key, const ValueT& value, + double exact_match_precision) = 0; + + /** @brief Returns whether a matched cache entry should be pruned. + * + * NOTE: The TrajectoryCache class also has some top-level logic to preserve cache entries that would have been + * pruned. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] key. The object used to key the insertion candidate with. + * @param[in] value. The object that the TrajectoryCache was passed to insert. + * @param[in] matched_entry. The matched cache entry to be possibly pruned. + * @returns True if the cache entry should be pruned. + */ + virtual bool + prunePredicate(const moveit::planning_interface::MoveGroupInterface& move_group, const KeyT& key, const ValueT& value, + const typename warehouse_ros::MessageWithMetadata::ConstPtr& matched_entry) = 0; + + /** @brief 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. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] key. The object used to key the insertion candidate with. + * @param[in] value. The object that the TrajectoryCache was passed to insert. + * @returns True if the insertion candidate should be inserted into the cache. + */ + virtual bool insertPredicate(const moveit::planning_interface::MoveGroupInterface& move_group, const KeyT& key, + const ValueT& value) = 0; + + /** @brief Appends the insert metadata with the features supported by the policy. + * + * See notes in docstrings regarding the feature contract. + * + * @param[in,out] metadata. The metadata to add features to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] key. The object used to key the insertion candidate with. + * @param[in] value. The object that the TrajectoryCache was passed to insert. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the metadata should not be reused, and the TrajectoryCache will abort the insert. + */ + virtual moveit::core::MoveItErrorCode + appendInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, const KeyT& key, + const ValueT& value) = 0; + + /** @brief Resets the state of the policy. */ + virtual void reset() = 0; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/cache_insert_policies/always_insert_never_prune_policy.cpp b/moveit_ros/trajectory_cache/src/cache_insert_policies/always_insert_never_prune_policy.cpp new file mode 100644 index 00000000000..fcf9795258f --- /dev/null +++ b/moveit_ros/trajectory_cache/src/cache_insert_policies/always_insert_never_prune_policy.cpp @@ -0,0 +1,248 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Implementation of a cache insertion policy that always decides to insert and never decides to prune for motion + plan requests. + * + * @see CacheInsertPolicyInterface + * + * @author methylDragon + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::MessageWithMetadata; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; + +using ::moveit_msgs::msg::MotionPlanRequest; +using ::moveit_msgs::msg::RobotTrajectory; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan ==== + +AlwaysInsertNeverPrunePolicy::AlwaysInsertNeverPrunePolicy() : name_("AlwaysInsertNeverPrunePolicy") +{ +} + +std::vector>> +AlwaysInsertNeverPrunePolicy::getSupportedFeatures(double start_tolerance, double goal_tolerance) +{ + std::vector>> out; + + // Start. + out.push_back(std::make_unique()); + out.push_back(std::make_unique(start_tolerance)); + + // Goal. + out.push_back(std::make_unique()); + out.push_back(std::make_unique(goal_tolerance)); + out.push_back(std::make_unique(goal_tolerance)); + out.push_back(std::make_unique(goal_tolerance)); + + return out; +} + +std::string AlwaysInsertNeverPrunePolicy::getName() const +{ + return name_; +} + +MoveItErrorCode AlwaysInsertNeverPrunePolicy::checkCacheInsertInputs(const MoveGroupInterface& move_group, + const MessageCollection& /*coll*/, + const MotionPlanRequest& key, + const MoveGroupInterface::Plan& value) +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, key.workspace_parameters); + + if (value.trajectory.joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Empty joint trajectory points."); + } + if (value.trajectory.joint_trajectory.joint_names.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping plan insert: Empty joint trajectory joint names."); + } + if (!value.trajectory.multi_dof_joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping plan insert: Multi-DOF trajectory plans are not supported."); + } + if (workspace_frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping plan insert: Workspace frame ID cannot be empty."); + } + if (value.trajectory.joint_trajectory.header.frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping plan insert: Trajectory frame ID cannot be empty."); + } + if (workspace_frame_id != value.trajectory.joint_trajectory.header.frame_id) + { + std::stringstream ss; + ss << "Skipping plan insert: Plan request frame (" << workspace_frame_id << ") does not match plan frame (" + << value.trajectory.joint_trajectory.header.frame_id << ")."; + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, ss.str()); + } + + return MoveItErrorCode::SUCCESS; +} + +std::vector::ConstPtr> AlwaysInsertNeverPrunePolicy::fetchMatchingEntries( + const MoveGroupInterface& move_group, const MessageCollection& coll, const MotionPlanRequest& key, + const MoveGroupInterface::Plan& /*value*/, double exact_match_precision) +{ + Query::Ptr query = coll.createQuery(); + + // We avoid the heap allocation from getSupportedFeatures() for performance reasons. + // Furthermore, we set match_precision to zero because we want "exact" matches. + + // Start. + if (MoveItErrorCode ret = + WorkspaceFeatures().appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + }; + if (MoveItErrorCode ret = StartStateJointStateFeatures(/*match_precision=*/0.0) + .appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + }; + + // Goal. + if (MoveItErrorCode ret = MaxSpeedAndAccelerationFeatures().appendFeaturesAsExactFetchQuery(*query, key, move_group, + exact_match_precision); + !ret) + { + return {}; + }; + if (MoveItErrorCode ret = GoalConstraintsFeatures(/*match_precision=*/0.0) + .appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + }; + if (MoveItErrorCode ret = PathConstraintsFeatures(/*match_precision=*/0.0) + .appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + }; + if (MoveItErrorCode ret = TrajectoryConstraintsFeatures(/*match_precision=*/0.0) + .appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + }; + + return coll.queryList(query); +} + +bool AlwaysInsertNeverPrunePolicy::prunePredicate( + const MoveGroupInterface& /*move_group*/, const MotionPlanRequest& /*key*/, + const MoveGroupInterface::Plan& /*value*/, const MessageWithMetadata::ConstPtr& /*matched_entry*/) +{ + return false; // Never prune. +} + +bool AlwaysInsertNeverPrunePolicy::insertPredicate(const MoveGroupInterface& /*move_group*/, + const MotionPlanRequest& /*key*/, + const MoveGroupInterface::Plan& /*value*/) +{ + return true; // Always insert. +} + +MoveItErrorCode AlwaysInsertNeverPrunePolicy::appendInsertMetadata(Metadata& metadata, + const MoveGroupInterface& move_group, + const MotionPlanRequest& key, + const MoveGroupInterface::Plan& value) +{ + // Extract and append features. + // We avoid the heap allocation from getSupportedFeatures() for performance reasons. + + // Start features. + if (MoveItErrorCode ret = WorkspaceFeatures().appendFeaturesAsInsertMetadata(metadata, key, move_group); !ret) + { + return ret; + } + if (MoveItErrorCode ret = + StartStateJointStateFeatures(/*match_tolerance=*/0).appendFeaturesAsInsertMetadata(metadata, key, move_group); + !ret) + { + return ret; + } + + // Goal features. + if (MoveItErrorCode ret = MaxSpeedAndAccelerationFeatures().appendFeaturesAsInsertMetadata(metadata, key, move_group); + !ret) + { + return ret; + } + if (MoveItErrorCode ret = + GoalConstraintsFeatures(/*match_tolerance=*/0).appendFeaturesAsInsertMetadata(metadata, key, move_group); + !ret) + { + return ret; + } + if (MoveItErrorCode ret = + PathConstraintsFeatures(/*match_tolerance=*/0).appendFeaturesAsInsertMetadata(metadata, key, move_group); + !ret) + { + return ret; + } + if (MoveItErrorCode ret = + TrajectoryConstraintsFeatures(/*match_tolerance=*/0).appendFeaturesAsInsertMetadata(metadata, key, move_group); + !ret) + { + return ret; + } + + // Append ValueT metadata. + metadata.append("execution_time_s", + rclcpp::Duration(value.trajectory.joint_trajectory.points.back().time_from_start).seconds()); + metadata.append("planning_time_s", value.planning_time); + + return MoveItErrorCode::SUCCESS; +} + +void AlwaysInsertNeverPrunePolicy::reset() +{ + return; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/test/CMakeLists.txt b/moveit_ros/trajectory_cache/test/CMakeLists.txt index dfd82d7ed5b..c44417e4b7b 100644 --- a/moveit_ros/trajectory_cache/test/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/test/CMakeLists.txt @@ -38,7 +38,7 @@ if(BUILD_TESTING) "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" "test_executable:=test_utils_with_move_group") -# Features ===================================================================== + # Features =================================================================== # Test constant features library. ament_add_gtest_executable(test_constant_features_with_move_group @@ -73,9 +73,22 @@ if(BUILD_TESTING) "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" "test_executable:=test_motion_plan_request_features_with_move_group") -# Cache Insert Policies ======================================================== + # Cache Insert Policies ====================================================== -# Integration Tests ============================================================ + # Test AlwaysInsertNeverPrunePolicy library. + ament_add_gtest_executable(test_always_insert_never_prune_policy_with_move_group + cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp) + target_link_libraries(test_always_insert_never_prune_policy_with_move_group + moveit_ros_trajectory_cache_cache_insert_policies_lib + moveit_ros_trajectory_cache_features_lib + moveit_ros_trajectory_cache_utils_lib + move_group_fixture) + add_ros_test(gtest_with_move_group.py + TARGET test_always_insert_never_prune_policy_with_move_group TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_always_insert_never_prune_policy_with_move_group") + + # Integration Tests ========================================================== # This test executable is run by the pytest_test, since a node is required for # testing move groups diff --git a/moveit_ros/trajectory_cache/test/cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp b/moveit_ros/trajectory_cache/test/cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp new file mode 100644 index 00000000000..f9719c1426e --- /dev/null +++ b/moveit_ros/trajectory_cache/test/cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp @@ -0,0 +1,220 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include + +#include +#include + +#include +#include + +#include "../move_group_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::MessageWithMetadata; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; + +using ::moveit_msgs::msg::MotionPlanRequest; +using ::moveit_msgs::msg::RobotTrajectory; +using ::moveit_ros::trajectory_cache::AlwaysInsertNeverPrunePolicy; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +TEST_F(MoveGroupFixture, AlwaysInsertNeverPrunePolicyChecks) +{ + // Setup. + AlwaysInsertNeverPrunePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + + MotionPlanRequest valid_msg; + MoveGroupInterface::Plan valid_plan; + + // Valid case, as control. + { + valid_msg.workspace_parameters.header.frame_id = "panda_link0"; + valid_plan.trajectory.joint_trajectory.header.frame_id = "panda_link0"; + valid_plan.trajectory.joint_trajectory.joint_names.emplace_back(); + valid_plan.trajectory.joint_trajectory.points.emplace_back(); + + MoveItErrorCode ret = policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, valid_plan); + ASSERT_TRUE(ret) << ret.message; + } + + // Empty joint trajectory points. + { + MotionPlanRequest msg = valid_msg; + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.points.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } + + // Empty joint trajectory names. + { + MotionPlanRequest msg = valid_msg; + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.joint_names.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } + + // Multi-DOF trajectory plan. + { + MotionPlanRequest msg = valid_msg; + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.multi_dof_joint_trajectory.points.emplace_back(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } + + // We can't test workspace ID frame empty. + // But it technically should be unreachable as long as the robot description is correct. + + // Trajectory frame ID empty. + { + MotionPlanRequest msg = valid_msg; + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.header.frame_id = ""; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } + + // Mismatched frames. + { + MotionPlanRequest msg = valid_msg; + MoveGroupInterface::Plan plan = valid_plan; + msg.workspace_parameters.header.frame_id = "panda_link0"; + plan.trajectory.joint_trajectory.header.frame_id = "clearly_a_different_frame"; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } +} + +TEST_F(MoveGroupFixture, AlwaysInsertNeverPrunePolicyWorks) +{ + AlwaysInsertNeverPrunePolicy policy; + + MessageCollection coll = + db_->openCollection("test_db", policy.getName()); + ASSERT_EQ(coll.count(), 0); + + // Setup. Get valid entries to insert. + moveit_msgs::msg::MotionPlanRequest msg; + MoveGroupInterface::Plan plan; + MoveItErrorCode ret = MoveItErrorCode::FAILURE; + + moveit_msgs::msg::MotionPlanRequest another_msg; + MoveGroupInterface::Plan another_plan; + MoveItErrorCode another_ret = MoveItErrorCode::FAILURE; + + do + { + ASSERT_TRUE(move_group_->setPoseTarget(move_group_->getRandomPose())); + move_group_->constructMotionPlanRequest(msg); + ret = move_group_->plan(plan); + } while (!ret); // Sometimes the plan fails with the random pose. + + do + { + ASSERT_TRUE(move_group_->setPoseTarget(move_group_->getRandomPose())); + move_group_->constructMotionPlanRequest(another_msg); + another_ret = move_group_->plan(another_plan); + } while (another_msg == msg && !another_ret); // Also, sometimes the random pose happens to be the same. + + // Ensure that the entries are valid. + { + MoveItErrorCode ret = policy.checkCacheInsertInputs(*move_group_, coll, msg, plan); + ASSERT_TRUE(ret) << ret.message; + } + { + MoveItErrorCode ret = policy.checkCacheInsertInputs(*move_group_, coll, another_msg, another_plan); + ASSERT_TRUE(ret) << ret.message; + } + + // Core test. ==================================================================================== + // NOTE: Be mindful that the policy is stateful. + + // Insert messages and check if policy-specific additional metadata are added. + size_t count = 0; + for (const auto& msg_plan_pair : { std::make_pair(msg, plan), std::make_pair(another_msg, another_plan) }) + { + Metadata::Ptr metadata = coll.createMetadata(); + EXPECT_TRUE(policy.appendInsertMetadata(*metadata, *move_group_, msg_plan_pair.first, msg_plan_pair.second)); + EXPECT_TRUE(metadata->lookupField("execution_time_s")); + EXPECT_TRUE(metadata->lookupField("planning_time_s")); + + // We add two to test the prune predicate, as appropriate. + coll.insert(msg_plan_pair.second.trajectory, metadata); + coll.insert(msg_plan_pair.second.trajectory, metadata); + count += 2; + ASSERT_EQ(coll.count(), count); + } + + // Fetch with features from getSupportedFeatures and fetchMatchingEntries. + // In this case the results should also match with fetchMatchingEntries. + // + // We also test the predicates here. + std::vector>> features = + AlwaysInsertNeverPrunePolicy::getSupportedFeatures(/*start_tolerance=*/0.025, /*goal_tolerance=*/0.001); + + for (const auto& msg_plan_pair : { std::make_pair(msg, plan), std::make_pair(another_msg, another_plan) }) + { + Query::Ptr query = coll.createQuery(); + for (const auto& feature : features) + { + ASSERT_TRUE(feature->appendFeaturesAsExactFetchQuery(*query, msg_plan_pair.first, *move_group_, + /*exact_match_precision=*/0.0001)); + } + + std::vector::ConstPtr> feature_fetch = coll.queryList(query); + std::vector::ConstPtr> policy_fetch = policy.fetchMatchingEntries( + *move_group_, coll, msg_plan_pair.first, msg_plan_pair.second, /*exact_match_precision=*/0.0001); + + ASSERT_EQ(feature_fetch.size(), 2); + ASSERT_EQ(policy_fetch.size(), 2); + for (size_t i = 0; i < feature_fetch.size(); ++i) + { + ASSERT_EQ(*feature_fetch[i], *policy_fetch[i]); + + // Policy is never prune. + EXPECT_FALSE(policy.prunePredicate(*move_group_, msg_plan_pair.first, msg_plan_pair.second, policy_fetch[i])); + } + + // Policy is always insert. + EXPECT_TRUE(policy.insertPredicate(*move_group_, msg_plan_pair.first, msg_plan_pair.second)); + + policy.reset(); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/test/gtest_with_move_group.py b/moveit_ros/trajectory_cache/test/gtest_with_move_group.py index 27aa30ff68e..9487166d8e6 100644 --- a/moveit_ros/trajectory_cache/test/gtest_with_move_group.py +++ b/moveit_ros/trajectory_cache/test/gtest_with_move_group.py @@ -101,7 +101,7 @@ def generate_test_description(): robot_state_publisher, ros2_control_node, *load_controllers, - launch.actions.TimerAction(period=0.1, actions=[gtest_node]), + launch.actions.TimerAction(period=1.0, actions=[gtest_node]), launch_testing.actions.ReadyToTest(), ] ), {