diff --git a/.codespell_words b/.codespell_words index 6be59c42cd..5c3f61b82e 100644 --- a/.codespell_words +++ b/.codespell_words @@ -1,6 +1,7 @@ aas ang ans +AppendT atleast ba brin diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index 933513c375..38d53920c7 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -4,7 +4,7 @@ # Source build of the repos file from the tutorial site ARG ROS_DISTRO=rolling -ARG GZ_VERSION=harmonic +ARG GZ_VERSION=ionic FROM moveit/moveit2:${ROS_DISTRO}-ci LABEL maintainer Tyler Weaver tyler@picknik.ai diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a96cb8299e..ccb076d896 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -32,7 +32,7 @@ jobs: # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option # https://stackoverflow.com/a/41673702 CXXFLAGS: >- - -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: > diff --git a/README.md b/README.md index 9504b3e69c..fd6e7f973d 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the [![Formatting (pre-commit)](https://github.com/moveit/moveit2/actions/workflows/format.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/format.yaml?query=branch%3Amain) [![CI (Rolling and Humble)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml?query=branch%3Amain) -[![Code Coverage](https://codecov.io/gh/ros-planning/moveit2/branch/main/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit2) +[![Code Coverage](https://codecov.io/gh/moveit/moveit2/branch/main/graph/badge.svg?token=QC1O0VzGpm)](https://codecov.io/gh/moveit/moveit2) ## Getting Started diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index bf8ea6df13..d69d120bb1 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -374,6 +374,7 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) scene->world_->removeObject(it.first); scene->removeObjectColor(it.first); scene->removeObjectType(it.first); + scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first); } else { @@ -1465,6 +1466,7 @@ void PlanningScene::removeAllCollisionObjects() world_->removeObject(object_id); removeObjectColor(object_id); removeObjectType(object_id); + getAllowedCollisionMatrixNonConst().removeEntry(object_id); } } } @@ -1939,6 +1941,7 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::msg::Collisi removeObjectColor(object.id); removeObjectType(object.id); + getAllowedCollisionMatrixNonConst().removeEntry(object.id); } return true; } diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index ae697ee8a8..07c0e5a86d 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -590,6 +590,61 @@ TEST(PlanningScene, RobotStateDiffBug) } } +TEST(PlanningScene, UpdateACMAfterObjectRemoval) +{ + auto robot_model = moveit::core::loadTestingRobotModel("panda"); + auto ps = std::make_shared(robot_model); + + const auto object_name = "object"; + collision_detection::CollisionRequest collision_request; + collision_request.group_name = "hand"; + collision_request.verbose = true; + + // Helper function to add an object to the planning scene + auto add_object = [&] { + const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::ADD); + ps->usePlanningSceneMsg(ps1); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{ object_name })); + }; + + // Modify the allowed collision matrix and make sure it is updated + auto modify_acm = [&] { + collision_detection::AllowedCollisionMatrix& acm = ps->getAllowedCollisionMatrixNonConst(); + acm.setEntry(object_name, ps->getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(), + true); + EXPECT_TRUE(ps->getAllowedCollisionMatrix().hasEntry(object_name)); + }; + + // Check collision + auto check_collision = [&] { + collision_detection::CollisionResult res; + ps->checkCollision(collision_request, res); + return res.collision; + }; + + // Test removing a collision object using a diff + add_object(); + EXPECT_TRUE(check_collision()); + modify_acm(); + EXPECT_FALSE(check_collision()); + { + const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE); + ps->usePlanningSceneMsg(ps1); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{})); + EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name)); + } + + // Test removing all objects + add_object(); + // This should report a collision since it's a completely new object + EXPECT_TRUE(check_collision()); + modify_acm(); + EXPECT_FALSE(check_collision()); + ps->removeAllCollisionObjects(); + EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set{})); + EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name)); +} + #ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__) #endif diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp index 54299304c1..195629c098 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp @@ -588,8 +588,10 @@ class JointModelGroup /** * @brief Get the lower and upper position limits of all active variables in the group. - * - * @return std::pair Containing the lower and upper joint limits for all active variables. + * @details In the case of variable without position bounds (e.g. continuous joints), the lower and upper limits are + * set to infinity. + * @return std::pair Containing the lower and upper joint limits for all active + * variables. */ [[nodiscard]] std::pair getLowerAndUpperLimits() const; diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index a9d23c33ea..bed2f9dd0c 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -842,8 +842,10 @@ std::pair JointModelGroup::getLowerAndUpperLim { for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds) { - lower_limits[variable_index] = variable_bounds.min_position_; - upper_limits[variable_index] = variable_bounds.max_position_; + lower_limits[variable_index] = + variable_bounds.position_bounded_ ? variable_bounds.min_position_ : -std::numeric_limits::infinity(); + upper_limits[variable_index] = + variable_bounds.position_bounded_ ? variable_bounds.max_position_ : std::numeric_limits::infinity(); variable_index++; } } diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index d013111c53..2b5459a6dc 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -28,9 +28,62 @@ set(TRAJECTORY_CACHE_DEPENDENCIES trajectory_msgs warehouse_ros) +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 +add_library(moveit_ros_trajectory_cache_utils_lib SHARED src/utils/utils.cpp) +generate_export_header(moveit_ros_trajectory_cache_utils_lib) +target_include_directories( + moveit_ros_trajectory_cache_utils_lib + PUBLIC $ + $) +ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) + +# Features library +add_library( + moveit_ros_trajectory_cache_features_lib SHARED + src/features/motion_plan_request_features.cpp + src/features/get_cartesian_path_request_features.cpp) +generate_export_header(moveit_ros_trajectory_cache_features_lib) +target_link_libraries(moveit_ros_trajectory_cache_features_lib + moveit_ros_trajectory_cache_utils_lib) +target_include_directories( + moveit_ros_trajectory_cache_features_lib + PUBLIC $ + $) +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 SHARED + src/cache_insert_policies/always_insert_never_prune_policy.cpp + src/cache_insert_policies/best_seen_execution_time_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_features_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 SHARED src/trajectory_cache.cpp) generate_export_header(moveit_ros_trajectory_cache_lib) +target_link_libraries( + moveit_ros_trajectory_cache_lib + moveit_ros_trajectory_cache_cache_insert_policies_lib + moveit_ros_trajectory_cache_features_lib + moveit_ros_trajectory_cache_utils_lib) target_include_directories( moveit_ros_trajectory_cache_lib PUBLIC $ @@ -39,7 +92,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib ${TRAJECTORY_CACHE_DEPENDENCIES}) install( - TARGETS moveit_ros_trajectory_cache_lib + TARGETS ${TRAJECTORY_CACHE_LIBRARIES} EXPORT moveit_ros_trajectory_cacheTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -48,29 +101,14 @@ install( DESTINATION include/moveit_ros_trajectory_cache) install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache) -install( - FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h - DESTINATION include/moveit_ros_trajectory_cache) - -if(BUILD_TESTING) - find_package(ament_cmake_pytest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - find_package(rmf_utils REQUIRED) - find_package(warehouse_ros_sqlite REQUIRED) - - # This test executable is run by the pytest_test, since a node is required for - # testing move groups - add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) - target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) - ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite) - - install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) - ament_add_pytest_test( - test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY - "${CMAKE_CURRENT_BINARY_DIR}") +# Install export headers for each library +foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES}) + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h + DESTINATION include/moveit_ros_trajectory_cache) +endforeach() -endif() +add_subdirectory(test) ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES}) diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index 659ee0f7a2..33a1d7eb65 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -1,31 +1,9 @@ -# Trajectory Cache - -``` - * This cache does NOT support collision detection! - * Plans will be put into and fetched from the cache IGNORING collision. - * If your planning scene is expected to change between cache lookups, do NOT - * use this cache, fetched plans are likely to result in collision then. - * - * To handle collisions this class will need to hash the planning scene world - * msg (after zeroing out std_msgs/Header timestamps and sequences) and do an - * appropriate lookup, or otherwise find a way to determine that a planning scene - * is "less than" or "in range of" another (e.g. checking that every obstacle/mesh - * exists within the other scene's). (very out of scope) - ``` +# Fuzzy-Matching Trajectory Cache A trajectory cache based on [`warehouse_ros`](https://github.com/moveit/warehouse_ros) for the move_group planning interface that supports fuzzy lookup for `MotionPlanRequest` and `GetCartesianPath` requests and trajectories. -The cache allows you to insert trajectories and fetch keyed fuzzily on the following: - -- Starting robot (joint) state -- Planning request constraints - - This includes ALL joint, position, and orientation constraints! - - And also workspace parameters, and some others. -- Planning request goal parameters - -It works generically for an arbitrary number of joints, across any number of move_groups. - -Furthermore, the strictness of the fuzzy lookup can be configured for start and goal conditions. +The cache will work on manipulators with an arbitrary number of joints, across any number of move groups. +Furthermore, the cache supports pruning and ranking of fetched trajectories, with extension points for injecting your own feature keying, cache insert, cache prune and cache sorting logic. ## Citations @@ -44,93 +22,165 @@ If you use this package in your work, please cite it using the following: } ``` -## Example usage +## WARNING: The following are unsupported / RFE + +Since this is an initial release, the following features are unsupported because they were a little too difficult for the time I had to implement this. So I am leaving it to the community to help! + +- **!!! This cache does NOT support collision detection, multi-DOF joints, or constraint regions!** + - Trajectories will be put into and fetched from the cache IGNORING collision. If your planning scene is expected to change significantly between cache lookups, it is likely that the fetched plan will result in collisions. + - To natively handle collisions this cache will need to hash the planning scene world msg (after zeroing out std_msgs/Header timestamps and sequences) and do an appropriate lookup, or do more complicated checks to see if the scene world is "close enough" or is a strictly less obstructed version of the scene in the cache entry. +- The fuzzy lookup can't be configured on a per-joint basis. + +That said, there are ways to get around the lack of native collision support to enable use of this cache, such as: +- Validating a fetched plan for collisions before execution. +- Make use of the hybrid planning pipeline, using local planners for collision avoidance, while keeping the cache as a stand-in for a "global planner", where applicable. + +## Example Usage + +**PRE-REQUISITE**: The `warehouse_plugin` ROS parameter must be set to a [`warehouse_ros`](https://github.com/moveit/warehouse_ros) plugin you have installed, which determines what database backend should be used for the cache. ```cpp -// Be sure to set some relevant ROS parameters: -// Relevant ROS Parameters: -// - `warehouse_plugin`: What database to use -auto traj_cache = std::make_shared(node); -traj_cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6); +auto cache = std::make_shared(node); +cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6); + +// The default feature extractors key the cache on starting robot state and goal constraints in the plan request. +// Keyed fuzzily with separate fuzziness on start and goal features. +auto default_features = TrajectoryCache::getDefaultFeatures(start_tolerance, goal_tolerance); + +std::string TrajectoryCache::getDefaultSortFeature(); // Sorts by planned execution time. +move_group.setPoseTarget(...); +moveit_msgs::msg::MotionPlanRequest motion_plan_req_msg; +move_group.constructMotionPlanRequest(motion_plan_request_msg); + +// Use the cache INSTEAD of planning! auto fetched_trajectory = - traj_cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, - _cache_start_match_tolerance, _cache_goal_match_tolerance, - /*sort_by=*/"execution_time_s"); + cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, + /*features=*/default_features, + /*sort_by=*/TrajectoryCache::getDefaultSortFeature(), + /*ascending=*/true); -if (fetched_trajectory) +if (fetched_trajectory) // Great! We got a cache hit, we can execute it. { - // Great! We got a cache hit - // Do something with the plan. + move_group.execute(*fetched_trajectory); } -else +else // Otherwise, plan... And put it for posterity! { - // Plan... And put it for posterity! - traj_cache->insertTrajectory( - *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, /*prune_worse_trajectories=*/true); + moveit::planning_interface::MoveGroupInterface::Plan plan; + if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) + { + cache->insertTrajectory( + *interface, robot_name, std::move(plan_req_msg), std::move(plan), + /*cache_insert_policy=*/BestSeenExecutionTimePolicy(), + /*prune_worse_trajectories=*/true, /*additional_features=*/{}); + } } ``` -It also has the following optimizations: -- Separate caches for separate move groups -- The cache does "canonicalization" of the poses to be relative to the planning frame to increase the chances of cache hits. -- Configurable fuzzy lookup for the keys above. -- It supports "overwriting" of worse trajectories with better trajectories - - If a trajectory with keys extremely close to a pre-existing cache entry is inserted with a better planned execution time, the cache can delete all "worse" trajectories. - - #IsThisMachineLearning - - It also prunes the database and mitigates a lot of query slow-down as the cache grows +## Main Features -## Working Principle +### Overview + +This trajectory cache package supports: +- Inserting and fetching trajectories, keyed fuzzily on any feature of the plan request and plan response. +- Ranking cache entries on any keying feature that is supported (e.g. sorting by execution time). +- Optional cache pruning to keep fetch times and database sizes low. +- Generic support for manipulators with any arbitrary number of joints, across any number of move_groups. +- Cache namespacing and partitioning +- Extension points for injecting your own feature keying, cache insert, cache prune, and cache sorting logic. + +The cache supports `MotionPlanRequest` and `GetCartesianPaths::Request` out of the box! + +### Fully Customizable Behavior + +This trajectory cache allows you to inject your own implementations to affect: +- What features of the plan request and plan response to key the cache on +- What cache insert and cache pruning policy to adopt + +For example, you may decide to write your own feature extractor to key the cache, and decide when to insert or prune a cache entry on features such as: +- Minimum jerk time +- Path length +- Any other feature not supported by this package! + +### Pre-Existing Implementations + +The package provides some starter implementations that covers most general cases of motion planning. -If a plan request has start, goal, and constraint conditions that are "close enough" (configurable per request) to an entry in the cache, then the cached trajectory should be suitable (as long as obstacles have not changed). +For more information, see the implementations of: +- [`FeaturesInterface`](./include/moveit/trajectory_cache/features/features_interface.hpp) +- [`CacheInsertPolicyInterface`](./include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp) -Goal fuzziness is a lot less lenient than start fuzziness by default. +#### Cache Keying Features -Finally, the databases are sharded by move group, and the constraints are added as columns in a name agnostic fashion (they are even somewhat robust to ordering, because they're sorted!) +The following are features of the plan request and response that you can key the cache on. + +These support separately configurable fuzzy lookup on start and goal conditions! +Additionally, these features "canonicalize" the inputs to reduce the cardinality of the cache, increasing the chances of cache hits. (e.g., restating poses relative to the planning frame). + +Supported Features: +- "Start" + - `WorkspaceFeatures`: Planning workspace + - `StartStateJointStateFeatures`: Starting robot joint state +- "Goal" + - `MaxSpeedAndAccelerationFeatures`: Max velocity, acceleration, and cartesian speed limits + - `GoalConstraintsFeatures`: Planning request `goal_constraints` + - This includes ALL joint, position, and orientation constraints (but not constraint regions)! + - `PathConstraintsFeatures`: Planning request `path_constraints` + - `TrajectoryConstraintsFeatures`: Planning request `trajectory_constraints`And also workspace parameters, and some others. +- Planning request goal parameters + +Additionally, support for user-specified features are provided for query-only or cache metadata tagging constant features. (See [`constant_features.hpp`](./include/moveit/trajectory_cache/features/constant_features.hpp)) + +Similar support exists for the cartesian variants of these. + +#### Cache Insert and Pruning Policies + +The following are cache insertion and pruning policies to govern when cache entries are inserted, and how they are (optionally) pruned. + +Supported Cache Insert Policies: +- `BestSeenExecutionTimePolicy`: Only insert best seen execution time, optionally prune on best execution time. +- `AlwaysInsertNeverPrunePolicy`: Always insert, never prune + +## Working Principle + +If a plan request has features (e.g., start, goal, and constraint conditions) that are "close enough" to an entry in the cache, then the cached trajectory should be reusable for that request, allowing us to skip planning. + +The cache extracts these features from the planning request and plan response, storing them in the cache database. +When a new planning request is used to attempt to fetch a matching plan, the cache attempts to fuzzily match the request to pre-existing cache entries keyed on similar requests. +Any "close enough" matches are then returned as valid cache hits for plan reuse, with the definition of "close enough" depending on the type of feature that is being extracted. ## Benefits A trajectory cache helps: -- Cut down on planning time (especially for known moves) +- Cut down on planning time - Allows for consistent predictable behavior of used together with a stochastic planner - It effectively allows you to "freeze" a move -These benefits come from the fact that the cache acts as a lookup table of plans that were already made for a given scenario and constraints, allowing the cache to be substituted for a planner call. The reuse of cached plans then allow you to get predictable execution behavior. +To explain this, consider that planners in MoveIt generally fall under two main camps: stochastic/probabilistic, and optimization based. +The probabilistic planners are fast, but usually non-deterministic, whereas the optimization based ones are usually slow, but deterministic. -A user may also choose when to leverage the cache (e.g. when planning moves from a static "home" position, or repetitive/cartesian moves) to get these benefits. +One way to get around this is to pre-plan and manually select and label robot trajectories to "freeze" in a trajectory database to then replay by name, which avoids needing to spend time to replan, and allows you to ensure that motions are constant and repeatable. +However, this approach is not very flexible and not easily reused. -Additionally, because the cache class has the ability to sort by planned execution time, over sufficient runs, the stochastic plans eventually converge to better and better plans (execution time wise). +The trajectory cache improves upon this approach by allowing a user to "freeze" and store successful plans, but **also** look up those plans in a more generalizable and natural way, using the planning request itself to key the cache, effectively allowing the cache to stand in for a planning call. -You may build abstractions on top of the class, for example, to expose the following behaviors: -- `TrainingOverwrite`: Always plan, and write to cache, deleting all worse trajectories for "matching" cache keys -- `TrainingAppendOnly`: Always plan, and always add to the cache. -- `ExecuteBestEffort`: Rely on cache wherever possible, but plan otherwise. -- `ExecuteReadOnly`: Only execute if cache hit occurred. +Furthermore, the specific properties of this trajectory cache provides further unique benefits: +1. With fuzzy matching, "frozen" plans are much easier to look up and reuse, while simultaneously increasing the chances of a cache hit. +2. The ability to rank trajectories will, if used with a stochastic planner over sufficient runs, cause the cache to eventually converge to increasingly optimal plans. -You can see how such behaviors effectively model the "dev" and "deploy" phases of a robot deployment, and how they could be useful. +Finally, the cache makes use of pruning to optimize fetch times, and also finds ways to "canonicalize" features of the keying request to increase chances of a cache hit. ## Best Practices -- Since this cache does not yet support collisions, ensure the planning scene and obstacles remain static -- Have looser start fuzziness, and stricter goal fuzziness -- Move the robot to static known poses where possible before planning to increase the chances of a cache hit +- Since this cache does not yet support collisions, ensure the planning scene and obstacles remain static, or always validate the fetched plan for collisions +- When using the default cache features, have looser start fuzziness, and stricter goal fuzziness +- Move the robot to fixed starting poses where possible before planning to increase the chances of a cache hit - Use the cache where repetitive, non-dynamic motion is likely to occur (e.g. known plans, short planned moves, etc.) -## WARNING: The following are unsupported / RFE - -Since this is an initial release, the following features are unsupported because they were a little too difficult for the time I had to implement this. So I am leaving it to the community to help! +Additionally, you may build abstractions on top of the class, for example, to expose the following behaviors: +- `TrainingOverwrite`: Always plan, and write to cache, pruning all worse trajectories for "matching" cache keys +- `TrainingAppendOnly`: Always plan, and always add to the cache. +- `ExecuteBestEffort`: Rely on cache wherever possible, but plan otherwise. +- `ExecuteReadOnly`: Only execute if cache hit occurred. -- **!!! This cache does NOT support collision detection!** - - Trajectories will be put into and fetched from the cache IGNORING collision. - - If your planning scene is expected to change between cache lookups, do NOT use this cache, fetched trajectories are likely to result in collision then. - - To handle collisions this class will need to hash the planning scene world msg (after zeroing out std_msgs/Header timestamps and sequences) and do an appropriate lookup, or do more complicated checks to see if the scene world is "close enough" or is a less obstructed version of the scene in the cache entry. - - !!! This cache does NOT support keying on joint velocities and efforts. - - The cache only keys on joint positions. -- !!! This cache does NOT support multi-DOF joints. -- !!! This cache does NOT support certain constraints - - 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) +You can see how such behaviors effectively model the "dev" and "deploy" phases of a robot deployment, and how they could be useful. 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 0000000000..86951dcf70 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp @@ -0,0 +1,215 @@ +// 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. + * + * This is mainly for explanatory purposes. + * @see CacheInsertPolicyInterface + * + * @author methylDragon + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// ================================================================================================= +// AlwaysInsertNeverPrunePolicy. +// ================================================================================================= +// 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 shouldPruneMatchingEntry( + 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& matching_entry, + std::string* reason = nullptr) override; + + bool shouldInsert(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value, + std::string* reason = nullptr) 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_; + std::vector>> exact_matching_supported_features_; +}; + +// ================================================================================================= +// CartesianAlwaysInsertNeverPrunePolicy. +// ================================================================================================= +// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response + +/** @class CartesianAlwaysInsertNeverPrunePolicy + * + * @brief 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: + * - fraction + * - execution_time_s + * + * 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: + * - CartesianWorkspaceFeatures + * - CartesianStartStateJointStateFeatures + * - CartesianMaxSpeedAndAccelerationFeatures + * - CartesianMaxStepAndJumpThresholdFeatures + * - CartesianWaypointsFeatures + * - CartesianPathConstraintsFeatures + * + * @see get_cartesian_path_request_features.hpp + * @see constant_features.hpp + * @see FeaturesInterface + * + * 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. + */ +class CartesianAlwaysInsertNeverPrunePolicy 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, double min_fraction); + + CartesianAlwaysInsertNeverPrunePolicy(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::srv::GetCartesianPath::Request& key, + const moveit_msgs::srv::GetCartesianPath::Response& value) override; + + std::vector::ConstPtr> + fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::srv::GetCartesianPath::Request& key, + const moveit_msgs::srv::GetCartesianPath::Response& value, + double exact_match_precision) override; + + 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::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; + + 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; + + void reset() override; + +private: + const std::string name_; + std::vector>> + exact_matching_supported_features_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/best_seen_execution_time_policy.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/best_seen_execution_time_policy.hpp new file mode 100644 index 0000000000..441ac50859 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/best_seen_execution_time_policy.hpp @@ -0,0 +1,246 @@ +// 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 only decides to insert if the motion plan is the one with + * the shortest execution time seen so far amongst exactly matching MotionPlanRequests. + * + * @see CacheInsertPolicyInterface + * + * @author methylDragon + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// ================================================================================================= +// BestSeenExecutionTimePolicy. +// ================================================================================================= +// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan + +/** @class BestSeenExecutionTimePolicy + * + * @brief A cache insertion policy that only decides to insert if the motion plan is the one with + * the shortest execution time seen so far amongst exactly matching MotionPlanRequests. + * + * 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 and Pruning + * ^^^^^^^^^^^^^^^^^^^ + * A matching cache entry is one that has a MotionPlanRequest 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 indicates that pruning should happen if there are any exactly matching plans that are + * worse than the insertion candidate. + * + * REMINDER: The TrajectoryCache still decides to honor the prune indication or not, based off the + * parameters passed to the insert call. + * + * Insertion + * ^^^^^^^^^ + * This policy indicates that insertion should happen if the candidate plan is the best seen in + * terms of shortest execution time than other plans with exactly matching MotionPlanRequest requests. + * + * This policy aggregates state in the fetchMatchingEntries call to facilitate this. + */ +class BestSeenExecutionTimePolicy 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); + + BestSeenExecutionTimePolicy(); + + 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 shouldPruneMatchingEntry( + 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& matching_entry, + std::string* reason = nullptr) override; + + bool shouldInsert(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& key, + const moveit::planning_interface::MoveGroupInterface::Plan& value, + std::string* reason = nullptr) 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_; + std::vector>> exact_matching_supported_features_; + + double best_seen_execution_time_; +}; + +// ================================================================================================= +// CartesianBestSeenExecutionTimePolicy. +// ================================================================================================= +// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response + +/** @class CartesianBestSeenExecutionTimePolicy + * + * @brief A cache insertion policy that only decides to insert if the motion plan is the one with + * the shortest execution time seen so far amongst exactly matching GetCartesianPath requests. + * + * Supported Metadata and Features + * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + * Appends the following additional metadata, which can be used for querying and sorting: + * - fraction + * - execution_time_s + * + * 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: + * - CartesianWorkspaceFeatures + * - CartesianStartStateJointStateFeatures + * - CartesianMaxSpeedAndAccelerationFeatures + * - CartesianMaxStepAndJumpThresholdFeatures + * - CartesianWaypointsFeatures + * - CartesianPathConstraintsFeatures + * + * @see get_cartesian_path_request_features.hpp + * @see constant_features.hpp + * @see FeaturesInterface + * + * Matches and Pruning + * ^^^^^^^^^^^^^^^^^^^ + * A matching cache entry is one that has a GetCartesianPath request 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 indicates that pruning should happen if there are any exactly matching plans that are + * worse than the insertion candidate. + * + * REMINDER: The TrajectoryCache still decides to honor the prune indication or not, based off the + * parameters passed to the insert call. + * + * Insertion + * ^^^^^^^^^ + * This policy indicates that insertion should happen if the candidate plan is the best seen in + * terms of shortest execution time than other plans with exactly matching GetCartesianPath requests. + * + * This policy aggregates state in the fetchMatchingEntries call to facilitate this. + */ +class CartesianBestSeenExecutionTimePolicy 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, double min_fraction); + + CartesianBestSeenExecutionTimePolicy(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::srv::GetCartesianPath::Request& key, + const moveit_msgs::srv::GetCartesianPath::Response& value) override; + + std::vector::ConstPtr> + fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group, + const warehouse_ros::MessageCollection& coll, + const moveit_msgs::srv::GetCartesianPath::Request& key, + const moveit_msgs::srv::GetCartesianPath::Response& value, + double exact_match_precision) override; + + 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::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; + + 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; + + void reset() override; + +private: + const std::string name_; + std::vector>> + exact_matching_supported_features_; + + double best_seen_execution_time_; +}; + +} // 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 0000000000..5eca3fe1d7 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp @@ -0,0 +1,222 @@ +// 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 Abstract template class for injecting logic for determining when to prune and insert a + * cache entry, and what metadata to attach to the cache entry. + * + * @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 object stored in 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 `shouldPruneMatchingEntry` 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. shouldPruneMatchingEntry, once per fetched entry from fetchMatchingEntries + * 4. shouldInsert, once + * 5. appendInsertMetadata, once, if shouldInsert 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. + * + * 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. + * + * @see CacheInsertPolicyInterface#shouldPruneMatchingEntry + * + * @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 only the metadata 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] matching_entry. The matched cache entry to be possibly pruned. + * @param[out] reason. The reason for the returned result. + * @returns True if the cache entry should be pruned. + */ + virtual bool + shouldPruneMatchingEntry(const moveit::planning_interface::MoveGroupInterface& move_group, const KeyT& key, + const ValueT& value, + const typename warehouse_ros::MessageWithMetadata::ConstPtr& matching_entry, + std::string* reason) = 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. + * @param[out] reason. The reason for the returned result. + * @returns True if the insertion candidate should be inserted into the cache. + */ + virtual bool shouldInsert(const moveit::planning_interface::MoveGroupInterface& move_group, const KeyT& key, + const ValueT& value, std::string* reason) = 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/include/moveit/trajectory_cache/features/constant_features.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/constant_features.hpp new file mode 100644 index 0000000000..1d75c20ad9 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/constant_features.hpp @@ -0,0 +1,269 @@ +// 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 constant_features.hpp + * @brief User-specified constant features to key the trajectory cache on. + * + * This allows a user to specify custom information to populate a fetch query or insert metadata that might not have + * been obtained via extracting from a FeatureSourceT. + * + * @see FeaturesInterface + * + * @author methylDragon + */ + +#pragma once + +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// Queries. ======================================================================================== + +/** @class QueryOnlyEqFeature + * @brief Appends an equals query, with no metadata. + */ +template +class QueryOnlyEqFeature final : public FeaturesInterface +{ +public: + QueryOnlyEqFeature(std::string name, AppendT value) : name_(std::move(name)), value_(value) + { + } + + std::string getName() const override + { + return "QueryOnlyEqFeature." + name_; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override + { + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); + } + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + query.append(name_, value_); + return moveit::core::MoveItErrorCode::SUCCESS; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& /*metadata*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + +private: + std::string name_; + AppendT value_; +}; + +/** @class QueryOnlyLTEFeature + * @brief Appends a less-than or equal-to query, with no metadata. + */ +template +class QueryOnlyLTEFeature final : public FeaturesInterface +{ +public: + QueryOnlyLTEFeature(std::string name, AppendT value) : name_(std::move(name)), value_(value) + { + } + + std::string getName() const override + { + return "QueryOnlyLTEFeature." + name_; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override + { + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); + } + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + query.appendLTE(name_, value_); + return moveit::core::MoveItErrorCode::SUCCESS; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& /*metadata*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + +private: + std::string name_; + AppendT value_; +}; + +/** @class QueryOnlyGTEFeature + * @brief Appends a less-than or equal-to query, with no metadata. + */ +template +class QueryOnlyGTEFeature final : public FeaturesInterface +{ +public: + QueryOnlyGTEFeature(std::string name, AppendT value) : name_(std::move(name)), value_(value) + { + } + + std::string getName() const override + { + return "QueryOnlyGTEFeature." + name_; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override + { + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); + } + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + query.appendGTE(name_, value_); + return moveit::core::MoveItErrorCode::SUCCESS; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& /*metadata*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + +private: + std::string name_; + AppendT value_; +}; + +/** @class QueryOnlyRangeInclusiveWithToleranceFeature + * @brief Appends a less-than or equal-to query, with no metadata. + */ +template +class QueryOnlyRangeInclusiveWithToleranceFeature final : public FeaturesInterface +{ +public: + QueryOnlyRangeInclusiveWithToleranceFeature(std::string name, AppendT lower, AppendT upper) + : name_(std::move(name)), lower_(lower), upper_(upper) + { + } + + std::string getName() const override + { + return "QueryOnlyRangeInclusiveWithToleranceFeature." + name_; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override + { + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); + } + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + query.appendRangeInclusive(name_, lower_, upper_); + return moveit::core::MoveItErrorCode::SUCCESS; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& /*metadata*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + +private: + std::string name_; + AppendT lower_; + AppendT upper_; +}; + +// Metadata. ======================================================================================= + +/** @class MetadataOnlyFeature + * @brief Appends a single metadata value, with no query. + */ +template +class MetadataOnlyFeature final : public FeaturesInterface +{ +public: + MetadataOnlyFeature(std::string name, AppendT value) : name_(std::move(name)), value_(value) + { + } + + std::string getName() const override + { + return "MetadataOnlyFeature." + name_; + } + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& /*query*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& /*query*/, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const override + { + return moveit::core::MoveItErrorCode::SUCCESS; // No-op. + } + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const FeatureSourceT& /*source*/, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const override + { + metadata.append(name_, value_); + return moveit::core::MoveItErrorCode::SUCCESS; + } + +private: + std::string name_; + AppendT value_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/features_interface.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/features_interface.hpp new file mode 100644 index 0000000000..355a0e01f9 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/features_interface.hpp @@ -0,0 +1,156 @@ +// 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 Abstract template class for extracting features from some FeatureSourceT. + * @author methylDragon + */ + +#pragma once + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +/** @interface FeaturesInterface + * @headerfile features_interface.hpp "moveit/trajectory_cache/features/features_interface.hpp" + * + * @brief Abstract class for extracting features from arbitrary type FeatureSourceT to append to + * warehouse_ros::Query and warehouse_ros::Metadata for keying TrajectoryCache entries with. + * + * @tparam FeatureSourceT. The object to extract features from. + * + * The features that are extracted from the FeatureSourceT can be used to key the TrajectoryCache + * cache entries in a fuzzy and exact manner. + * + * Users may implement this interface to add additional keying functionality to the cache beyond the + * ones provided by this package. + * + * @see TrajectoryCache + * + * Usage + * ^^^^^ + * In order for a cache entry to be fetched using an implementation of FeaturesInterface, + * it must also have been put with the same implementation, as fetching a cache entry via some + * features requires that the features were added to the cache entry's metadata. + * + * This typically means adding implementations of FeaturesInterface as arguments to + * the TrajectoryCache class's insertion methods. Or by using an appropriate CacheInsertPolicyInterface that composes the set of FeaturesInterface instances you are concerned with. + * + * Be sure to check the appropriate CacheInsertPolicyInterface + * implementations' docstrings to see what FeaturesInterface they support, and make + * sure to only use a subset of those, unless you explicitly add additional metadata by providing + * the appropriate additional FeaturesInterface instances on cache insertion. + * + * @see CacheInsertPolicyInterface + * + * Composite Keys + * ^^^^^^^^^^^^^^ + * Multiple unique instances of FeaturesInterface can be used together to express + * composite key expressions, allowing you to constrain your match on a larger set of metadata + * (provided the metadata was added to the cache entry by a superset of the same set of + * FeaturesInterface instances used to fetch). + * + * For example, the following can be used together to more completely key a cache entry: + * - A FeaturesInterface that appends workspace information + * - A FeaturesInterface that appends robot joint state + * - A FeaturesInterface that appends the final pose goal + * + * You may then fetch that cache entry with a query formed from a subset of the instances used to key it, e.g.: + * - A FeaturesInterface that appends workspace information + * - A FeaturesInterface that appends robot joint state + * + * WARNING: + * Care must be taken to ensure that there are no collisions between the names of the query or + * metadata being added amongst the different FeaturesInterface interfaces being + * used together. + * + * A good way of preventing this is adding a prefix. + * + * User-Specified Keys + * ^^^^^^^^^^^^^^^^^^^ + * You may also implement this interface to constrain a fetch query or tag cache entry metadata with + * user-specified parameters that do not depend on FeatureSourceT or any other arguments. + * + * Simply ignore any passed arguments as necessary, and ensure that the correct information is + * appended in the append implementations as appropriate. + * + * @see constant_features.hpp + */ +template +class FeaturesInterface +{ +public: + virtual ~FeaturesInterface() = default; + + /** @brief Gets the name of the features implementation. */ + virtual std::string getName() const = 0; + + /** + * @brief Extracts relevant features from FeatureSourceT, to be appended to a fetch query, with fuzzy matching. + * + * These parameters will be used key the cache element in a fuzzy manner. + * + * @param[in,out] query. The query to add features to. + * @param[in] source. A FeatureSourceT to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the query should not be reused. + */ + virtual moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const = 0; + + /** + * @brief Extracts relevant features from FeatureSourceT, to be appended to a fetch query, with exact matching. + * + * These parameters will be used key the cache element in an exact manner. + * + * @param[in,out] query. The query to add features to. + * @param[in] source. A FeatureSourceT to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the query should not be reused. + */ + virtual moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const = 0; + + /** + * @brief Extracts relevant features from FeatureSourceT, to be appended to a cache entry's metadata. + * + * These parameters will be used key the cache element. + * + * @param[in,out] metadata. The metadata to add features to. + * @param[in] source. A FeatureSourceT to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the metadata should not be reused. + */ + virtual moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const FeatureSourceT& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const = 0; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp new file mode 100644 index 0000000000..3e8dfdbf4f --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/get_cartesian_path_request_features.hpp @@ -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 moveit_msgs::srv::GetCartesianPath::Request features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#pragma once + +#include +#include +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +/** @class CartesianWorkspaceFeatures + * @brief Extracts group name and frame ID from the plan request. + */ +class CartesianWorkspaceFeatures final : public FeaturesInterface +{ +public: + CartesianWorkspaceFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianStartStateJointStateFeatures + * @brief Extracts details of the joint state from the `start_state` field in the plan request. + * + * The start state will always be re-interpreted into explicit joint state positions. + * + * WARNING: + * MultiDOF joints and attached collision objects are not supported. + */ +class CartesianStartStateJointStateFeatures final + : public FeaturesInterface +{ +public: + CartesianStartStateJointStateFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +// "Goal" features. ================================================================================ + +/** @class CartesianMaxSpeedAndAccelerationFeatures + * @brief Extracts max velocity and acceleration scaling, and cartesian speed limits from the plan request. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If the max scaling factors are outside the range of (0, 1], they will be set to 1. + * If max_cartesian_speed is <= 0, it will be ignored instead. + */ +class CartesianMaxSpeedAndAccelerationFeatures final + : public FeaturesInterface +{ +public: + CartesianMaxSpeedAndAccelerationFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianMaxStepAndJumpThresholdFeatures + * @brief Extracts max step and jump thresholds from the plan request. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If a jump threshold is set to 0, it will be ignored instead. + */ +class CartesianMaxStepAndJumpThresholdFeatures final + : public FeaturesInterface +{ +public: + CartesianMaxStepAndJumpThresholdFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class CartesianWaypointsFeatures + * @brief Extracts features from the `waypoints` and `link_name` field in the plan request. + * + * `link_name` is extracted here because it is what the waypoints are stated with reference to. + * Additionally, the waypoints will be restated in the robot's model frame. + * + * NOTE: In accordance with the source message's field descriptions: + * If link_name is empty, uses move_group.getEndEffectorLink(). + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class CartesianWaypointsFeatures final : public FeaturesInterface +{ +public: + CartesianWaypointsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class CartesianPathConstraintsFeatures + * @brief Extracts features from the `path_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class CartesianPathConstraintsFeatures final : public FeaturesInterface +{ +public: + CartesianPathConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, + const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::srv::GetCartesianPath::Request& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp new file mode 100644 index 0000000000..3e385720db --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp @@ -0,0 +1,266 @@ +// 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 moveit_msgs::msg::MotionPlanRequest features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +/** @class WorkspaceFeatures + * @brief Extracts group name and details of the `workspace_parameters` field in the plan request. + * + * A cache hit with this feature is valid if the requested planning constraints has a workspace that + * completely subsumes a cached entry's workspace. + * + * For example: (We ignore z for simplicity) + * If workspace is defined by the extrema (x_min, y_min, x_max, y_max), + * + * Potential valid match if other constraints fulfilled: + * Request: (-1, -1, 1, 1) + * Plan in cache: (-0.5, -0.5, 0.5, 0.5) + * + * No match, since this plan might cause the end effector to go out of bounds.: + * Request: (-1, -1, 1, 1) + * Plan in cache: (-2, -0.5, 0.5, 0.5) + */ +class WorkspaceFeatures final : public FeaturesInterface +{ +public: + WorkspaceFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class StartStateJointStateFeatures + * @brief Extracts details of the joint state from the `start_state` field in the plan request. + * + * The start state will always be re-interpreted into explicit joint state positions. + * + * WARNING: + * MultiDOF joints and attached collision objects are not supported. + * + * @see appendRobotStateJointStateAsFetchQueryWithTolerance + * @see appendRobotStateJointStateAsInsertMetadata + */ +class StartStateJointStateFeatures final : public FeaturesInterface +{ +public: + StartStateJointStateFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +// "Goal" features. ================================================================================ + +/** @class MaxSpeedAndAccelerationFeatures + * @brief Extracts max velocity and acceleration scaling, and cartesian speed limits from the plan request. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If the max scaling factors are outside the range of (0, 1], they will be set to 1. + * If max_cartesian_speed is <= 0, it will be ignored instead. + */ +class MaxSpeedAndAccelerationFeatures final : public FeaturesInterface +{ +public: + MaxSpeedAndAccelerationFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class GoalConstraintsFeatures + * @brief Extracts features from the `goal_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class GoalConstraintsFeatures final : public FeaturesInterface +{ +public: + GoalConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class PathConstraintsFeatures + * @brief Extracts features from the `path_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class PathConstraintsFeatures final : public FeaturesInterface +{ +public: + PathConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class TrajectoryConstraintsFeatures + * @brief Extracts features from the `trajectory_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class TrajectoryConstraintsFeatures final : public FeaturesInterface +{ +public: + TrajectoryConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 87288cba4d..d6c3524aa4 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -21,25 +21,25 @@ #include #include -#include +#include #include #include #include -// TF2 -#include -#include - -// ROS2 Messages #include #include // moveit modules #include +#include +#include #include +#include +#include + namespace moveit_ros { namespace trajectory_cache @@ -51,14 +51,28 @@ namespace trajectory_cache * * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` * by using `warehouse_ros` to manage a database of executed trajectories, keyed - * by the start and goal conditions, and sorted by how long the trajectories - * took to execute. This allows for the lookup and reuse of the best performing - * trajectories found so far. + * with injectable feature extractors, and pruned and inserted by cache insert + * policies. This allows for the lookup and reuse of the best performing + * trajectories found so far in a user-specified manner. + * + * Trajectories may be looked up with some tolerance at call time. + * + * The following ROS Parameters MUST be set: + * - `warehouse_plugin`: What database to use + * + * This class supports trajectories planned from move_group MotionPlanRequests + * as well as GetCartesianPath requests. That is, both normal motion plans and + * cartesian plans are supported. + * + * A cache fetch is intended to be usable as a stand-in for the + * MoveGroupInterface `plan` and `computeCartesianPath` methods. * * WARNING: RFE: - * !!! This cache does NOT support collision detection! - * Trajectories will be put into and fetched from the cache IGNORING - * collision. + * !!! The default set of feature extractors and cache insert policies do + * NOT support collision detection! + * + * Trajectories using them will be inserted into and fetched from the cache + * IGNORING collision. * * If your planning scene is expected to change between cache lookups, do NOT * use this cache, fetched trajectories are likely to result in collision @@ -70,58 +84,120 @@ namespace trajectory_cache * is "close enough" or is a less obstructed version of the scene in the cache * entry. * - * !!! This cache does NOT support keying on joint velocities and efforts. + * Alternatively, use your planning scene after fetching the cache entry to + * validate if the cached trajectory will result in collisions or not. + * + * !!! They also do NOT support keying on joint velocities and efforts. * The cache only keys on joint positions. * - * !!! This cache does NOT support multi-DOF joints. + * !!! They also do NOT support multi-DOF joints. - * !!! This cache does NOT support certain constraints - * Including: path, constraint regions, everything related to collision. + * !!! They also do NOT support certain constraints + * Including: constraint regions, everything related to collision. * * This is because they are difficult (but not impossible) to implement key * logic for. * - * Relevant ROS Parameters: - * - `warehouse_plugin`: What database to use + * Thread-Safety + * ^^^^^^^^^^^^^ + * This class is NOT thread safe. Synchronize use of it if you need it in + * multi-threaded contexts. * - * This class supports trajectories planned from move_group MotionPlanRequests - * as well as GetCartesianPath requests. That is, both normal motion plans and - * cartesian plans are supported. + * Injectable Feature Extraction and Cache Insert Policies + * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + * The specific features of cache entries and cache insertion candidates is + * determined by passing the TrajectoryCache's insert and fetch methods with the + * appropriate FeaturesInterface implementations, which will + * extract and append the appropriate features to the query. + * + * Similarly, a CacheInsertPolicyInterface + * implementation must be passed to the insert method to determine what pruning + * and insertion logic to apply. * + * Each cache insert policy implementation constrains what features can be used + * to fetch cache entries inserted with them. See the related interface classes + * for more information. + * + * @see FeaturesInterface + * @see CacheInsertPolicyInterface + * + * This class provides a few helper methods to have "default" behavior of: + * - Fetching and caching on "start" and "goal" parameters + * - Pruning on `execution_time_s` + * - Sorting on `execution_time_s` + * + * @see TrajectoryCache::getDefaultFeatures + * @see TrajectoryCache::getDefaultCacheInsertPolicy + * + * @see TrajectoryCache::getDefaultCartesianFeatures + * @see TrajectoryCache::getDefaultCartesianCacheInsertPolicy + * + * @see TrajectoryCache::getDefaultSortFeature + * + * Cache Sections + * ^^^^^^^^^^^^^^ * Motion plan trajectories are stored in the `move_group_trajectory_cache` * database within the database file, with trajectories for each move group * stored in a collection named after the relevant move group's name. * * For example, the "my_move_group" move group will have its cache stored in - * `move_group_trajectory_cache@my_move_group` - * - * Motion Plan Trajectories are keyed on: - * - Plan Start: robot joint state - * - Plan Goal (either of): - * - Final pose (wrt. `planning_frame` (usually `base_link`)) - * - Final robot joint states - * - Plan Constraints (but not collision) - * - * Trajectories may be looked up with some tolerance at call time. + * `move_group_trajectory_cache@my_move_group`. * * Similarly, the cartesian trajectories are stored in the * `move_group_cartesian_trajectory_cache` database within the database file, * with trajectories for each move group stored in a collection named after the * relevant move group's name. - * - * Cartesian Trajectories are keyed on: - * - Plan Start: robot joint state - * - Plan Goal: - * - Pose waypoints */ class TrajectoryCache { public: + /** + * @name Default cache behavior helpers. + */ + /**@{*/ + + /** @brief Gets the default features for MotionPlanRequest messages. + * @see BestSeenExecutionTimePolicy::getDefaultFeatures + */ + static std::vector>> + getDefaultFeatures(double start_tolerance, double goal_tolerance); + + /** @brief Gets the default cache insert policy for MotionPlanRequest messages. + * @see BestSeenExecutionTimePolicy + */ + static std::unique_ptr> + getDefaultCacheInsertPolicy(); + + /** @brief Gets the default features for GetCartesianPath requests. + * @see CartesianBestSeenExecutionTimePolicy::getDefaultFeatures + */ + static std::vector>> + getDefaultCartesianFeatures(double start_tolerance, double goal_tolerance, double min_fraction); + + /** @brief Gets the default cache insert policy for GetCartesianPath requests. + * @see CartesianBestSeenExecutionTimePolicy + */ + static std::unique_ptr< + CacheInsertPolicyInterface> + getDefaultCartesianCacheInsertPolicy(); + + /** @brief Gets the default sort feature. */ + static std::string getDefaultSortFeature(); + + /**@}*/ + + /** + * @name Cache configuration. + */ + /**@{*/ + /** * @brief Constructs a TrajectoryCache. * - * @param[in] node. An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters, log, and listen - * for TF. + * @param[in] node. An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters and log. * * TODO: methylDragon - * We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports it... @@ -138,7 +214,7 @@ class TrajectoryCache * An exact match is when: * (candidate >= value - (exact_match_precision / 2) * && candidate <= value + (exact_match_precision / 2)) - * @property num_additional_trajectories_to_preserve_when_deleting_worse. The number of additional cached trajectories + * @property num_additional_trajectories_to_preserve_when_pruning_worse. The number of additional cached trajectories * to preserve when `prune_worse_trajectories` is true. It is useful to keep more than one matching trajectory to * have alternative trajectories to handle obstacles. */ @@ -148,7 +224,7 @@ class TrajectoryCache uint32_t db_port = 0; double exact_match_precision = 1e-6; - size_t num_additional_trajectories_to_preserve_when_deleting_worse = 1; + size_t num_additional_trajectories_to_preserve_when_pruning_worse = 1; }; /** @@ -159,11 +235,17 @@ class TrajectoryCache * * @param[in] options. An instance of TrajectoryCache::Options to initialize the cache with. * @see TrajectoryCache::Options - * @returns true if the database was successfully connected to. - * @throws When options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1. + * @returns True if the database was successfully connected to. * */ bool init(const Options& options); + /**@}*/ + + /** + * @name Getters and setters. + */ + /**@{*/ + /** * @brief Count the number of non-cartesian trajectories for a particular cache namespace. * @@ -180,29 +262,24 @@ class TrajectoryCache */ unsigned countCartesianTrajectories(const std::string& cache_namespace); - /** - * @name Getters and setters. - */ - /**@{*/ - - /// @brief Gets the database path. + /** @brief Gets the database path. */ std::string getDbPath() const; - /// @brief Gets the database port. + /** @brief Gets the database port. */ uint32_t getDbPort() const; - /// @brief Gets the exact match precision. + /** @brief Gets the exact match precision. */ double getExactMatchPrecision() const; - /// @brief Sets the exact match precision. + /** @brief Sets the exact match precision. */ void setExactMatchPrecision(double exact_match_precision); - /// @brief Get the number of trajectories to preserve when deleting worse trajectories. - size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const; + /** @brief Get the number of trajectories to preserve when pruning worse trajectories. */ + size_t getNumAdditionalTrajectoriesToPreserveWhenPruningWorse() const; - /// @brief Set the number of additional trajectories to preserve when deleting worse trajectories. - void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( - size_t num_additional_trajectories_to_preserve_when_deleting_worse); + /** @brief Set the number of additional trajectories to preserve when pruning worse trajectories. */ + void setNumAdditionalTrajectoriesToPreserveWhenPruningWorse( + size_t num_additional_trajectories_to_preserve_when_pruning_worse); /**@}*/ @@ -212,72 +289,76 @@ class TrajectoryCache /**@{*/ /** - * @brief Fetches all plans that fit within the requested tolerances for start and goal conditions, returning them as - * a vector, sorted by some cache column. + * @brief Fetches all trajectories keyed on user-specified features, returning them as a vector, + * sorted by some cache feature. + * + * @see FeaturesInterface * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The motion plan request to key the cache with. - * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * @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] plan_request. The motion plan request to extract features from to key the cache with. + * @param[in] features. The features to key the cache with. + * @param[in] sort_by. The cache feature to sort by. * @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. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @returns A vector of cache hits, sorted by the `sort_by` parameter. */ std::vector::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", bool ascending = true) const; + fetchAllMatchingTrajectories( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending = true, bool metadata_only = false) const; /** - * @brief Fetches the best trajectory that fits within the requested tolerances for start and goal conditions, by some - * cache column. + * @brief Fetches the best trajectory keyed on user-specified features, with respect to some cache feature. + * + * @see FeaturesInterface * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The motion plan request to key the cache with. - * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] plan_request. The motion plan request to extract features from to key the cache with. + * @param[in] features. The features to key the cache with. * @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] sort_by. The cache feature to sort by. * @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. + * @returns The best cache hit, with respect to the `sort_by` parameter. */ warehouse_ros::MessageWithMetadata::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 ascending = true) const; + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending = true, bool metadata_only = false) const; /** - * @brief Inserts 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 are higher priority in the sorting order specified by `sort_by` than another - * exactly matching trajectory. - * - * 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_precision` arg passed in init(). - * @see init() + * @brief Inserts a trajectory into the database, with user-specified insert policy. * * Optionally deletes all worse trajectories by default to prune the cache. * + * @see FeaturesInterface + * @see CacheInsertPolicyInterface + * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The motion plan request to key the cache with. - * @param[in] trajectory. The trajectory to put. - * @param[in] execution_time_s. The execution time of the trajectory, in seconds. - * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. - * @param[in] prune_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the - * `plan_request` exactly, if they are worse than the `trajectory`, even if it was not put. - * @returns true if the trajectory was the best seen yet and hence put into the cache. + * @param[in] plan_request. The motion plan request to extract features from to key the cache with. + * @param[in] plan. The plan containing the trajectory to insert. + * @param[in,out] cache_insert_policy. The cache insert policy to use. Will determine what features can be used to + * fetch entries, and pruning and insertion logic. Will be reset at the end of the call. + * @param[in] prune_worse_trajectories. If true, will prune the cache according to the `cache_insert_policy`'s pruning + * logic. + * @param[in] additional_features. Additional features to key the cache with. Must not intersect with the set of + * features supported by the `cache_insert_policy`. + * @returns True if the trajectory was inserted into the cache. */ bool insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool prune_worse_trajectories = true); + const moveit::planning_interface::MoveGroupInterface::Plan& plan, + CacheInsertPolicyInterface& cache_insert_policy, + bool prune_worse_trajectories = true, + const std::vector>>& + additional_features = {}); /**@}*/ @@ -287,257 +368,86 @@ class TrajectoryCache /**@{*/ /** - * @brief Constructs a GetCartesianPath request. + * @brief Fetches all cartesian trajectories keyed on user-specified features, returning them as a + * vector, sorted by some cache feature. * - * This mimics the move group computeCartesianPath signature (without path constraints). - * - * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. - * @param[in] waypoints. The cartesian waypoints to request the path for. - * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. - * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. - * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. - * @returns - */ - moveit_msgs::srv::GetCartesianPath::Request - constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double max_step, - double jump_threshold, bool avoid_collisions = true); - - /** - * @brief Fetches all cartesian trajectories that fit within the requested tolerances for start and goal conditions, - * returning them as a vector, sorted by some cache column. + * @see FeaturesInterface * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The cartesian plan request to key the cache with. - * @param[in] min_fraction. The minimum fraction required for a cache hit. - * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * @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] plan_request. The cartesian plan request to extract features from to key the cache with. + * @param[in] features. The features to key the cache with. + * @param[in] sort_by. The cache feature 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. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @returns A vector of cache hits, sorted by the `sort_by` parameter. */ std::vector::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 ascending = true) const; + fetchAllMatchingCartesianTrajectories( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending = true, bool metadata_only = false) const; /** - * @brief Fetches the best cartesian trajectory that fits within the requested tolerances for start and goal - * conditions, by some cache column. + * @brief Fetches the best cartesian trajectory keyed on user-specified features, with respect to some cache feature. + * + * @see FeaturesInterface * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The cartesian plan request to key the cache with. - * @param[in] min_fraction. The minimum fraction required for a cache hit. - * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. - * @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] plan_request. The cartesian plan request to extract features from to key the cache with. + * @param[in] features. The features to key the cache with. + * @param[in] sort_by. The cache feature to sort by. * @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. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @returns The best cache hit, with respect to the `sort_by` parameter. */ warehouse_ros::MessageWithMetadata::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) const; + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending = true, bool metadata_only = false) const; /** - * @brief Inserts 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 are higher priority in the sorting order specified by `sort_by` than another - * exactly matching cartesian trajectory. - * - * 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_precision` arg passed in init(). - * @see init() + * @brief Inserts a cartesian trajectory into the database, with user-specified insert policy. * * Optionally deletes all worse cartesian trajectories by default to prune the cache. * + * @see FeaturesInterface + * @see CacheInsertPolicyInterface + * * @param[in] move_group. The manipulator move group, used to get its state. * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. - * @param[in] plan_request. The cartesian plan request to key the cache with. - * @param[in] trajectory. The trajectory to put. - * @param[in] execution_time_s. The execution time of the trajectory, in seconds. - * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. - * @param[in] fraction. The fraction of the path that was computed. - * @param[in] prune_worse_trajectories. If true, will prune the cache by deleting all cache entries that match the - * `plan_request` exactly, if they are worse than `trajectory`, even if it was not put. - * @returns true if the trajectory was the best seen yet and hence put into the cache. + * @param[in] plan_request. The cartesian path plan request to extract features from to key the cache with. + * @param[in] plan. The plan containing the trajectory to insert. + * @param[in,out] cache_insert_policy. The cache insert policy to use. Will determine what features can be used to + * fetch entries, and pruning and insertion logic. Will be reset at the end of the call. + * @param[in] prune_worse_trajectories. If true, will prune the cache according to the `cache_insert_policy`'s pruning + * logic. + * @param[in] additional_features. Additional features to key the cache with, must not intersect with the set of + * features supported by the `cache_insert_policy`. + * @returns True if the trajectory was inserted into the cache. */ - bool insertCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, double fraction, bool prune_worse_trajectories = true); + bool insertCartesianTrajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::srv::GetCartesianPath::Response& plan, + CacheInsertPolicyInterface& + cache_insert_policy, + bool prune_worse_trajectories = true, + const std::vector>>& + additional_features = {}); /**@}*/ private: - /** - * @name Motion plan trajectory query and metadata construction - */ - /**@{*/ - - /** - * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache db query, - * with some match tolerance. - * - * These parameters will be used to look-up relevant sections of a cache element's key. - * - * @param[out] query. The query 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. - * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * @returns true if successfully added to. If false, the query might have been partially modified and should not be - * used. - */ - bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance) const; - - /** - * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache db query, with - * some match tolerance. - * - * These parameters will be used to look-up relevant sections of a cache element's key. - * - * @param[out] query. The query 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. - * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * @returns true if successfully added to. If false, the query might have been partially modified and should not be - * used. - */ - bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - double match_tolerance) const; - - /** - * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache entry's - * metadata. - * - * These parameters will be used key the cache element. - * - * @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 - * used. - */ - bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) const; - - /** - * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache entry's - * metadata. - * - * These parameters will be used key the cache element. - * - * @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 - * used. - */ - bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) const; - - /**@}*/ - - /** - * @name Cartesian trajectory query and metadata construction - */ - /**@{*/ - - /** - * @brief Extracts relevant parameters from a cartesian plan request's start parameters to populate a cache db query, - * with some match tolerance. - * - * These parameters will be used to look-up relevant sections of a cache element's key. - * - * @param[out] query. The query 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. - * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * @returns true if successfully added to. If false, the query might have been partially modified and should not be - * used. - */ - bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double match_tolerance) const; - - /** - * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, - * with some match tolerance. - * - * These parameters will be used to look-up relevant sections of a cache element's key. - * - * @param[out] query. The query 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. - * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. - * @returns true if successfully added to. If false, the query might have been partially modified and should not be - * used. - */ - bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, - const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - double match_tolerance) const; - - /** - * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's - * metadata. - * - * These parameters will be used key the cache element. - * - * @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 - * used. - */ - bool extractAndAppendCartesianTrajectoryStartToMetadata( - warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; - - /** - * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's - * metadata. - * - * These parameters will be used key the cache element. - * - * @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 - * used. - */ - bool extractAndAppendCartesianTrajectoryGoalToMetadata( - warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; - - /**@}*/ - rclcpp::Node::SharedPtr node_; rclcpp::Logger logger_; warehouse_ros::DatabaseConnection::Ptr db_; Options options_; - - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; }; } // namespace trajectory_cache diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp new file mode 100644 index 0000000000..f754754b04 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp @@ -0,0 +1,231 @@ +// 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 Utilities used by the trajectory_cache package. + * @author methylDragon + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// Frames. ========================================================================================= + +/** @brief Gets workspace frame ID. + * If workspace_parameters has no frame ID, fetch it from move_group. + * + * It makes sense to use getPoseReferenceFrame() in the absence of a frame ID in the request because + * the same method is used to populate the header frame ID in the MoveGroupInterface's + * computeCartesianPath() method, which this function is associated with. + */ +std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters); + +/** @brief Gets cartesian path request frame ID. + * If path_request has no frame ID, fetch it from move_group. + * + * It makes sense to use getPoseReferenceFrame() in the absence of a frame ID in the request because + * the same method is used to populate the header frame ID in the MoveGroupInterface's + * computeCartesianPath() method, which this function is associated with. + */ +std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& path_request); + +/** @brief Restates a translation and rotation in a new frame. + * + * @param[in] tf. The transform buffer to use. + * @param[in] target_frame. The frame to restate in. + * @param[in] source_frame. The frame to restate from. + * @param[in,out] translation. The translation to restate. Ignored if nullptr. + * @param[in,out] rotation. The rotation to restate. Ignored if nullptr. + * @returns MoveItErrorCode::SUCCESS if successfully restated. Otherwise, will return return + * MoveItErrorCode::FRAME_TRANSFORM_FAILURE if the transform could not be retrieved. + */ +moveit::core::MoveItErrorCode restateInNewFrame(const std::shared_ptr& tf, + const std::string& target_frame, const std::string& source_frame, + geometry_msgs::msg::Point* translation, + geometry_msgs::msg::Quaternion* rotation, + const tf2::TimePoint& lookup_time = tf2::TimePointZero); + +// Execution Time. ================================================================================= + +/** @brief Returns the execution time of the trajectory in double seconds. */ +double getExecutionTime(const moveit_msgs::msg::RobotTrajectory& trajectory); + +// Request Construction. =========================================================================== + +/** + * @brief Constructs a GetCartesianPath request. + * + * This is a convenience function. + * This mimics the move group computeCartesianPath signature (without path constraints). + * + * WARNING: The following fields are not supported, if you want to specify them, add them in yourself. + * - prismatic_jump_threshold + * - revolute_jump_threshold + * - cartesian_speed_limited_link + * - max_cartesian_speed + * + * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. + * @param[in] waypoints. The cartesian waypoints to request the path for. + * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. + * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. + * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. + * @returns + */ +moveit_msgs::srv::GetCartesianPath::Request +constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double max_step, + double jump_threshold, bool avoid_collisions = true); + +// Queries. ======================================================================================== + +/** @brief Appends a range inclusive query with some tolerance about some center value. */ +void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center, + double tolerance); + +// Constraints. ==================================================================================== + +/** @brief Sorts a vector of joint constraints in-place by joint name. */ +void sortJointConstraints(std::vector& joint_constraints); + +/** @brief Sorts a vector of position constraints in-place by link name. */ +void sortPositionConstraints(std::vector& position_constraints); + +/** @brief Sorts a vector of orientation constraints in-place by link name. */ +void sortOrientationConstraints(std::vector& orientation_constraints); + +/** @brief Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a fetch query, with + * tolerance. + * + * This will extract relevant features from the joint, position, and orientation constraints per element. + * This exists because many keyable messages contain constraints which should be handled similarly. + * + * WARNING: Visibility constraints are not supported. + * + * Additionally, the component constraints within each vector element are sorted to reduce cache cardinality. + * For the same reason, constraints with frames are restated in terms of the workspace frame. + * + * We copy the input constraints to support this. + * + * @param[in,out] query. The query to add features to. + * @param[in] constraints. The constraints to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] reference_frame_id. The frame to restate constraints in. + * @param[in] prefix. A prefix to add to feature keys. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the query should not be reused. + */ +moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( + warehouse_ros::Query& query, std::vector constraints, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance, + const std::string& reference_frame_id, const std::string& prefix); + +/** @brief Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a cache entry's + * metadata. + * + * This will extract relevant features from the joint, position, and orientation constraints per element. + * This exists because many keyable messages contain constraints which should be handled similarly. + * + * WARNING: Visibility constraints and constraint regions are not supported. + * + * Additionally, the component constraints within each vector element are sorted to reduce cache cardinality. + * For the same reason, constraints with frames are restated in terms of the workspace frame. + * + * We copy the input constraints to support this. + * + * @param[in,out] metadata. The metadata to add features to. + * @param[in] constraints. The constraints to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] reference_frame_id. The frame to restate constraints in. + * @param[in] prefix. A prefix to add to feature keys. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the metadata should not be reused. + */ +moveit::core::MoveItErrorCode +appendConstraintsAsInsertMetadata(warehouse_ros::Metadata& metadata, + std::vector constraints, + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& reference_frame_id, const std::string& prefix); + +// RobotState. ===================================================================================== + +/** @brief Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a fetch query, with + * tolerance. + * + * This will extract relevant features from the joint, position, and orientation constraints per element. + * This exists because many keyable messages contain constraints which should be handled similarly. + * + * WARNING: Visibility constraints are not supported. + * + * Additionally, the component constraints within each vector element are sorted to reduce cache cardinality. + * For the same reason, constraints with frames are restated in terms of the workspace frame. + * + * We copy the input constraints to support this. + * + * @param[in,out] query. The query to add features to. + * @param[in] robot_state. The robot state to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] prefix. A prefix to add to feature keys. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the query should not be reused. + */ +moveit::core::MoveItErrorCode +appendRobotStateJointStateAsFetchQueryWithTolerance(warehouse_ros::Query& query, + const moveit_msgs::msg::RobotState& robot_state, + const moveit::planning_interface::MoveGroupInterface& move_group, + double match_tolerance, const std::string& prefix); + +/** @brief Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a cache entry's + * metadata. + * + * This will extract relevant features from the joint, position, and orientation constraints per element. + * This exists because many keyable messages contain constraints which should be handled similarly. + * + * WARNING: Visibility constraints and constraint regions are not supported. + * + * Additionally, the component constraints within each vector element are sorted to reduce cache cardinality. + * For the same reason, constraints with frames are restated in terms of the workspace frame. + * + * We copy the input constraints to support this. + * + * @param[in,out] metadata. The metadata to add features to. + * @param[in] robot_state. The robot state to extract features from. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] prefix. A prefix to add to feature keys. + * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error + * code, in which case the metadata should not be reused. + */ +moveit::core::MoveItErrorCode appendRobotStateJointStateAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::RobotState& robot_state, + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& prefix); + +} // 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 0000000000..a3c01cad5b --- /dev/null +++ b/moveit_ros/trajectory_cache/src/cache_insert_policies/always_insert_never_prune_policy.cpp @@ -0,0 +1,372 @@ +// 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. + * + * @see CacheInsertPolicyInterface + * + * @author methylDragon + */ + +#include +#include +#include + +#include +#include + +#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_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +namespace +{ + +const std::string EXECUTION_TIME = "execution_time_s"; +const std::string FRACTION = "fraction"; +const std::string PLANNING_TIME = "planning_time_s"; + +} // namespace + +// ================================================================================================= +// AlwaysInsertNeverPrunePolicy. +// ================================================================================================= +// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan + +AlwaysInsertNeverPrunePolicy::AlwaysInsertNeverPrunePolicy() : name_("AlwaysInsertNeverPrunePolicy") +{ + exact_matching_supported_features_ = AlwaysInsertNeverPrunePolicy::getSupportedFeatures(/*start_tolerance=*/0.0, + /*goal_tolerance=*/0.0); +} + +std::vector>> +AlwaysInsertNeverPrunePolicy::getSupportedFeatures(double start_tolerance, double goal_tolerance) +{ + std::vector>> out; + out.reserve(6); + + // 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 frame_id = getWorkspaceFrameId(move_group, key.workspace_parameters); + + // Check key. + if (frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Workspace frame ID cannot be empty."); + } + if (key.goal_constraints.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Skipping insert: No goal."); + } + + // Check value. + 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 insert: Empty joint trajectory joint names."); + } + if (!value.trajectory.multi_dof_joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Multi-DOF trajectory plans are not supported."); + } + if (value.trajectory.joint_trajectory.header.frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Trajectory frame ID cannot be empty."); + } + if (frame_id != value.trajectory.joint_trajectory.header.frame_id) + { + std::stringstream ss; + ss << "Skipping insert: Plan request frame `" << 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(); + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + } + } + return coll.queryList(query, /*metadata_only=*/true); +} + +bool AlwaysInsertNeverPrunePolicy::shouldPruneMatchingEntry( + const MoveGroupInterface& /*move_group*/, const MotionPlanRequest& /*key*/, + const MoveGroupInterface::Plan& /*value*/, const MessageWithMetadata::ConstPtr& /*matching_entry*/, + std::string* reason) +{ + if (reason != nullptr) + { + *reason = "Never prune."; + } + return false; // Never prune. +} + +bool AlwaysInsertNeverPrunePolicy::shouldInsert(const MoveGroupInterface& /*move_group*/, + const MotionPlanRequest& /*key*/, + const MoveGroupInterface::Plan& /*value*/, std::string* reason) +{ + if (reason != nullptr) + { + *reason = "Always insert."; + } + return true; // Always insert. +} + +MoveItErrorCode AlwaysInsertNeverPrunePolicy::appendInsertMetadata(Metadata& metadata, + const MoveGroupInterface& move_group, + const MotionPlanRequest& key, + const MoveGroupInterface::Plan& value) +{ + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsInsertMetadata(metadata, key, move_group); !ret) + { + return ret; + } + } + + // Append ValueT metadata. + metadata.append(EXECUTION_TIME, getExecutionTime(value.trajectory)); + metadata.append(PLANNING_TIME, value.planning_time); + + return MoveItErrorCode::SUCCESS; +} + +void AlwaysInsertNeverPrunePolicy::reset() +{ + return; +} + +// ================================================================================================= +// CartesianAlwaysInsertNeverPrunePolicy. +// ================================================================================================= +// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response + +CartesianAlwaysInsertNeverPrunePolicy::CartesianAlwaysInsertNeverPrunePolicy() + : name_("CartesianAlwaysInsertNeverPrunePolicy") +{ + exact_matching_supported_features_ = + CartesianAlwaysInsertNeverPrunePolicy::getSupportedFeatures(/*start_tolerance=*/0.0, + /*goal_tolerance=*/0.0, /*min_fraction=*/0.0); +} + +std::vector>> +CartesianAlwaysInsertNeverPrunePolicy::getSupportedFeatures(double start_tolerance, double goal_tolerance, + double min_fraction) +{ + std::vector>> out; + out.reserve(7); + + // 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()); + out.push_back(std::make_unique(goal_tolerance)); + out.push_back(std::make_unique(goal_tolerance)); + + // Min fraction. + out.push_back(std::make_unique>(FRACTION, min_fraction)); + + return out; +} + +std::string CartesianAlwaysInsertNeverPrunePolicy::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianAlwaysInsertNeverPrunePolicy::checkCacheInsertInputs( + const MoveGroupInterface& move_group, const MessageCollection& /*coll*/, + const GetCartesianPath::Request& key, const GetCartesianPath::Response& value) +{ + std::string frame_id = getCartesianPathRequestFrameId(move_group, key); + + // Check key. + if (frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Workspace frame ID cannot be empty."); + } + if (key.waypoints.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Skipping insert: No waypoints."); + } + + // Check value. + if (value.solution.joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Empty joint trajectory points."); + } + if (value.solution.joint_trajectory.joint_names.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Empty joint trajectory joint names."); + } + if (!value.solution.multi_dof_joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Multi-DOF trajectory plans are not supported."); + } + if (value.solution.joint_trajectory.header.frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Trajectory frame ID cannot be empty."); + } + if (frame_id != value.solution.joint_trajectory.header.frame_id) + { + std::stringstream ss; + ss << "Skipping insert: Plan request frame `" << frame_id << "` does not match plan frame `" + << value.solution.joint_trajectory.header.frame_id << "`."; + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, ss.str()); + } + + return MoveItErrorCode::SUCCESS; +} + +std::vector::ConstPtr> CartesianAlwaysInsertNeverPrunePolicy::fetchMatchingEntries( + const MoveGroupInterface& move_group, const MessageCollection& coll, + const GetCartesianPath::Request& key, const GetCartesianPath::Response& /*value*/, double exact_match_precision) +{ + Query::Ptr query = coll.createQuery(); + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + } + } + return coll.queryList(query, /*metadata_only=*/true); +} + +bool CartesianAlwaysInsertNeverPrunePolicy::shouldPruneMatchingEntry( + const MoveGroupInterface& /*move_group*/, const GetCartesianPath::Request& /*key*/, + const GetCartesianPath::Response& /*value*/, + const MessageWithMetadata::ConstPtr& /*matching_entry*/, std::string* reason) +{ + if (reason != nullptr) + { + *reason = "Never prune."; + } + return false; // Never prune. +} + +bool CartesianAlwaysInsertNeverPrunePolicy::shouldInsert(const MoveGroupInterface& /*move_group*/, + const GetCartesianPath::Request& /*key*/, + const GetCartesianPath::Response& /*value*/, + std::string* reason) +{ + if (reason != nullptr) + { + *reason = "Always insert."; + } + return true; // Always insert. +} + +MoveItErrorCode CartesianAlwaysInsertNeverPrunePolicy::appendInsertMetadata(Metadata& metadata, + const MoveGroupInterface& move_group, + const GetCartesianPath::Request& key, + const GetCartesianPath::Response& value) +{ + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsInsertMetadata(metadata, key, move_group); !ret) + { + return ret; + } + } + + // Append ValueT metadata. + metadata.append(EXECUTION_TIME, getExecutionTime(value.solution)); + metadata.append(FRACTION, value.fraction); + + return MoveItErrorCode::SUCCESS; +} + +void CartesianAlwaysInsertNeverPrunePolicy::reset() +{ + return; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/cache_insert_policies/best_seen_execution_time_policy.cpp b/moveit_ros/trajectory_cache/src/cache_insert_policies/best_seen_execution_time_policy.cpp new file mode 100644 index 0000000000..1a86c92ad7 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/cache_insert_policies/best_seen_execution_time_policy.cpp @@ -0,0 +1,468 @@ +// 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 +#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_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +namespace +{ + +const std::string EXECUTION_TIME = "execution_time_s"; +const std::string FRACTION = "fraction"; +const std::string PLANNING_TIME = "planning_time_s"; + +} // namespace + +// ================================================================================================= +// BestSeenExecutionTimePolicy. +// ================================================================================================= +// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan + +BestSeenExecutionTimePolicy::BestSeenExecutionTimePolicy() + : name_("BestSeenExecutionTimePolicy"), best_seen_execution_time_(std::numeric_limits::infinity()) +{ + exact_matching_supported_features_ = BestSeenExecutionTimePolicy::getSupportedFeatures(/*start_tolerance=*/0.0, + /*goal_tolerance=*/0.0); +} + +std::vector>> +BestSeenExecutionTimePolicy::getSupportedFeatures(double start_tolerance, double goal_tolerance) +{ + std::vector>> out; + out.reserve(6); + + // 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 BestSeenExecutionTimePolicy::getName() const +{ + return name_; +} + +MoveItErrorCode BestSeenExecutionTimePolicy::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); + + // Check key. + if (workspace_frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Workspace frame ID cannot be empty."); + } + if (key.goal_constraints.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Skipping insert: No goal."); + } + + // Check value. + 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 insert: Empty joint trajectory joint names."); + } + if (!value.trajectory.multi_dof_joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Multi-DOF trajectory plans are not supported."); + } + if (value.trajectory.joint_trajectory.header.frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Trajectory frame ID cannot be empty."); + } + if (workspace_frame_id != value.trajectory.joint_trajectory.header.frame_id) + { + std::stringstream ss; + ss << "Skipping 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> BestSeenExecutionTimePolicy::fetchMatchingEntries( + const MoveGroupInterface& move_group, const MessageCollection& coll, const MotionPlanRequest& key, + const MoveGroupInterface::Plan& /*value*/, double exact_match_precision) +{ + Query::Ptr query = coll.createQuery(); + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + } + } + + std::vector::ConstPtr> out = + coll.queryList(query, /*metadata_only=*/true, /*sort_by=*/EXECUTION_TIME, /*ascending=*/true); + if (!out.empty()) + { + best_seen_execution_time_ = out[0]->lookupDouble(EXECUTION_TIME); + } + + return out; +} + +bool BestSeenExecutionTimePolicy::shouldPruneMatchingEntry( + const MoveGroupInterface& /*move_group*/, const MotionPlanRequest& /*key*/, const MoveGroupInterface::Plan& value, + const MessageWithMetadata::ConstPtr& matching_entry, std::string* reason) +{ + double matching_entry_execution_time_s = matching_entry->lookupDouble(EXECUTION_TIME); + double candidate_execution_time_s = getExecutionTime(value.trajectory); + + if (matching_entry_execution_time_s >= candidate_execution_time_s) + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "Matching trajectory execution_time_s `" << matching_entry_execution_time_s << "s` " + << "is worse than candidate trajectory's execution_time_s `" << candidate_execution_time_s << "s`"; + *reason = ss.str(); + } + return true; + } + else + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "Matching trajectory execution_time_s `" << matching_entry_execution_time_s << "s` " + << "is better than candidate trajectory's execution_time_s `" << candidate_execution_time_s << "s`"; + *reason = ss.str(); + } + return false; + } +} + +bool BestSeenExecutionTimePolicy::shouldInsert(const MoveGroupInterface& /*move_group*/, + const MotionPlanRequest& /*key*/, const MoveGroupInterface::Plan& value, + std::string* reason) +{ + double execution_time_s = getExecutionTime(value.trajectory); + + if (execution_time_s < best_seen_execution_time_) + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "New trajectory execution_time_s `" << execution_time_s << "s` " + << "is better than best trajectory's execution_time_s `" << best_seen_execution_time_ << "s`"; + *reason = ss.str(); + } + return true; + } + else + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "New trajectory execution_time `" << execution_time_s << "s` " + << "is worse than best trajectory's execution_time `" << best_seen_execution_time_ << "s`"; + *reason = ss.str(); + } + return false; + } +} + +MoveItErrorCode BestSeenExecutionTimePolicy::appendInsertMetadata(Metadata& metadata, + const MoveGroupInterface& move_group, + const MotionPlanRequest& key, + const MoveGroupInterface::Plan& value) +{ + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsInsertMetadata(metadata, key, move_group); !ret) + { + return ret; + } + } + + // Append ValueT metadata. + metadata.append(EXECUTION_TIME, getExecutionTime(value.trajectory)); + metadata.append(PLANNING_TIME, value.planning_time); + + return MoveItErrorCode::SUCCESS; +} + +void BestSeenExecutionTimePolicy::reset() +{ + best_seen_execution_time_ = std::numeric_limits::infinity(); + return; +} + +// ================================================================================================= +// CartesianBestSeenExecutionTimePolicy. +// ================================================================================================= +// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response + +CartesianBestSeenExecutionTimePolicy::CartesianBestSeenExecutionTimePolicy() + : name_("CartesianBestSeenExecutionTimePolicy"), best_seen_execution_time_(std::numeric_limits::infinity()) +{ + exact_matching_supported_features_ = + CartesianBestSeenExecutionTimePolicy::getSupportedFeatures(/*start_tolerance=*/0.0, + /*goal_tolerance=*/0.0, /*min_fraction=*/0.0); +} + +std::vector>> +CartesianBestSeenExecutionTimePolicy::getSupportedFeatures(double start_tolerance, double goal_tolerance, + double min_fraction) +{ + std::vector>> out; + out.reserve(7); + + // 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()); + out.push_back(std::make_unique(goal_tolerance)); + out.push_back(std::make_unique(goal_tolerance)); + + // Min fraction. + out.push_back(std::make_unique>(FRACTION, min_fraction)); + + return out; +} + +std::string CartesianBestSeenExecutionTimePolicy::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianBestSeenExecutionTimePolicy::checkCacheInsertInputs( + const MoveGroupInterface& move_group, const MessageCollection& /*coll*/, + const GetCartesianPath::Request& key, const GetCartesianPath::Response& value) +{ + std::string frame_id = getCartesianPathRequestFrameId(move_group, key); + + // Check key. + if (frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Workspace frame ID cannot be empty."); + } + if (key.waypoints.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Skipping insert: No waypoints."); + } + + // Check value. + if (value.solution.joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, name_ + ": Empty joint trajectory points."); + } + if (value.solution.joint_trajectory.joint_names.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Empty joint trajectory joint names."); + } + if (!value.solution.multi_dof_joint_trajectory.points.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Multi-DOF trajectory plans are not supported."); + } + if (value.solution.joint_trajectory.header.frame_id.empty()) + { + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, + name_ + ": Skipping insert: Trajectory frame ID cannot be empty."); + } + if (frame_id != value.solution.joint_trajectory.header.frame_id) + { + std::stringstream ss; + ss << "Skipping insert: Plan request frame `" << frame_id << "` does not match plan frame `" + << value.solution.joint_trajectory.header.frame_id << "`."; + return MoveItErrorCode(MoveItErrorCode::INVALID_MOTION_PLAN, ss.str()); + } + + return MoveItErrorCode::SUCCESS; +} + +std::vector::ConstPtr> CartesianBestSeenExecutionTimePolicy::fetchMatchingEntries( + const MoveGroupInterface& move_group, const MessageCollection& coll, + const GetCartesianPath::Request& key, const GetCartesianPath::Response& /*value*/, double exact_match_precision) +{ + Query::Ptr query = coll.createQuery(); + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsExactFetchQuery(*query, key, move_group, exact_match_precision); + !ret) + { + return {}; + } + } + + std::vector::ConstPtr> out = + coll.queryList(query, /*metadata_only=*/true, /*sort_by=*/EXECUTION_TIME, /*ascending=*/true); + if (!out.empty()) + { + best_seen_execution_time_ = out[0]->lookupDouble(EXECUTION_TIME); + } + + return out; +} + +bool CartesianBestSeenExecutionTimePolicy::shouldPruneMatchingEntry( + const MoveGroupInterface& /*move_group*/, const GetCartesianPath::Request& /*key*/, + const GetCartesianPath::Response& value, const MessageWithMetadata::ConstPtr& matching_entry, + std::string* reason) +{ + double matching_entry_execution_time_s = matching_entry->lookupDouble(EXECUTION_TIME); + double candidate_execution_time_s = getExecutionTime(value.solution); + + if (matching_entry_execution_time_s >= candidate_execution_time_s) + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "Matching trajectory execution_time_s `" << matching_entry_execution_time_s << "s` " + << "is worse than candidate trajectory's execution_time_s `" << candidate_execution_time_s << "s`"; + *reason = ss.str(); + } + return true; + } + else + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "Matching trajectory execution_time_s `" << matching_entry_execution_time_s << "s` " + << "is better than candidate trajectory's execution_time_s `" << candidate_execution_time_s << "s`"; + *reason = ss.str(); + } + return false; + } +} + +bool CartesianBestSeenExecutionTimePolicy::shouldInsert(const MoveGroupInterface& /*move_group*/, + const GetCartesianPath::Request& /*key*/, + const GetCartesianPath::Response& value, std::string* reason) +{ + double execution_time_s = getExecutionTime(value.solution); + + if (execution_time_s < best_seen_execution_time_) + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "New cartesian trajectory execution_time_s `" << execution_time_s << "s` " + << "is better than best cartesian trajectory's execution_time_s `" << best_seen_execution_time_ << "s`"; + *reason = ss.str(); + } + return true; + } + else + { + if (reason != nullptr) + { + std::stringstream ss; + ss << "New cartesian trajectory execution_time `" << execution_time_s << "s` " + << "is worse than best cartesian trajectory's execution_time `" << best_seen_execution_time_ << "s`"; + *reason = ss.str(); + } + return false; + } +} + +MoveItErrorCode CartesianBestSeenExecutionTimePolicy::appendInsertMetadata(Metadata& metadata, + const MoveGroupInterface& move_group, + const GetCartesianPath::Request& key, + const GetCartesianPath::Response& value) +{ + for (const auto& feature : exact_matching_supported_features_) + { + if (MoveItErrorCode ret = feature->appendFeaturesAsInsertMetadata(metadata, key, move_group); !ret) + { + return ret; + } + } + + // Append ValueT metadata. + metadata.append(EXECUTION_TIME, getExecutionTime(value.solution)); + metadata.append(FRACTION, value.fraction); + + return MoveItErrorCode::SUCCESS; +} + +void CartesianBestSeenExecutionTimePolicy::reset() +{ + best_seen_execution_time_ = std::numeric_limits::infinity(); + return; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp b/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp new file mode 100644 index 0000000000..1136331754 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/features/get_cartesian_path_request_features.cpp @@ -0,0 +1,446 @@ +// 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 GetCartesianPath::Request features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; + +using ::moveit_msgs::srv::GetCartesianPath; + +// "Start" features. =============================================================================== + +// CartesianWorkspaceFeatures. + +CartesianWorkspaceFeatures::CartesianWorkspaceFeatures() : name_("CartesianWorkspaceFeatures") +{ +} + +std::string CartesianWorkspaceFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsExactFetchQuery(Query& query, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double /*exact_match_precision*/) const +{ + query.append(name_ + ".group_name", source.group_name); + query.append(name_ + ".header.frame_id", getCartesianPathRequestFrameId(move_group, source)); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode CartesianWorkspaceFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group) const +{ + metadata.append(name_ + ".group_name", source.group_name); + metadata.append(name_ + ".header.frame_id", getCartesianPathRequestFrameId(move_group, source)); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianStartStateJointStateFeatures. + +CartesianStartStateJointStateFeatures::CartesianStartStateJointStateFeatures(double match_tolerance) + : name_("CartesianStartStateJointStateFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianStartStateJointStateFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsFuzzyFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsExactFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsInsertMetadata( + Metadata& metadata, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group) const +{ + return appendRobotStateJointStateAsInsertMetadata(metadata, source.start_state, move_group, name_ + ".start_state"); +}; + +MoveItErrorCode CartesianStartStateJointStateFeatures::appendFeaturesAsFetchQueryWithTolerance( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double match_tolerance) const +{ + return appendRobotStateJointStateAsFetchQueryWithTolerance(query, source.start_state, move_group, match_tolerance, + name_ + ".start_state"); +}; + +// "Goal" features. ================================================================================ + +// CartesianMaxSpeedAndAccelerationFeatures. + +CartesianMaxSpeedAndAccelerationFeatures::CartesianMaxSpeedAndAccelerationFeatures() + : name_("CartesianMaxSpeedAndAccelerationFeatures") +{ +} + +std::string CartesianMaxSpeedAndAccelerationFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsFuzzyFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsExactFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + query.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + query.appendLTE(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode CartesianMaxSpeedAndAccelerationFeatures::appendFeaturesAsInsertMetadata( + Metadata& metadata, const GetCartesianPath::Request& source, const MoveGroupInterface& /*move_group*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + metadata.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + metadata.append(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianMaxStepAndJumpThresholdFeatures. + +CartesianMaxStepAndJumpThresholdFeatures::CartesianMaxStepAndJumpThresholdFeatures() + : name_("CartesianMaxStepAndJumpThresholdFeatures") +{ +} + +std::string CartesianMaxStepAndJumpThresholdFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsFuzzyFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsExactFetchQuery( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const +{ + query.appendLTE(name_ + ".max_step", source.max_step); + + if (source.jump_threshold > 0) + { + query.appendLTE(name_ + ".jump_threshold", source.jump_threshold); + } + if (source.prismatic_jump_threshold > 0) + { + query.appendLTE(name_ + ".prismatic_jump_threshold", source.prismatic_jump_threshold); + } + if (source.revolute_jump_threshold > 0) + { + query.appendLTE(name_ + ".revolute_jump_threshold", source.revolute_jump_threshold); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode CartesianMaxStepAndJumpThresholdFeatures::appendFeaturesAsInsertMetadata( + Metadata& metadata, const GetCartesianPath::Request& source, const MoveGroupInterface& /*move_group*/) const +{ + metadata.append(name_ + ".max_step", source.max_step); + + if (source.jump_threshold > 0) + { + metadata.append(name_ + ".jump_threshold", source.jump_threshold); + } + if (source.prismatic_jump_threshold > 0) + { + metadata.append(name_ + ".prismatic_jump_threshold", source.prismatic_jump_threshold); + } + if (source.revolute_jump_threshold > 0) + { + metadata.append(name_ + ".revolute_jump_threshold", source.revolute_jump_threshold); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianWaypointsFeatures. + +CartesianWaypointsFeatures::CartesianWaypointsFeatures(double match_tolerance) + : name_("CartesianWaypointsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianWaypointsFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsExactFetchQuery(Query& query, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, + const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, source); + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + metadata.append(name_ + ".link_name", source.link_name); + metadata.append(name_ + ".robot_model.frame_id", base_frame); + + // Waypoints. + + size_t waypoint_idx = 0; + for (const auto& waypoint : source.waypoints) + { + std::string meta_name = name_ + ".waypoints_" + std::to_string(waypoint_idx++); + + geometry_msgs::msg::Point canonical_position = waypoint.position; + geometry_msgs::msg::Quaternion canonical_orientation = waypoint.orientation; + + // Canonicalize to robot base frame if necessary. + if (path_request_frame_id != base_frame) + { + if (MoveItErrorCode status = restateInNewFrame(move_group.getTF(), path_request_frame_id, base_frame, + &canonical_position, &canonical_orientation, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << name_ << " metadata append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + // Position + metadata.append(meta_name + ".position.x", canonical_position.x); + metadata.append(meta_name + ".position.y", canonical_position.y); + metadata.append(meta_name + ".position.z", canonical_position.z); + + // Orientation + metadata.append(meta_name + ".orientation.x", canonical_orientation.x); + metadata.append(meta_name + ".orientation.y", canonical_orientation.y); + metadata.append(meta_name + ".orientation.z", canonical_orientation.z); + metadata.append(meta_name + ".orientation.w", canonical_orientation.w); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode CartesianWaypointsFeatures::appendFeaturesAsFetchQueryWithTolerance( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double match_tolerance) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, source); + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + query.append(name_ + ".link_name", source.link_name); + query.append(name_ + ".robot_model.frame_id", base_frame); + + // Waypoints. + + size_t waypoint_idx = 0; + for (const auto& waypoint : source.waypoints) + { + std::string meta_name = name_ + ".waypoints_" + std::to_string(waypoint_idx++); + + geometry_msgs::msg::Point canonical_position = waypoint.position; + geometry_msgs::msg::Quaternion canonical_orientation = waypoint.orientation; + + // Canonicalize to robot base frame if necessary. + if (path_request_frame_id != base_frame) + { + if (MoveItErrorCode status = restateInNewFrame(move_group.getTF(), path_request_frame_id, base_frame, + &canonical_position, &canonical_orientation, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << name_ << " query append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + // Position + queryAppendCenterWithTolerance(query, meta_name + ".position.x", canonical_position.x, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".position.y", canonical_position.y, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".position.z", canonical_position.z, match_tolerance); + + // Orientation + queryAppendCenterWithTolerance(query, meta_name + ".orientation.x", canonical_orientation.x, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.y", canonical_orientation.y, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.z", canonical_orientation.z, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.w", canonical_orientation.w, match_tolerance); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// CartesianPathConstraintsFeatures. + +CartesianPathConstraintsFeatures::CartesianPathConstraintsFeatures(double match_tolerance) + : name_("CartesianPathConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string CartesianPathConstraintsFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode +CartesianPathConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode +CartesianPathConstraintsFeatures::appendFeaturesAsExactFetchQuery(Query& query, const GetCartesianPath::Request& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsInsertMetadata( + Metadata& metadata, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group) const +{ + return appendConstraintsAsInsertMetadata(metadata, { source.path_constraints }, move_group, + getCartesianPathRequestFrameId(move_group, source), + name_ + ".path_constraints"); +}; + +MoveItErrorCode CartesianPathConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + Query& query, const GetCartesianPath::Request& source, const MoveGroupInterface& move_group, + double match_tolerance) const +{ + return appendConstraintsAsFetchQueryWithTolerance(query, { source.path_constraints }, move_group, match_tolerance, + getCartesianPathRequestFrameId(move_group, source), + name_ + ".path_constraints"); +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp b/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp new file mode 100644 index 0000000000..0060c671de --- /dev/null +++ b/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp @@ -0,0 +1,360 @@ +// 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 moveit_msgs::msg::MotionPlanRequest features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; + +using ::moveit_msgs::msg::MotionPlanRequest; + +// "Start" features. =============================================================================== + +// WorkspaceFeatures. + +WorkspaceFeatures::WorkspaceFeatures() : name_("WorkspaceFeatures") +{ +} + +std::string WorkspaceFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode WorkspaceFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode WorkspaceFeatures::appendFeaturesAsExactFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double /*exact_match_precision*/) const +{ + query.append(name_ + ".group_name", source.group_name); + query.append(name_ + ".workspace_parameters.header.frame_id", + getWorkspaceFrameId(move_group, source.workspace_parameters)); + query.appendGTE(name_ + ".workspace_parameters.min_corner.x", source.workspace_parameters.min_corner.x); + query.appendGTE(name_ + ".workspace_parameters.min_corner.y", source.workspace_parameters.min_corner.y); + query.appendGTE(name_ + ".workspace_parameters.min_corner.z", source.workspace_parameters.min_corner.z); + query.appendLTE(name_ + ".workspace_parameters.max_corner.x", source.workspace_parameters.max_corner.x); + query.appendLTE(name_ + ".workspace_parameters.max_corner.y", source.workspace_parameters.max_corner.y); + query.appendLTE(name_ + ".workspace_parameters.max_corner.z", source.workspace_parameters.max_corner.z); + return MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode WorkspaceFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, const MotionPlanRequest& source, + const MoveGroupInterface& move_group) const +{ + metadata.append(name_ + ".group_name", source.group_name); + metadata.append(name_ + ".workspace_parameters.header.frame_id", + getWorkspaceFrameId(move_group, source.workspace_parameters)); + metadata.append(name_ + ".workspace_parameters.min_corner.x", source.workspace_parameters.min_corner.x); + metadata.append(name_ + ".workspace_parameters.min_corner.y", source.workspace_parameters.min_corner.y); + metadata.append(name_ + ".workspace_parameters.min_corner.z", source.workspace_parameters.min_corner.z); + metadata.append(name_ + ".workspace_parameters.max_corner.x", source.workspace_parameters.max_corner.x); + metadata.append(name_ + ".workspace_parameters.max_corner.y", source.workspace_parameters.max_corner.y); + metadata.append(name_ + ".workspace_parameters.max_corner.z", source.workspace_parameters.max_corner.z); + return MoveItErrorCode::SUCCESS; +}; + +// StartStateJointStateFeatures. + +StartStateJointStateFeatures::StartStateJointStateFeatures(double match_tolerance) + : name_("StartStateJointStateFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string StartStateJointStateFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsExactFetchQuery(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group) const +{ + return appendRobotStateJointStateAsInsertMetadata(metadata, source.start_state, move_group, name_ + ".start_state"); +}; + +MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsFetchQueryWithTolerance( + Query& query, const MotionPlanRequest& source, const MoveGroupInterface& move_group, double match_tolerance) const +{ + return appendRobotStateJointStateAsFetchQueryWithTolerance(query, source.start_state, move_group, match_tolerance, + name_ + ".start_state"); +}; + +// "Goal" features. ================================================================================ + +// MaxSpeedAndAccelerationFeatures. + +MaxSpeedAndAccelerationFeatures::MaxSpeedAndAccelerationFeatures() : name_("MaxSpeedAndAccelerationFeatures") +{ +} + +std::string MaxSpeedAndAccelerationFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode +MaxSpeedAndAccelerationFeatures::appendFeaturesAsExactFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& /*move_group*/, + double /*exact_match_precision*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + query.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + query.appendLTE(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return MoveItErrorCode::SUCCESS; +}; + +MoveItErrorCode +MaxSpeedAndAccelerationFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, const MotionPlanRequest& source, + const MoveGroupInterface& /*move_group*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + metadata.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + metadata.append(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return MoveItErrorCode::SUCCESS; +}; + +// GoalConstraintsFeatures. + +GoalConstraintsFeatures::GoalConstraintsFeatures(double match_tolerance) + : name_("GoalConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string GoalConstraintsFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsExactFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsInsertMetadata(metadata, source.goal_constraints, move_group, workspace_id, + name_ + ".goal_constraints"); +}; + +MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsFetchQueryWithTolerance(query, source.goal_constraints, move_group, match_tolerance, + workspace_id, name_ + ".goal_constraints"); +}; + +// PathConstraintsFeatures. + +PathConstraintsFeatures::PathConstraintsFeatures(double match_tolerance) + : name_("PathConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string PathConstraintsFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsExactFetchQuery(Query& query, const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsInsertMetadata(metadata, { source.path_constraints }, move_group, workspace_id, + name_ + ".path_constraints"); +}; + +MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsFetchQueryWithTolerance(query, { source.path_constraints }, move_group, match_tolerance, + workspace_id, name_ + ".path_constraints"); +}; + +// TrajectoryConstraintsFeatures. + +TrajectoryConstraintsFeatures::TrajectoryConstraintsFeatures(double match_tolerance) + : name_("TrajectoryConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string TrajectoryConstraintsFeatures::getName() const +{ + return name_; +} + +MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsExactFetchQuery(Query& query, + const MotionPlanRequest& source, + const MoveGroupInterface& move_group, + double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +MoveItErrorCode +TrajectoryConstraintsFeatures::appendFeaturesAsInsertMetadata(Metadata& metadata, const MotionPlanRequest& source, + const MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsInsertMetadata(metadata, source.trajectory_constraints.constraints, move_group, + workspace_id, name_ + ".trajectory_constraints.constraints"); +}; + +MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + Query& query, const MotionPlanRequest& source, const MoveGroupInterface& move_group, double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + return appendConstraintsAsFetchQueryWithTolerance(query, source.trajectory_constraints.constraints, move_group, + match_tolerance, workspace_id, + name_ + ".trajectory_constraints.constraints"); +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index de4939428e..b0a5488025 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -17,16 +17,40 @@ * @author methylDragon */ +#include #include #include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include #include #include #include -#include -#include -#include +// Cache insert policies. +#include +#include + +// Features. +#include +#include +#include +#include + #include namespace moveit_ros @@ -34,81 +58,73 @@ namespace moveit_ros namespace trajectory_cache { -using warehouse_ros::MessageWithMetadata; -using warehouse_ros::Metadata; -using warehouse_ros::Query; +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::MessageWithMetadata; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; -// Utils ======================================================================= +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; -// Ensure we always have a workspace frame ID. -// -// It makes sense to use getModelFrame() in the absence of a workspace parameter frame ID because the same method is -// used to populate the workspace header frame ID in the MoveGroupInterface's setWorkspace() method, which this function -// is associated with. -std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) +using ::moveit_msgs::msg::MotionPlanRequest; +using ::moveit_msgs::msg::RobotTrajectory; +using ::moveit_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::BestSeenExecutionTimePolicy; +using ::moveit_ros::trajectory_cache::CacheInsertPolicyInterface; +using ::moveit_ros::trajectory_cache::CartesianBestSeenExecutionTimePolicy; + +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +namespace { - if (workspace_parameters.header.frame_id.empty()) - { - return move_group.getRobotModel()->getModelFrame(); - } - else - { - return workspace_parameters.header.frame_id; - } + +const std::string EXECUTION_TIME = "execution_time_s"; +const std::string FRACTION = "fraction"; +const std::string PLANNING_TIME = "planning_time_s"; + +} // namespace + +// ================================================================================================= +// Default Behavior Helpers. +// ================================================================================================= + +std::vector>> +TrajectoryCache::getDefaultFeatures(double start_tolerance, double goal_tolerance) +{ + return BestSeenExecutionTimePolicy::getSupportedFeatures(start_tolerance, goal_tolerance); } -// Ensure we always have a cartesian path request frame ID. -// -// It makes sense to use getPoseReferenceFrame() in the absence of a frame ID in the request because the same method is -// used to populate the header frame ID in the MoveGroupInterface's computeCartesianPath() method, which this function is associated with. -std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& path_request) +std::unique_ptr> +TrajectoryCache::getDefaultCacheInsertPolicy() { - if (path_request.header.frame_id.empty()) - { - return move_group.getPoseReferenceFrame(); - } - else - { - return path_request.header.frame_id; - } + return std::make_unique(); } -// Append a range inclusive query with some tolerance about some center value. -void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) +std::vector>> +TrajectoryCache::getDefaultCartesianFeatures(double start_tolerance, double goal_tolerance, double min_fraction) { - query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); + return CartesianBestSeenExecutionTimePolicy::getSupportedFeatures(start_tolerance, goal_tolerance, min_fraction); } -// Sort constraint components by joint or link name. -void sortConstraints(std::vector& joint_constraints, - std::vector& position_constraints, - std::vector& orientation_constraints) +std::unique_ptr> +TrajectoryCache::getDefaultCartesianCacheInsertPolicy() { - std::sort(joint_constraints.begin(), joint_constraints.end(), - [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { - return l.joint_name < r.joint_name; - }); - - std::sort(position_constraints.begin(), position_constraints.end(), - [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { - return l.link_name < r.link_name; - }); - - std::sort(orientation_constraints.begin(), orientation_constraints.end(), - [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { - return l.link_name < r.link_name; - }); + return std::make_unique(); } -// Trajectory Cache ============================================================ +std::string TrajectoryCache::getDefaultSortFeature() +{ + return EXECUTION_TIME; +} + +// ================================================================================================= +// Cache Configuration. +// ================================================================================================= TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(node), logger_(moveit::getLogger("moveit.ros.trajectory_cache")) { - tf_buffer_ = std::make_unique(node_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); } bool TrajectoryCache::init(const TrajectoryCache::Options& options) @@ -125,23 +141,24 @@ bool TrajectoryCache::init(const TrajectoryCache::Options& options) return db_->connect(); } +// ================================================================================================= +// Getters and Setters. +// ================================================================================================= + unsigned TrajectoryCache::countTrajectories(const std::string& cache_namespace) { - auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_trajectory_cache", cache_namespace); return coll.count(); } unsigned TrajectoryCache::countCartesianTrajectories(const std::string& cache_namespace) { - auto coll = - db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); return coll.count(); } -// ============================================================================= -// GETTERS AND SETTERS -// ============================================================================= - std::string TrajectoryCache::getDbPath() const { return options_.db_path; @@ -162,65 +179,64 @@ void TrajectoryCache::setExactMatchPrecision(double exact_match_precision) options_.exact_match_precision = exact_match_precision; } -size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const +size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenPruningWorse() const { - return options_.num_additional_trajectories_to_preserve_when_deleting_worse; + return options_.num_additional_trajectories_to_preserve_when_pruning_worse; } -void TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( - size_t num_additional_trajectories_to_preserve_when_deleting_worse) +void TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenPruningWorse( + size_t num_additional_trajectories_to_preserve_when_pruning_worse) { - options_.num_additional_trajectories_to_preserve_when_deleting_worse = - num_additional_trajectories_to_preserve_when_deleting_worse; + options_.num_additional_trajectories_to_preserve_when_pruning_worse = + num_additional_trajectories_to_preserve_when_pruning_worse; } -// ============================================================================= -// MOTION PLAN TRAJECTORY CACHING -// ============================================================================= +// ================================================================================================= +// Motion Plan Trajectory Caching. +// ================================================================================================= -std::vector::ConstPtr> -TrajectoryCache::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, - const std::string& sort_by, bool ascending) const +std::vector::ConstPtr> TrajectoryCache::fetchAllMatchingTrajectories( + const MoveGroupInterface& move_group, const std::string& cache_namespace, const MotionPlanRequest& plan_request, + const std::vector>>& features, const std::string& sort_by, + bool ascending, bool metadata_only) const { - auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_trajectory_cache", cache_namespace); Query::Ptr query = coll.createQuery(); - - bool start_ok = this->extractAndAppendTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extractAndAppendTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); - - if (!start_ok || !goal_ok) + for (const auto& feature : features) { - RCLCPP_ERROR(logger_, "Could not construct trajectory query."); - return {}; + if (MoveItErrorCode ret = + feature->appendFeaturesAsFuzzyFetchQuery(*query, plan_request, move_group, + /*exact_match_precision=*/options_.exact_match_precision); + !ret) + { + RCLCPP_ERROR_STREAM(logger_, "Could not construct trajectory query: " << ret.message); + return {}; + } } - return coll.queryList(query, metadata_only, sort_by, ascending); } -MessageWithMetadata::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 ascending) const +MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingTrajectory( + const MoveGroupInterface& move_group, const std::string& cache_namespace, const MotionPlanRequest& plan_request, + const std::vector>>& features, const std::string& sort_by, + bool ascending, bool metadata_only) const { - // 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, ascending); - + // Find all matching, with metadata only. We'll use the ID of the best trajectory to pull it. + std::vector::ConstPtr> matching_trajectories = + this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request, features, sort_by, ascending, + /*metadata_only=*/true); if (matching_trajectories.empty()) { RCLCPP_DEBUG(logger_, "No matching trajectories found."); return nullptr; } - auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_trajectory_cache", cache_namespace); - // Best plan is at first index, since the lookup query was sorted by - // execution_time. + // Best trajectory is at first index, since the lookup query was sorted. int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); Query::Ptr best_query = coll.createQuery(); best_query->append("id", best_trajectory_id); @@ -228,194 +244,138 @@ MessageWithMetadata::ConstPtr TrajectoryCache return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::msg::MotionPlanRequest& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, - double planning_time_s, bool prune_worse_trajectories) +bool TrajectoryCache::insertTrajectory( + const MoveGroupInterface& move_group, const std::string& cache_namespace, const MotionPlanRequest& plan_request, + const MoveGroupInterface::Plan& plan, + CacheInsertPolicyInterface& cache_insert_policy, + bool prune_worse_trajectories, + const std::vector>>& additional_features) { - std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - - // Check pre-conditions - if (!trajectory.multi_dof_joint_trajectory.points.empty()) - { - RCLCPP_ERROR(logger_, "Skipping plan insert: Multi-DOF trajectory plans are not supported."); - return false; - } - if (workspace_frame_id.empty()) - { - RCLCPP_ERROR(logger_, "Skipping plan insert: Workspace frame ID cannot be empty."); - return false; - } - if (trajectory.joint_trajectory.header.frame_id.empty()) - { - RCLCPP_ERROR(logger_, "Skipping plan insert: Trajectory frame ID cannot be empty."); - return false; - } - if (workspace_frame_id != trajectory.joint_trajectory.header.frame_id) - { - RCLCPP_ERROR(logger_, - "Skipping plan insert: " - "Plan request frame (%s) does not match plan frame (%s).", - workspace_frame_id.c_str(), trajectory.joint_trajectory.header.frame_id.c_str()); - return false; - } - - auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_trajectory_cache", cache_namespace); - // Pull out trajectories "exactly" keyed by request in cache. - Query::Ptr exact_query = coll.createQuery(); - - bool start_query_ok = this->extractAndAppendTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); - bool goal_query_ok = this->extractAndAppendTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); - - if (!start_query_ok || !goal_query_ok) + // Check pre-preconditions. + if (MoveItErrorCode ret = cache_insert_policy.checkCacheInsertInputs(move_group, coll, plan_request, plan); !ret) { - RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct lookup query."); + RCLCPP_ERROR_STREAM(logger_, "Skipping trajectory insert, invalid inputs: " << ret.message); + cache_insert_policy.reset(); return false; } - auto exact_matches = - coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); + std::vector::ConstPtr> matching_entries = + cache_insert_policy.fetchMatchingEntries(move_group, coll, plan_request, plan, options_.exact_match_precision); - double best_execution_time = std::numeric_limits::infinity(); - if (!exact_matches.empty()) + // Prune. + if (prune_worse_trajectories) { - best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - - if (prune_worse_trajectories) + size_t preserved_count = 0; + for (const auto& matching_entry : matching_entries) { - size_t preserved_count = 0; - for (auto& match : exact_matches) + std::string prune_reason; + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_pruning_worse && + cache_insert_policy.shouldPruneMatchingEntry(move_group, plan_request, plan, matching_entry, &prune_reason)) { - double match_execution_time_s = match->lookupDouble("execution_time_s"); - if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && - execution_time_s < match_execution_time_s) - { - int delete_id = match->lookupInt("id"); - RCLCPP_DEBUG(logger_, - "Overwriting plan (id: %d): " - "execution_time (%es) > new trajectory's execution_time (%es)", - delete_id, match_execution_time_s, execution_time_s); - - Query::Ptr delete_query = coll.createQuery(); - delete_query->append("id", delete_id); - coll.removeMessages(delete_query); - } + int delete_id = matching_entry->lookupInt("id"); + RCLCPP_DEBUG_STREAM(logger_, "Pruning plan (id: `" << delete_id << "`): " << prune_reason); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); } } } - // Insert if candidate is best seen. - if (execution_time_s < best_execution_time) + // Insert. + std::string insert_reason; + if (cache_insert_policy.shouldInsert(move_group, plan_request, plan, &insert_reason)) { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extractAndAppendTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extractAndAppendTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); - insert_metadata->append("execution_time_s", execution_time_s); - insert_metadata->append("planning_time_s", planning_time_s); - - if (!start_meta_ok || !goal_meta_ok) + if (MoveItErrorCode ret = cache_insert_policy.appendInsertMetadata(*insert_metadata, move_group, plan_request, plan); + !ret) { - RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct insert metadata."); + RCLCPP_ERROR_STREAM(logger_, + "Skipping trajectory insert: Could not construct insert metadata from cache_insert_policy: " + << cache_insert_policy.getName() << ": " << ret.message); + cache_insert_policy.reset(); return false; } - RCLCPP_DEBUG(logger_, - "Inserting trajectory: New trajectory execution_time (%es) " - "is better than best trajectory's execution_time (%es)", - execution_time_s, best_execution_time); + for (const auto& additional_feature : additional_features) + { + if (MoveItErrorCode ret = + additional_feature->appendFeaturesAsInsertMetadata(*insert_metadata, plan_request, move_group); + !ret) + { + RCLCPP_ERROR_STREAM(logger_, + "Skipping trajectory insert: Could not construct insert metadata additional_feature: " + << additional_feature->getName() << ": " << ret.message); + cache_insert_policy.reset(); + return false; + } + } - coll.insert(trajectory, insert_metadata); + RCLCPP_DEBUG_STREAM(logger_, "Inserting trajectory:" << insert_reason); + coll.insert(plan.trajectory, insert_metadata); + cache_insert_policy.reset(); return true; } - - RCLCPP_DEBUG(logger_, - "Skipping plan insert: New trajectory execution_time (%es) " - "is worse than best trajectory's execution_time (%es)", - execution_time_s, best_execution_time); - return false; + else + { + RCLCPP_DEBUG_STREAM(logger_, "Skipping trajectory insert:" << insert_reason); + cache_insert_policy.reset(); + return false; + } } -// ============================================================================= -// CARTESIAN TRAJECTORY CACHING -// ============================================================================= - -moveit_msgs::srv::GetCartesianPath::Request -TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, - double max_step, double jump_threshold, bool avoid_collisions) -{ - moveit_msgs::srv::GetCartesianPath::Request out; - - move_group.constructRobotState(out.start_state); +// ================================================================================================= +// Cartesian Trajectory Caching. +// ================================================================================================= - out.group_name = move_group.getName(); - out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); - out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); - - out.header.frame_id = move_group.getPoseReferenceFrame(); - out.waypoints = waypoints; - out.max_step = max_step; - out.jump_threshold = jump_threshold; - out.path_constraints = moveit_msgs::msg::Constraints(); - out.avoid_collisions = avoid_collisions; - out.link_name = move_group.getEndEffectorLink(); - out.header.stamp = move_group.getNode()->now(); - - return out; -} - -std::vector::ConstPtr> -TrajectoryCache::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, - const std::string& sort_by, bool ascending) const +std::vector::ConstPtr> TrajectoryCache::fetchAllMatchingCartesianTrajectories( + const MoveGroupInterface& move_group, const std::string& cache_namespace, + const GetCartesianPath::Request& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending, bool metadata_only) const { - auto coll = - db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); Query::Ptr query = coll.createQuery(); - - bool start_ok = - this->extractAndAppendCartesianTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); - - if (!start_ok || !goal_ok) + for (const auto& feature : features) { - RCLCPP_ERROR(logger_, "Could not construct cartesian trajectory query."); - return {}; + if (MoveItErrorCode ret = + feature->appendFeaturesAsFuzzyFetchQuery(*query, plan_request, move_group, + /*exact_match_precision=*/options_.exact_match_precision); + !ret) + { + RCLCPP_ERROR_STREAM(logger_, "Could not construct cartesian trajectory query: " << ret.message); + return {}; + } } - - query->appendGTE("fraction", min_fraction); return coll.queryList(query, metadata_only, sort_by, ascending); } -MessageWithMetadata::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, bool ascending) const +MessageWithMetadata::ConstPtr TrajectoryCache::fetchBestMatchingCartesianTrajectory( + const MoveGroupInterface& move_group, const std::string& cache_namespace, + const GetCartesianPath::Request& plan_request, + const std::vector>>& features, + const std::string& sort_by, bool ascending, bool metadata_only) const { - // 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, ascending); - + // Find all matching, with metadata only. We'll use the ID of the best trajectory to pull it. + std::vector::ConstPtr> matching_trajectories = + this->fetchAllMatchingCartesianTrajectories(move_group, cache_namespace, plan_request, features, sort_by, + ascending, /*metadata_only=*/true); if (matching_trajectories.empty()) { RCLCPP_DEBUG(logger_, "No matching cartesian trajectories found."); return nullptr; } - auto coll = - db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + MessageCollection coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); - // Best plan is at first index, since the lookup query was sorted by - // execution_time. + // Best trajectory is at first index, since the lookup query was sorted. int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); Query::Ptr best_query = coll.createQuery(); best_query->append("id", best_trajectory_id); @@ -423,1015 +383,89 @@ MessageWithMetadata::ConstPtr TrajectoryCache return coll.findOne(best_query, metadata_only); } -bool TrajectoryCache::insertCartesianTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& cache_namespace, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, - const moveit_msgs::msg::RobotTrajectory& trajectory, - double execution_time_s, double planning_time_s, double fraction, - bool prune_worse_trajectories) +bool TrajectoryCache::insertCartesianTrajectory( + const MoveGroupInterface& move_group, const std::string& cache_namespace, + const GetCartesianPath::Request& plan_request, const GetCartesianPath::Response& plan, + CacheInsertPolicyInterface& + cache_insert_policy, + bool prune_worse_trajectories, + const std::vector>>& additional_features) { - std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + MessageCollection coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); - // Check pre-conditions - if (!trajectory.multi_dof_joint_trajectory.points.empty()) + // Check pre-preconditions. + if (MoveItErrorCode ret = cache_insert_policy.checkCacheInsertInputs(move_group, coll, plan_request, plan); !ret) { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Multi-DOF trajectory plans are not supported."); + RCLCPP_ERROR_STREAM(logger_, "Skipping cartesian trajectory insert, invalid inputs: " << ret.message); + cache_insert_policy.reset(); return false; } - if (path_request_frame_id.empty()) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Path request frame ID cannot be empty."); - return false; - } - if (trajectory.joint_trajectory.header.frame_id.empty()) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Trajectory frame ID cannot be empty."); - return false; - } - - auto coll = - db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); - // Pull out trajectories "exactly" keyed by request in cache. - Query::Ptr exact_query = coll.createQuery(); + std::vector::ConstPtr> matching_entries = + cache_insert_policy.fetchMatchingEntries(move_group, coll, plan_request, plan, options_.exact_match_precision); - bool start_query_ok = - this->extractAndAppendCartesianTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); - bool goal_query_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); - exact_query->append("fraction", fraction); - - if (!start_query_ok || !goal_query_ok) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Could not construct lookup query."); - return false; - } - - auto exact_matches = - coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); - double best_execution_time = std::numeric_limits::infinity(); - if (!exact_matches.empty()) + // Prune. + if (prune_worse_trajectories) { - best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - - if (prune_worse_trajectories) + size_t preserved_count = 0; + for (const auto& matching_entry : matching_entries) { - size_t preserved_count = 0; - for (auto& match : exact_matches) + std::string prune_reason; + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_pruning_worse && + cache_insert_policy.shouldPruneMatchingEntry(move_group, plan_request, plan, matching_entry, &prune_reason)) { - double match_execution_time_s = match->lookupDouble("execution_time_s"); - if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && - execution_time_s < match_execution_time_s) - { - int delete_id = match->lookupInt("id"); - RCLCPP_DEBUG(logger_, - "Overwriting cartesian trajectory (id: %d): " - "execution_time (%es) > new trajectory's execution_time (%es)", - delete_id, match_execution_time_s, execution_time_s); - - Query::Ptr delete_query = coll.createQuery(); - delete_query->append("id", delete_id); - coll.removeMessages(delete_query); - } - } - } - } - - // Insert if candidate is best seen. - if (execution_time_s < best_execution_time) - { - Metadata::Ptr insert_metadata = coll.createMetadata(); - - bool start_meta_ok = - this->extractAndAppendCartesianTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); - bool goal_meta_ok = - this->extractAndAppendCartesianTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); - insert_metadata->append("execution_time_s", execution_time_s); - insert_metadata->append("planning_time_s", planning_time_s); - insert_metadata->append("fraction", fraction); - - if (!start_meta_ok || !goal_meta_ok) - { - RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " - "Could not construct insert metadata."); - return false; - } - - RCLCPP_DEBUG(logger_, - "Inserting cartesian trajectory: New trajectory execution_time (%es) " - "is better than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); - - coll.insert(trajectory, insert_metadata); - return true; - } - - RCLCPP_DEBUG(logger_, - "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " - "is worse than best trajectory's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time, fraction); - return false; -} - -// ============================================================================= -// MOTION PLAN TRAJECTORY QUERY AND METADATA CONSTRUCTION -// ============================================================================= - -// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION ========================== - -bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( - Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const -{ - std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - match_tolerance += options_.exact_match_precision; - - // Make ignored members explicit - if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); - } - if (!plan_request.start_state.attached_collision_objects.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); - } - - query.append("group_name", plan_request.group_name); - - // Workspace params - // Match anything within our specified workspace limits. - query.append("workspace_parameters.header.frame_id", workspace_frame_id); - query.appendGTE("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); - query.appendGTE("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); - query.appendGTE("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); - query.appendLTE("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); - query.appendLTE("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); - query.appendLTE("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); - - // Joint state - // Only accounts for joint_state position. Ignores velocity and effort. - if (plan_request.start_state.is_diff) - { - // If plan request states that the start_state is_diff, then we need to get - // the current state from the move_group. - - // NOTE: methyldragon - - // I think if is_diff is on, the joint states will not be populated in all - // of our motion plan requests? If this isn't the case we might need to - // apply the joint states as offsets as well. - // - // TODO: Since MoveIt also potentially does another getCurrentState() call - // when planning, there is a chance that the current state in the cache - // differs from the state used in MoveIt's plan. - // - // When upstreaming this class to MoveIt, this issue should go away once - // the class is used within the move group's Plan call. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); - if (!current_state) - { - RCLCPP_WARN(logger_, "Skipping start query append: Could not get robot state."); - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - - moveit_msgs::msg::RobotState current_state_msg; - robotStateToRobotStateMsg(*current_state, current_state_msg); - - for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) - { - query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); - } - } - else - { - for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) - { - query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); - } - } - return true; -} - -bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( - Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const -{ - std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - match_tolerance += options_.exact_match_precision; - - // Make ignored members explicit - bool emit_position_constraint_warning = false; - for (auto& constraint : plan_request.goal_constraints) - { - for (auto& position_constraint : constraint.position_constraints) - { - if (!position_constraint.constraint_region.primitives.empty()) - { - emit_position_constraint_warning = true; - break; - } - } - if (emit_position_constraint_warning) - { - break; - } - } - if (emit_position_constraint_warning) - { - RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " - "Not supported."); - } - - queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, - match_tolerance); - - // Extract constraints (so we don't have cardinality on goal_constraint idx.) - std::vector joint_constraints; - std::vector position_constraints; - std::vector orientation_constraints; - for (auto& constraint : plan_request.goal_constraints) - { - for (auto& joint_constraint : constraint.joint_constraints) - { - joint_constraints.push_back(joint_constraint); - } - for (auto& position_constraint : constraint.position_constraints) - { - position_constraints.push_back(position_constraint); - } - for (auto& orientation_constraint : constraint.orientation_constraints) - { - orientation_constraints.push_back(orientation_constraint); - } - - // Also sort for even less cardinality. - sortConstraints(joint_constraints, position_constraints, orientation_constraints); - } - - // Joint constraints - size_t joint_idx = 0; - for (auto& constraint : joint_constraints) - { - std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); - - query.append(meta_name + ".joint_name", constraint.joint_name); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance); - query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); - query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); - } - - // Position constraints - // All offsets will be "frozen" and computed wrt. the workspace frame - // instead. - if (!position_constraints.empty()) - { - query.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); - - size_t pos_idx = 0; - for (auto& constraint : position_constraints) - { - std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(pos_idx++); - - // Compute offsets wrt. to workspace frame. - double x_offset = 0; - double y_offset = 0; - double z_offset = 0; - - if (workspace_frame_id != constraint.header.frame_id) - { - try - { - auto transform = - tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); - - x_offset = transform.transform.translation.x; - y_offset = transform.transform.translation.y; - z_offset = transform.transform.translation.z; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal query append: " - "Could not get goal transform for translation %s to %s: %s", - workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - } - - query.append(meta_name + ".link_name", constraint.link_name); - - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", - x_offset + constraint.target_point_offset.x, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", - y_offset + constraint.target_point_offset.y, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", - z_offset + constraint.target_point_offset.z, match_tolerance); - } - } - - // Orientation constraints - // All offsets will be "frozen" and computed wrt. the workspace frame - // instead. - if (!orientation_constraints.empty()) - { - query.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); - - size_t ori_idx = 0; - for (auto& constraint : orientation_constraints) - { - std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + int delete_id = matching_entry->lookupInt("id"); + RCLCPP_DEBUG_STREAM(logger_, "Pruning cartesian trajectory (id: `" << delete_id << "`): " << prune_reason); - // Compute offsets wrt. to workspace frame. - geometry_msgs::msg::Quaternion quat_offset; - quat_offset.x = 0; - quat_offset.y = 0; - quat_offset.z = 0; - quat_offset.w = 1; - - if (workspace_frame_id != constraint.header.frame_id) - { - try - { - auto transform = - tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); - - quat_offset = transform.transform.rotation; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal query append: " - "Could not get goal transform for orientation %s to %s: %s", - workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); } - - query.append(meta_name + ".link_name", constraint.link_name); - - // Orientation of constraint frame wrt. workspace frame - tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); - - // Added offset on top of the constraint frame's orientation stated in - // the constraint. - tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, - constraint.orientation.w); - - tf2_quat_frame_offset.normalize(); - tf2_quat_goal_offset.normalize(); - - auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; - final_quat.normalize(); - - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), - match_tolerance); } } - return true; -} - -// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION ======================= - -bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( - Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) const -{ - std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - - // Make ignored members explicit - if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); - } - if (!plan_request.start_state.attached_collision_objects.empty()) + // Insert. + std::string insert_reason; + if (cache_insert_policy.shouldInsert(move_group, plan_request, plan, &insert_reason)) { - RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); - } - - metadata.append("group_name", plan_request.group_name); - - // Workspace params - metadata.append("workspace_parameters.header.frame_id", workspace_frame_id); - metadata.append("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); - metadata.append("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); - metadata.append("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); - metadata.append("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); - metadata.append("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); - metadata.append("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); + Metadata::Ptr insert_metadata = coll.createMetadata(); - // Joint state - // Only accounts for joint_state position. Ignores velocity and effort. - if (plan_request.start_state.is_diff) - { - // If plan request states that the start_state is_diff, then we need to get - // the current state from the move_group. - - // NOTE: methyldragon - - // I think if is_diff is on, the joint states will not be populated in all - // of our motion plan requests? If this isn't the case we might need to - // apply the joint states as offsets as well. - // - // TODO: Since MoveIt also potentially does another getCurrentState() call - // when planning, there is a chance that the current state in the cache - // differs from the state used in MoveIt's plan. - // - // When upstreaming this class to MoveIt, this issue should go away once - // the class is used within the move group's Plan call. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); - if (!current_state) + if (MoveItErrorCode ret = cache_insert_policy.appendInsertMetadata(*insert_metadata, move_group, plan_request, plan); + !ret) { - RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. + RCLCPP_ERROR_STREAM(logger_, "Skipping cartesian trajectory insert: Could not construct insert metadata from " + "cache_insert_policy: " + << cache_insert_policy.getName() << ": " << ret.message); + cache_insert_policy.reset(); return false; } - moveit_msgs::msg::RobotState current_state_msg; - robotStateToRobotStateMsg(*current_state, current_state_msg); - - for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) - { - metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - metadata.append("start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i)); - } - } - else - { - for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) - { - metadata.append("start_state.joint_state.name_" + std::to_string(i), - plan_request.start_state.joint_state.name.at(i)); - metadata.append("start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i)); - } - } - - return true; -} - -bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( - Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::MotionPlanRequest& plan_request) const -{ - std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); - - // Make ignored members explicit - bool emit_position_constraint_warning = false; - for (auto& constraint : plan_request.goal_constraints) - { - for (auto& position_constraint : constraint.position_constraints) - { - if (!position_constraint.constraint_region.primitives.empty()) - { - emit_position_constraint_warning = true; - break; - } - } - if (emit_position_constraint_warning) - { - break; - } - } - if (emit_position_constraint_warning) - { - RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " - "Not supported."); - } - - metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); - metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); - metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed); - - // Extract constraints (so we don't have cardinality on goal_constraint idx.) - std::vector joint_constraints; - std::vector position_constraints; - std::vector orientation_constraints; - for (auto& constraint : plan_request.goal_constraints) - { - for (auto& joint_constraint : constraint.joint_constraints) - { - joint_constraints.push_back(joint_constraint); - } - for (auto& position_constraint : constraint.position_constraints) - { - position_constraints.push_back(position_constraint); - } - for (auto& orientation_constraint : constraint.orientation_constraints) - { - orientation_constraints.push_back(orientation_constraint); - } - - // Also sort for even less cardinality. - sortConstraints(joint_constraints, position_constraints, orientation_constraints); - } - - // Joint constraints - size_t joint_idx = 0; - for (auto& constraint : joint_constraints) - { - std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); - - metadata.append(meta_name + ".joint_name", constraint.joint_name); - metadata.append(meta_name + ".position", constraint.position); - metadata.append(meta_name + ".tolerance_above", constraint.tolerance_above); - metadata.append(meta_name + ".tolerance_below", constraint.tolerance_below); - } - - // Position constraints - if (!position_constraints.empty()) - { - // All offsets will be "frozen" and computed wrt. the workspace frame - // instead. - metadata.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); - - size_t position_idx = 0; - for (auto& constraint : position_constraints) - { - std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(position_idx++); - - // Compute offsets wrt. to workspace frame. - double x_offset = 0; - double y_offset = 0; - double z_offset = 0; - - if (workspace_frame_id != constraint.header.frame_id) - { - try - { - auto transform = - tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); - - x_offset = transform.transform.translation.x; - y_offset = transform.transform.translation.y; - z_offset = transform.transform.translation.z; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal metadata append: " - "Could not get goal transform for translation %s to %s: %s", - workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - } - - metadata.append(meta_name + ".link_name", constraint.link_name); - - metadata.append(meta_name + ".target_point_offset.x", x_offset + constraint.target_point_offset.x); - metadata.append(meta_name + ".target_point_offset.y", y_offset + constraint.target_point_offset.y); - metadata.append(meta_name + ".target_point_offset.z", z_offset + constraint.target_point_offset.z); - } - } - - // Orientation constraints - if (!orientation_constraints.empty()) - { - // All offsets will be "frozen" and computed wrt. the workspace frame - // instead. - metadata.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); - - size_t ori_idx = 0; - for (auto& constraint : orientation_constraints) + for (const auto& additional_feature : additional_features) { - std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); - - // Compute offsets wrt. to workspace frame. - geometry_msgs::msg::Quaternion quat_offset; - quat_offset.x = 0; - quat_offset.y = 0; - quat_offset.z = 0; - quat_offset.w = 1; - - if (workspace_frame_id != constraint.header.frame_id) + if (MoveItErrorCode ret = + additional_feature->appendFeaturesAsInsertMetadata(*insert_metadata, plan_request, move_group); + !ret) { - try - { - auto transform = - tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); - - quat_offset = transform.transform.rotation; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal metadata append: " - "Could not get goal transform for orientation %s to %s: %s", - workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } + RCLCPP_ERROR_STREAM( + logger_, "Skipping cartesian trajectory insert: Could not construct insert metadata additional_feature: " + << additional_feature->getName() << ": " << ret.message); + cache_insert_policy.reset(); + return false; } - - metadata.append(meta_name + ".link_name", constraint.link_name); - - // Orientation of constraint frame wrt. workspace frame - tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); - - // Added offset on top of the constraint frame's orientation stated in - // the constraint. - tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, - constraint.orientation.w); - - tf2_quat_frame_offset.normalize(); - tf2_quat_goal_offset.normalize(); - - auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; - final_quat.normalize(); - - metadata.append(meta_name + ".target_point_offset.x", final_quat.getX()); - metadata.append(meta_name + ".target_point_offset.y", final_quat.getY()); - metadata.append(meta_name + ".target_point_offset.z", final_quat.getZ()); - metadata.append(meta_name + ".target_point_offset.w", final_quat.getW()); } - } - - return true; -} - -// ============================================================================= -// CARTESIAN TRAJECTORY QUERY AND METADATA CONSTRUCTION -// ============================================================================= - -// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION ============================ -bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( - Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const -{ - std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - match_tolerance += options_.exact_match_precision; - - // Make ignored members explicit - if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); - } - if (!plan_request.start_state.attached_collision_objects.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); - } - - query.append("group_name", plan_request.group_name); - query.append("path_request.header.frame_id", path_request_frame_id); - - // Joint state - // Only accounts for joint_state position. Ignores velocity and effort. - if (plan_request.start_state.is_diff) - { - // If plan request states that the start_state is_diff, then we need to get - // the current state from the move_group. - - // NOTE: methyldragon - - // I think if is_diff is on, the joint states will not be populated in all - // of our motion plan requests? If this isn't the case we might need to - // apply the joint states as offsets as well. - // - // TODO: Since MoveIt also potentially does another getCurrentState() call - // when planning, there is a chance that the current state in the cache - // differs from the state used in MoveIt's plan. - // - // When upstreaming this class to MoveIt, this issue should go away once - // the class is used within the move group's Plan call. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); - if (!current_state) - { - RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - - moveit_msgs::msg::RobotState current_state_msg; - robotStateToRobotStateMsg(*current_state, current_state_msg); - - for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) - { - query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); - } - } - else - { - for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) - { - query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); - } - } - - return true; -} - -bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( - Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const -{ - std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - match_tolerance += options_.exact_match_precision; - - // Make ignored members explicit - if (!plan_request.path_constraints.joint_constraints.empty() || - !plan_request.path_constraints.position_constraints.empty() || - !plan_request.path_constraints.orientation_constraints.empty() || - !plan_request.path_constraints.visibility_constraints.empty()) - { - RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); - } - if (plan_request.avoid_collisions) - { - RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); - } - - queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_step", plan_request.max_step, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); - - // Waypoints - // Restating them in terms of the robot model frame (usually base_link) - std::string base_frame = move_group.getRobotModel()->getModelFrame(); - - // Compute offsets. - double x_offset = 0; - double y_offset = 0; - double z_offset = 0; - - geometry_msgs::msg::Quaternion quat_offset; - quat_offset.x = 0; - quat_offset.y = 0; - quat_offset.z = 0; - quat_offset.w = 1; - - if (base_frame != path_request_frame_id) - { - try - { - auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); - - x_offset = transform.transform.translation.x; - y_offset = transform.transform.translation.y; - z_offset = transform.transform.translation.z; - quat_offset = transform.transform.rotation; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal metadata append: " - "Could not get goal transform for %s to %s: %s", - base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - } - - tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); - tf2_quat_frame_offset.normalize(); - - size_t waypoint_idx = 0; - for (auto& waypoint : plan_request.waypoints) - { - std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); - - // Apply offsets - // Position - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, - match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, - match_tolerance); - - // Orientation - tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, - waypoint.orientation.w); - tf2_quat_goal_offset.normalize(); - - auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; - final_quat.normalize(); - - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); - } - - query.append("link_name", plan_request.link_name); - query.append("header.frame_id", base_frame); - - return true; -} - -// CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION ========================= - -bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( - Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const -{ - std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - - // Make ignored members explicit - if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); - } - if (!plan_request.start_state.attached_collision_objects.empty()) - { - RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); - } - - metadata.append("group_name", plan_request.group_name); - metadata.append("path_request.header.frame_id", path_request_frame_id); - - // Joint state - // Only accounts for joint_state position. Ignores velocity and effort. - if (plan_request.start_state.is_diff) - { - // If plan request states that the start_state is_diff, then we need to get - // the current state from the move_group. - - // NOTE: methyldragon - - // I think if is_diff is on, the joint states will not be populated in all - // of our motion plan requests? If this isn't the case we might need to - // apply the joint states as offsets as well. - // - // TODO: Since MoveIt also potentially does another getCurrentState() call - // when planning, there is a chance that the current state in the cache - // differs from the state used in MoveIt's plan. - // - // When upstreaming this class to MoveIt, this issue should go away once - // the class is used within the move group's Plan call. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); - if (!current_state) - { - RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - - moveit_msgs::msg::RobotState current_state_msg; - robotStateToRobotStateMsg(*current_state, current_state_msg); - - for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) - { - metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - metadata.append("start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i)); - } + RCLCPP_DEBUG_STREAM(logger_, "Inserting cartesian trajectory:" << insert_reason); + coll.insert(plan.solution, insert_metadata); + cache_insert_policy.reset(); + return true; } else { - for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) - { - metadata.append("start_state.joint_state.name_" + std::to_string(i), - plan_request.start_state.joint_state.name.at(i)); - metadata.append("start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i)); - } - } - - return true; -} - -bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( - Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const -{ - std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); - - // Make ignored members explicit - if (!plan_request.path_constraints.joint_constraints.empty() || - !plan_request.path_constraints.position_constraints.empty() || - !plan_request.path_constraints.orientation_constraints.empty() || - !plan_request.path_constraints.visibility_constraints.empty()) - { - RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); - } - if (plan_request.avoid_collisions) - { - RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); - } - - metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); - metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); - metadata.append("max_step", plan_request.max_step); - metadata.append("jump_threshold", plan_request.jump_threshold); - - // Waypoints - // Restating them in terms of the robot model frame (usually base_link) - std::string base_frame = move_group.getRobotModel()->getModelFrame(); - - // Compute offsets. - double x_offset = 0; - double y_offset = 0; - double z_offset = 0; - - geometry_msgs::msg::Quaternion quat_offset; - quat_offset.x = 0; - quat_offset.y = 0; - quat_offset.z = 0; - quat_offset.w = 1; - - if (base_frame != path_request_frame_id) - { - try - { - auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); - - x_offset = transform.transform.translation.x; - y_offset = transform.transform.translation.y; - z_offset = transform.transform.translation.z; - quat_offset = transform.transform.rotation; - } - catch (tf2::TransformException& ex) - { - RCLCPP_WARN(logger_, - "Skipping goal metadata append: " - "Could not get goal transform for %s to %s: %s", - base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); - - // NOTE: methyldragon - - // Ideally we would restore the original state here and undo our changes, however copy of the query is not - // supported. - return false; - } - } - - tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); - tf2_quat_frame_offset.normalize(); - - size_t waypoint_idx = 0; - for (auto& waypoint : plan_request.waypoints) - { - std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); - - // Apply offsets - // Position - metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); - metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); - metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); - - // Orientation - tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, - waypoint.orientation.w); - tf2_quat_goal_offset.normalize(); - - auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; - final_quat.normalize(); - - metadata.append(meta_name + ".orientation.x", final_quat.getX()); - metadata.append(meta_name + ".orientation.y", final_quat.getY()); - metadata.append(meta_name + ".orientation.z", final_quat.getZ()); - metadata.append(meta_name + ".orientation.w", final_quat.getW()); + RCLCPP_DEBUG_STREAM(logger_, "Skipping cartesian insert:" << insert_reason); + cache_insert_policy.reset(); + return false; } - - metadata.append("link_name", plan_request.link_name); - metadata.append("header.frame_id", base_frame); - - return true; } } // namespace trajectory_cache diff --git a/moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp new file mode 100644 index 0000000000..5d3a293408 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/utils/utils.cpp @@ -0,0 +1,650 @@ +// 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 the utilities used by the trajectory_cache package. + * @author methylDragon + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace +{ + +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.ros.trajectory_cache.features"); +} + +} // namespace + +// Frames. ========================================================================================= + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit::core::MoveItErrorCode; +using ::moveit::planning_interface::MoveGroupInterface; + +using ::moveit_msgs::srv::GetCartesianPath; + +std::string getWorkspaceFrameId(const MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) +{ + if (workspace_parameters.header.frame_id.empty()) + { + return move_group.getRobotModel()->getModelFrame(); + } + else + { + return workspace_parameters.header.frame_id; + } +} + +std::string getCartesianPathRequestFrameId(const MoveGroupInterface& move_group, + const GetCartesianPath::Request& path_request) +{ + if (path_request.header.frame_id.empty()) + { + return move_group.getPoseReferenceFrame(); + } + else + { + return path_request.header.frame_id; + } +} + +MoveItErrorCode restateInNewFrame(const std::shared_ptr& tf, const std::string& target_frame, + const std::string& source_frame, geometry_msgs::msg::Point* translation, + geometry_msgs::msg::Quaternion* rotation, const tf2::TimePoint& lookup_time) +{ + if (target_frame == source_frame) + { + return MoveItErrorCode::SUCCESS; + } + if (translation == nullptr && rotation == nullptr) + { + return MoveItErrorCode::SUCCESS; + } + + // Fetch transforms. + geometry_msgs::msg::TransformStamped transform; + try + { + transform = tf->lookupTransform(target_frame, source_frame, lookup_time); + } + catch (tf2::TransformException& ex) + { + std::stringstream ss; + ss << "Could not get transform for " << source_frame << " to " << target_frame << ": " << ex.what(); + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE, ss.str()); + } + + // Translation. + if (translation != nullptr) + { + translation->x += transform.transform.translation.x; + translation->y += transform.transform.translation.y; + translation->z += transform.transform.translation.z; + } + + // Rotation. + if (rotation != nullptr) + { + tf2::Quaternion input_quat(rotation->x, rotation->y, rotation->z, rotation->w); + tf2::Quaternion transform_quat(transform.transform.rotation.x, transform.transform.rotation.y, + transform.transform.rotation.z, transform.transform.rotation.w); + + input_quat.normalize(); + transform_quat.normalize(); + + auto final_quat = input_quat * transform_quat; + final_quat.normalize(); + + rotation->x = final_quat.getX(); + rotation->y = final_quat.getY(); + rotation->z = final_quat.getZ(); + rotation->w = final_quat.getW(); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +} + +// Execution Time. ================================================================================= + +double getExecutionTime(const moveit_msgs::msg::RobotTrajectory& trajectory) +{ + return rclcpp::Duration(trajectory.joint_trajectory.points.back().time_from_start).seconds(); +} + +// Request Construction. =========================================================================== + +GetCartesianPath::Request constructGetCartesianPathRequest(MoveGroupInterface& move_group, + const std::vector& waypoints, + double max_step, double jump_threshold, + bool avoid_collisions) +{ + GetCartesianPath::Request out; + + move_group.constructRobotState(out.start_state); + + out.group_name = move_group.getName(); + out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); + out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = max_step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + out.header.stamp = move_group.getNode()->now(); + + return out; +} + +// Queries. ======================================================================================== + +void queryAppendCenterWithTolerance(Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); +} + +// Constraints. ==================================================================================== + +void sortJointConstraints(std::vector& joint_constraints) +{ + std::sort(joint_constraints.begin(), joint_constraints.end(), + [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { + return l.joint_name < r.joint_name; + }); +} + +void sortPositionConstraints(std::vector& position_constraints) +{ + std::sort(position_constraints.begin(), position_constraints.end(), + [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { + return l.link_name < r.link_name; + }); +} + +void sortOrientationConstraints(std::vector& orientation_constraints) +{ + std::sort(orientation_constraints.begin(), orientation_constraints.end(), + [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { + return l.link_name < r.link_name; + }); +} + +moveit::core::MoveItErrorCode +appendConstraintsAsFetchQueryWithTolerance(Query& query, std::vector constraints, + const MoveGroupInterface& move_group, double match_tolerance, + const std::string& reference_frame_id, const std::string& prefix) +{ + const std::shared_ptr tf_buffer = move_group.getTF(); // NOLINT: Deliberate lifetime extension. + + // Make ignored members explicit. + + bool emit_position_constraint_warning = false; + for (const auto& constraint : constraints) + { + for (const auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".position_constraints.constraint_region: Not supported."); + } + + bool emit_visibility_constraint_warning = false; + for (const auto& constraint : constraints) + { + if (!constraint.visibility_constraints.empty()) + { + emit_visibility_constraint_warning = true; + break; + } + } + if (emit_visibility_constraint_warning) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".visibility_constraints: Not supported."); + } + + // Begin extraction. + + size_t constraint_idx = 0; + for (auto& constraint : constraints) // Non-const as constraints are sorted in-place. + { + // We sort to avoid cardinality. + sortJointConstraints(constraint.joint_constraints); + sortPositionConstraints(constraint.position_constraints); + sortOrientationConstraints(constraint.orientation_constraints); + + std::string constraint_prefix = prefix + "_" + std::to_string(constraint_idx++); + + // Joint constraints + size_t joint_idx = 0; + for (const auto& joint_constraint : constraint.joint_constraints) + { + std::string query_name = constraint_prefix + ".joint_constraints_" + std::to_string(joint_idx++); + query.append(query_name + ".joint_name", joint_constraint.joint_name); + queryAppendCenterWithTolerance(query, query_name + ".position", joint_constraint.position, match_tolerance); + query.appendGTE(query_name + ".tolerance_above", joint_constraint.tolerance_above); + query.appendLTE(query_name + ".tolerance_below", joint_constraint.tolerance_below); + } + + // Position constraints + if (!constraint.position_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame instead. + query.append(constraint_prefix + ".position_constraints.header.frame_id", reference_frame_id); + + size_t position_idx = 0; + for (const auto& position_constraint : constraint.position_constraints) + { + std::string query_name = constraint_prefix + ".position_constraints_" + std::to_string(position_idx++); + + geometry_msgs::msg::Point canonical_position; + canonical_position.x = position_constraint.target_point_offset.x; + canonical_position.y = position_constraint.target_point_offset.y; + canonical_position.z = position_constraint.target_point_offset.z; + + // Canonicalize to robot base frame if necessary. + if (position_constraint.header.frame_id != reference_frame_id) + { + if (MoveItErrorCode status = + restateInNewFrame(move_group.getTF(), position_constraint.header.frame_id, reference_frame_id, + &canonical_position, /*rotation=*/nullptr, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << ":" << query_name << " query append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + query.append(query_name + ".link_name", position_constraint.link_name); + queryAppendCenterWithTolerance(query, query_name + ".target_point_offset.x", canonical_position.x, + match_tolerance); + queryAppendCenterWithTolerance(query, query_name + ".target_point_offset.y", canonical_position.y, + match_tolerance); + queryAppendCenterWithTolerance(query, query_name + ".target_point_offset.z", canonical_position.z, + match_tolerance); + } + } + + // Orientation constraints + if (!constraint.orientation_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame instead. + query.append(constraint_prefix + ".orientation_constraints.header.frame_id", reference_frame_id); + + size_t ori_idx = 0; + for (const auto& orientation_constraint : constraint.orientation_constraints) + { + std::string query_name = constraint_prefix + ".orientation_constraints_" + std::to_string(ori_idx++); + geometry_msgs::msg::Quaternion canonical_orientation = orientation_constraint.orientation; + + // Canonicalize to robot base frame if necessary. + if (orientation_constraint.header.frame_id != reference_frame_id) + { + if (MoveItErrorCode status = + restateInNewFrame(move_group.getTF(), orientation_constraint.header.frame_id, reference_frame_id, + /*translation=*/nullptr, &canonical_orientation, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << ":" << query_name << " query append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + query.append(query_name + ".link_name", orientation_constraint.link_name); + queryAppendCenterWithTolerance(query, query_name + ".orientation.x", canonical_orientation.x, match_tolerance); + queryAppendCenterWithTolerance(query, query_name + ".orientation.y", canonical_orientation.y, match_tolerance); + queryAppendCenterWithTolerance(query, query_name + ".orientation.z", canonical_orientation.z, match_tolerance); + queryAppendCenterWithTolerance(query, query_name + ".orientation.w", canonical_orientation.w, match_tolerance); + } + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +} + +moveit::core::MoveItErrorCode appendConstraintsAsInsertMetadata(Metadata& metadata, + std::vector constraints, + const MoveGroupInterface& move_group, + const std::string& workspace_frame_id, + const std::string& prefix) +{ + const std::shared_ptr tf_buffer = move_group.getTF(); // NOLINT: Deliberate lifetime extension. + + // Make ignored members explicit + + bool emit_position_constraint_warning = false; + for (const auto& constraint : constraints) + { + for (const auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".position_constraints.constraint_region: Not supported."); + } + + bool emit_visibility_constraint_warning = false; + for (const auto& constraint : constraints) + { + if (!constraint.visibility_constraints.empty()) + { + emit_visibility_constraint_warning = true; + break; + } + } + if (emit_visibility_constraint_warning) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".visibility_constraints: Not supported."); + } + + // Begin extraction. + + size_t constraint_idx = 0; + for (auto& constraint : constraints) // Non-const as constraints are sorted in-place. + { + // We sort to avoid cardinality. + sortJointConstraints(constraint.joint_constraints); + sortPositionConstraints(constraint.position_constraints); + sortOrientationConstraints(constraint.orientation_constraints); + + std::string constraint_prefix = prefix + "_" + std::to_string(constraint_idx++); + + // Joint constraints + size_t joint_idx = 0; + for (const auto& joint_constraint : constraint.joint_constraints) + { + std::string meta_name = constraint_prefix + ".joint_constraints_" + std::to_string(joint_idx++); + metadata.append(meta_name + ".joint_name", joint_constraint.joint_name); + metadata.append(meta_name + ".position", joint_constraint.position); + metadata.append(meta_name + ".tolerance_above", joint_constraint.tolerance_above); + metadata.append(meta_name + ".tolerance_below", joint_constraint.tolerance_below); + } + + // Position constraints + if (!constraint.position_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame instead. + metadata.append(constraint_prefix + ".position_constraints.header.frame_id", workspace_frame_id); + + size_t position_idx = 0; + for (const auto& position_constraint : constraint.position_constraints) + { + std::string meta_name = constraint_prefix + ".position_constraints_" + std::to_string(position_idx++); + + geometry_msgs::msg::Point canonical_position; + canonical_position.x = position_constraint.target_point_offset.x; + canonical_position.y = position_constraint.target_point_offset.y; + canonical_position.z = position_constraint.target_point_offset.z; + + // Canonicalize to robot base frame if necessary. + if (position_constraint.header.frame_id != workspace_frame_id) + { + if (MoveItErrorCode status = + restateInNewFrame(move_group.getTF(), position_constraint.header.frame_id, workspace_frame_id, + &canonical_position, /*rotation=*/nullptr, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << ":" << meta_name << " metadata append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + metadata.append(meta_name + ".link_name", position_constraint.link_name); + metadata.append(meta_name + ".target_point_offset.x", canonical_position.x); + metadata.append(meta_name + ".target_point_offset.y", canonical_position.y); + metadata.append(meta_name + ".target_point_offset.z", canonical_position.z); + } + } + + // Orientation constraints + if (!constraint.orientation_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame instead. + metadata.append(constraint_prefix + ".orientation_constraints.header.frame_id", workspace_frame_id); + + size_t ori_idx = 0; + for (const auto& orientation_constraint : constraint.orientation_constraints) + { + std::string meta_name = constraint_prefix + ".orientation_constraints_" + std::to_string(ori_idx++); + geometry_msgs::msg::Quaternion canonical_orientation = orientation_constraint.orientation; + + // Canonicalize to robot base frame if necessary. + if (orientation_constraint.header.frame_id != workspace_frame_id) + { + if (MoveItErrorCode status = + restateInNewFrame(move_group.getTF(), orientation_constraint.header.frame_id, workspace_frame_id, + /*translation=*/nullptr, &canonical_orientation, tf2::TimePointZero); + status != MoveItErrorCode::SUCCESS) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << ":" << meta_name << " metadata append: " << status.message; + return MoveItErrorCode(status.val, status.message); + } + } + + metadata.append(meta_name + ".link_name", orientation_constraint.link_name); + metadata.append(meta_name + ".orientation.x", canonical_orientation.x); + metadata.append(meta_name + ".orientation.y", canonical_orientation.y); + metadata.append(meta_name + ".orientation.z", canonical_orientation.z); + metadata.append(meta_name + ".orientation.w", canonical_orientation.w); + } + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +} + +// RobotState. ===================================================================================== + +moveit::core::MoveItErrorCode +appendRobotStateJointStateAsFetchQueryWithTolerance(Query& query, const moveit_msgs::msg::RobotState& robot_state, + const MoveGroupInterface& move_group, double match_tolerance, + const std::string& prefix) +{ + // Make ignored members explicit + + if (!robot_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".multi_dof_joint_states: Not supported."); + } + if (!robot_state.attached_collision_objects.empty()) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".attached_collision_objects: Not supported."); + } + + // Begin extraction. + + if (robot_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of the motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // This issue should go away once the class is used within the move group's + // Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the metadata is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << " query append: " + << "Could not get robot state."; + return MoveItErrorCode(MoveItErrorCode::UNABLE_TO_AQUIRE_SENSOR_DATA, ss.str()); + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); ++i) + { + query.append(prefix + ".joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + queryAppendCenterWithTolerance(query, prefix + ".joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < robot_state.joint_state.name.size(); ++i) + { + query.append(prefix + ".joint_state.name_" + std::to_string(i), robot_state.joint_state.name.at(i)); + queryAppendCenterWithTolerance(query, prefix + ".joint_state.position_" + std::to_string(i), + robot_state.joint_state.position.at(i), match_tolerance); + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +} + +moveit::core::MoveItErrorCode +appendRobotStateJointStateAsInsertMetadata(Metadata& metadata, const moveit_msgs::msg::RobotState& robot_state, + const MoveGroupInterface& move_group, const std::string& prefix) +{ + // Make ignored members explicit + + if (!robot_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".multi_dof_joint_states: Not supported."); + } + if (!robot_state.attached_collision_objects.empty()) + { + RCLCPP_WARN_STREAM(getLogger(), "Ignoring " << prefix << ".attached_collision_objects: Not supported."); + } + + // Begin extraction. + + if (robot_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of the motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // This issue should go away once the class is used within the move group's + // Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the metadata is not + // supported. + std::stringstream ss; + ss << "Skipping " << prefix << " metadata append: " + << "Could not get robot state."; + return MoveItErrorCode(MoveItErrorCode::UNABLE_TO_AQUIRE_SENSOR_DATA, ss.str()); + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); ++i) + { + metadata.append(prefix + ".joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + metadata.append(prefix + ".joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < robot_state.joint_state.name.size(); ++i) + { + metadata.append(prefix + ".joint_state.name_" + std::to_string(i), robot_state.joint_state.name.at(i)); + metadata.append(prefix + ".joint_state.position_" + std::to_string(i), robot_state.joint_state.position.at(i)); + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/test/CMakeLists.txt b/moveit_ros/trajectory_cache/test/CMakeLists.txt new file mode 100644 index 0000000000..ae39a79c46 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/CMakeLists.txt @@ -0,0 +1,152 @@ +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(ros_testing REQUIRED) + find_package(warehouse_ros_sqlite REQUIRED) + + # Fixtures. + add_library(warehouse_fixture SHARED fixtures/warehouse_fixture.cpp) + target_include_directories( + warehouse_fixture PUBLIC $) + ament_target_dependencies(warehouse_fixture ${TRAJECTORY_CACHE_DEPENDENCIES} + ament_cmake_gtest warehouse_ros_sqlite) + + add_library(move_group_fixture SHARED fixtures/move_group_fixture.cpp) + target_include_directories( + move_group_fixture PUBLIC $) + ament_target_dependencies(move_group_fixture ${TRAJECTORY_CACHE_DEPENDENCIES} + ament_cmake_gtest warehouse_ros_sqlite) + + # Test utils library. + ament_add_gtest(test_utils utils/test_utils.cpp) + target_link_libraries(test_utils moveit_ros_trajectory_cache_utils_lib + warehouse_fixture) + + ament_add_gtest_executable(test_utils_with_move_group + utils/test_utils_with_move_group.cpp) + target_link_libraries( + test_utils_with_move_group moveit_ros_trajectory_cache_utils_lib + move_group_fixture) + add_ros_test( + fixtures/gtest_with_move_group.py + TARGET + test_utils_with_move_group + TIMEOUT + 30 + ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_utils_with_move_group") + + # Features =================================================================== + + # Test constant features library. + ament_add_gtest_executable( + test_constant_features_with_move_group + features/test_constant_features_with_move_group.cpp) + target_link_libraries( + test_constant_features_with_move_group + moveit_ros_trajectory_cache_utils_lib move_group_fixture) + add_ros_test( + fixtures/gtest_with_move_group.py + TARGET + test_constant_features_with_move_group + TIMEOUT + 30 + ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_constant_features_with_move_group") + + # Test GetCartesianPath::Request features library. + ament_add_gtest_executable( + test_get_cartesian_path_request_features_with_move_group + features/test_get_cartesian_path_request_features_with_move_group.cpp) + target_link_libraries( + test_get_cartesian_path_request_features_with_move_group + moveit_ros_trajectory_cache_features_lib move_group_fixture) + add_ros_test( + fixtures/gtest_with_move_group.py + TARGET + test_get_cartesian_path_request_features_with_move_group + TIMEOUT + 30 + ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_get_cartesian_path_request_features_with_move_group") + + # Test MotionPlanRequest features library. + ament_add_gtest_executable( + test_motion_plan_request_features_with_move_group + features/test_motion_plan_request_features_with_move_group.cpp) + target_link_libraries( + test_motion_plan_request_features_with_move_group + moveit_ros_trajectory_cache_features_lib move_group_fixture) + add_ros_test( + fixtures/gtest_with_move_group.py + TARGET + test_motion_plan_request_features_with_move_group + TIMEOUT + 30 + ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_motion_plan_request_features_with_move_group") + + # Cache Insert Policies ====================================================== + + # Test always_insert_never_prune policies 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( + fixtures/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") + + # Test best_seen_execution_time policies library. + ament_add_gtest_executable( + test_best_seen_execution_time_policy_with_move_group + cache_insert_policies/test_best_seen_execution_time_policy_with_move_group.cpp + ) + target_link_libraries( + test_best_seen_execution_time_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( + fixtures/gtest_with_move_group.py + TARGET + test_best_seen_execution_time_policy_with_move_group + TIMEOUT + 30 + ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_best_seen_execution_time_policy_with_move_group") + + # Integration Tests ========================================================== + + # This test executable is run by the pytest_test, since a node is required for + # testing move groups add_executable(test_trajectory_cache + # test_trajectory_cache.cpp) target_link_libraries(test_trajectory_cache + # moveit_ros_trajectory_cache_lib) + + # install(TARGETS test_trajectory_cache RUNTIME DESTINATION + # lib/${PROJECT_NAME}) + + # ament_add_pytest_test( test_trajectory_cache_py "test_trajectory_cache.py" + # WORKING_DIRECTORY + # "${CMAKE_CURRENT_BINARY_DIR}") + +endif() 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 0000000000..078e3c8f28 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/cache_insert_policies/test_always_insert_never_prune_policy_with_move_group.cpp @@ -0,0 +1,449 @@ +// 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 + +#include +#include +#include + +#include "../fixtures/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_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::AlwaysInsertNeverPrunePolicy; +using ::moveit_ros::trajectory_cache::CartesianAlwaysInsertNeverPrunePolicy; +using ::moveit_ros::trajectory_cache::constructGetCartesianPathRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +// ================================================================================================= +// AlwaysInsertNeverPrunePolicy. +// ================================================================================================= + +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_msg.goal_constraints.emplace_back(); + + 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; + } + + // We can't test workspace ID frame empty. + // But it technically should be unreachable as long as the robot description is correct. + + // No goal. + { + MotionPlanRequest msg = valid_msg; + msg.goal_constraints.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, valid_plan)); + } + + // Empty joint trajectory points. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.points.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Empty joint trajectory names. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.joint_names.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Multi-DOF trajectory plan. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.multi_dof_joint_trajectory.points.emplace_back(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Trajectory frame ID empty. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.header.frame_id = ""; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_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. + MotionPlanRequest msg; + MoveGroupInterface::Plan plan; + MoveItErrorCode ret = MoveItErrorCode::FAILURE; + + 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 || plan.trajectory.joint_trajectory.points.empty()); // 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 || + another_plan.trajectory.joint_trajectory.points + .empty()); // 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, /*metadata_only=*/true); + 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. + std::string prune_reason; + EXPECT_FALSE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, msg_plan_pair.second, + policy_fetch[i], &prune_reason)); + EXPECT_FALSE(prune_reason.empty()); + } + + // Policy is always insert. + std::string insert_reason; + EXPECT_TRUE(policy.shouldInsert(*move_group_, msg_plan_pair.first, msg_plan_pair.second, &insert_reason)); + EXPECT_FALSE(insert_reason.empty()); + + policy.reset(); + } +} + +// ================================================================================================= +// CartesianAlwaysInsertNeverPrunePolicy. +// ================================================================================================= + +TEST_F(MoveGroupFixture, CartesianAlwaysInsertNeverPrunePolicyChecks) +{ + // Setup. + CartesianAlwaysInsertNeverPrunePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + + GetCartesianPath::Request valid_msg; + GetCartesianPath::Response valid_plan; + valid_plan.fraction = -1; + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + valid_msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + valid_plan.fraction = move_group_->computeCartesianPath(valid_msg.waypoints, valid_msg.max_step, + valid_msg.jump_threshold, valid_plan.solution); +#pragma GCC diagnostic pop + } while (valid_plan.fraction <= 0); // Sometimes the plan fails with the random pose. + + // Valid case, as control. + { + MoveItErrorCode ret = policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, valid_plan); + ASSERT_TRUE(ret) << ret.message; + } + + // We can't test workspace ID frame empty. + // But it technically should be unreachable as long as the robot description is correct. + + // No waypoints. + { + GetCartesianPath::Request msg = valid_msg; + msg.waypoints.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, valid_plan)); + } + + // Empty joint trajectory points. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.points.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Empty joint trajectory names. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.joint_names.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Multi-DOF trajectory plan. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.multi_dof_joint_trajectory.points.emplace_back(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Trajectory frame ID empty. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.header.frame_id = ""; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Mismatched frames. + { + GetCartesianPath::Request msg = valid_msg; + GetCartesianPath::Response plan = valid_plan; + msg.header.frame_id = "panda_link0"; + plan.solution.joint_trajectory.header.frame_id = "clearly_a_different_frame"; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } +} + +TEST_F(MoveGroupFixture, CartesianAlwaysInsertNeverPrunePolicyWorks) +{ + CartesianAlwaysInsertNeverPrunePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + ASSERT_EQ(coll.count(), 0); + + // Setup. Get valid entries to insert. + GetCartesianPath::Request msg; + GetCartesianPath::Response plan; + plan.fraction = -1; + + GetCartesianPath::Request another_msg; + GetCartesianPath::Response another_plan; + another_plan.fraction = -1; + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + plan.fraction = move_group_->computeCartesianPath(msg.waypoints, msg.max_step, msg.jump_threshold, plan.solution); +#pragma GCC diagnostic pop + } while (plan.fraction <= -1 && + plan.solution.joint_trajectory.points.size() < 2); // Sometimes the plan fails with the random pose. + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + another_msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + another_plan.fraction = move_group_->computeCartesianPath(another_msg.waypoints, another_msg.max_step, + another_msg.jump_threshold, another_plan.solution); +#pragma GCC diagnostic pop + } while (another_plan.fraction <= -1 && + plan.solution.joint_trajectory.points.size() < 2); // Sometimes the plan fails with the random pose. + + // 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("fraction")); + + // We add two to test the prune predicate, as appropriate. + coll.insert(msg_plan_pair.second.solution, metadata); + coll.insert(msg_plan_pair.second.solution, 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 = + CartesianAlwaysInsertNeverPrunePolicy::getSupportedFeatures(/*start_tolerance=*/0.025, + /*goal_tolerance=*/0.001, + /*min_fraction=*/0.0); + + 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, /*metadata_only=*/true); + 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. + std::string prune_reason; + EXPECT_FALSE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, msg_plan_pair.second, + policy_fetch[i], &prune_reason)); + EXPECT_FALSE(prune_reason.empty()); + } + + // Policy is always insert. + std::string insert_reason; + EXPECT_TRUE(policy.shouldInsert(*move_group_, msg_plan_pair.first, msg_plan_pair.second, &insert_reason)); + EXPECT_FALSE(insert_reason.empty()); + + 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; +} diff --git a/moveit_ros/trajectory_cache/test/cache_insert_policies/test_best_seen_execution_time_policy_with_move_group.cpp b/moveit_ros/trajectory_cache/test/cache_insert_policies/test_best_seen_execution_time_policy_with_move_group.cpp new file mode 100644 index 0000000000..b685981e2d --- /dev/null +++ b/moveit_ros/trajectory_cache/test/cache_insert_policies/test_best_seen_execution_time_policy_with_move_group.cpp @@ -0,0 +1,490 @@ +// 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 + +#include +#include +#include + +#include "../fixtures/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_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::BestSeenExecutionTimePolicy; +using ::moveit_ros::trajectory_cache::CartesianBestSeenExecutionTimePolicy; +using ::moveit_ros::trajectory_cache::constructGetCartesianPathRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +// ================================================================================================= +// BestSeenExecutionTimePolicy. +// ================================================================================================= + +TEST_F(MoveGroupFixture, BestSeenExecutionTimePolicy) +{ + // Setup. + BestSeenExecutionTimePolicy 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_msg.goal_constraints.emplace_back(); + + 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; + } + + // We can't test workspace ID frame empty. + // But it technically should be unreachable as long as the robot description is correct. + + // No goal. + { + MotionPlanRequest msg = valid_msg; + msg.goal_constraints.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, valid_plan)); + } + + // Empty joint trajectory points. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.points.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Empty joint trajectory names. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.joint_names.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Multi-DOF trajectory plan. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.multi_dof_joint_trajectory.points.emplace_back(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Trajectory frame ID empty. + { + MoveGroupInterface::Plan plan = valid_plan; + plan.trajectory.joint_trajectory.header.frame_id = ""; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_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, BestSeenExecutionTimePolicyWorks) +{ + BestSeenExecutionTimePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + ASSERT_EQ(coll.count(), 0); + + // Setup. Get valid entries to insert. + MotionPlanRequest msg; + MoveGroupInterface::Plan plan; + MoveItErrorCode ret = MoveItErrorCode::FAILURE; + + 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 || plan.trajectory.joint_trajectory.points.empty()); // 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 || + another_plan.trajectory.joint_trajectory.points + .empty()); // 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. + + // Set the execution times for the plans to be between the shorter and longer plan execution times + // later. + plan.trajectory.joint_trajectory.points.back().time_from_start.sec = 10; + plan.trajectory.joint_trajectory.points.back().time_from_start.nanosec = 0; + another_plan.trajectory.joint_trajectory.points.back().time_from_start.sec = 10; + another_plan.trajectory.joint_trajectory.points.back().time_from_start.nanosec = 0; + + // 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 = + BestSeenExecutionTimePolicy::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, /*metadata_only=*/true); + 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]); + + // This guarantees that the longer plan will have a larger time_from_start than the shorter plan. + auto longer_plan = msg_plan_pair.second; + longer_plan.trajectory.joint_trajectory.points.back().time_from_start.sec = 100; + longer_plan.trajectory.joint_trajectory.points.back().time_from_start.nanosec = 0; + + auto shorter_plan = msg_plan_pair.second; + shorter_plan.trajectory.joint_trajectory.points.back().time_from_start.sec = 0; + shorter_plan.trajectory.joint_trajectory.points.back().time_from_start.nanosec = 100; + + // Should prune matched plan if execution time is longer than candidate. + EXPECT_FALSE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, longer_plan, policy_fetch[i])); + EXPECT_TRUE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, shorter_plan, policy_fetch[i])); + + // Should insert candidate plan if execution time is best seen. + EXPECT_FALSE(policy.shouldInsert(*move_group_, msg_plan_pair.first, longer_plan)); + EXPECT_TRUE(policy.shouldInsert(*move_group_, msg_plan_pair.first, shorter_plan)); + } + + policy.reset(); + } +} + +// ================================================================================================= +// CartesianBestSeenExecutionTimePolicy. +// ================================================================================================= + +TEST_F(MoveGroupFixture, CartesianBestSeenExecutionTimePolicyChecks) +{ + // Setup. + CartesianBestSeenExecutionTimePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + + GetCartesianPath::Request valid_msg; + GetCartesianPath::Response valid_plan; + valid_plan.fraction = -1; + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + valid_msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + valid_plan.fraction = move_group_->computeCartesianPath(valid_msg.waypoints, valid_msg.max_step, + valid_msg.jump_threshold, valid_plan.solution); +#pragma GCC diagnostic pop + } while (valid_plan.fraction <= 0 && + valid_plan.solution.joint_trajectory.points.size() < 2); // Sometimes the plan fails with the random pose. + + // Valid case, as control. + { + MoveItErrorCode ret = policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, valid_plan); + ASSERT_TRUE(ret) << ret.message; + } + + // We can't test workspace ID frame empty. + // But it technically should be unreachable as long as the robot description is correct. + + // No waypoints. + { + GetCartesianPath::Request msg = valid_msg; + msg.waypoints.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, valid_plan)); + } + + // Empty joint trajectory points. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.points.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Empty joint trajectory names. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.joint_names.clear(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Multi-DOF trajectory plan. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.multi_dof_joint_trajectory.points.emplace_back(); + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Trajectory frame ID empty. + { + GetCartesianPath::Response plan = valid_plan; + plan.solution.joint_trajectory.header.frame_id = ""; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, valid_msg, plan)); + } + + // Mismatched frames. + { + GetCartesianPath::Request msg = valid_msg; + GetCartesianPath::Response plan = valid_plan; + msg.header.frame_id = "panda_link0"; + plan.solution.joint_trajectory.header.frame_id = "clearly_a_different_frame"; + EXPECT_FALSE(policy.checkCacheInsertInputs(*move_group_, coll, msg, plan)); + } +} + +TEST_F(MoveGroupFixture, CartesianBestSeenExecutionTimePolicyWorks) +{ + CartesianBestSeenExecutionTimePolicy policy; + + MessageCollection coll = db_->openCollection("test_db", policy.getName()); + ASSERT_EQ(coll.count(), 0); + + // Setup. Get valid entries to insert. + GetCartesianPath::Request msg; + GetCartesianPath::Response plan; + plan.fraction = -1; + + GetCartesianPath::Request another_msg; + GetCartesianPath::Response another_plan; + another_plan.fraction = -1; + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + plan.fraction = move_group_->computeCartesianPath(msg.waypoints, msg.max_step, msg.jump_threshold, plan.solution); +#pragma GCC diagnostic pop + } while (plan.fraction <= 0 && + plan.solution.joint_trajectory.points.size() < 2); // Sometimes the plan fails with the random pose. + + do + { + std::vector waypoints; + waypoints.push_back(move_group_->getCurrentPose().pose); + waypoints.push_back(move_group_->getRandomPose().pose); + + another_msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/0.01, /*jump_threshold=*/0.0); +// TODO: Swap this over to the new computeCartesianPath API. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + another_plan.fraction = move_group_->computeCartesianPath(another_msg.waypoints, another_msg.max_step, + another_msg.jump_threshold, another_plan.solution); +#pragma GCC diagnostic pop + } while (another_plan.fraction <= 0 && + another_plan.solution.joint_trajectory.points.size() < 2); // Sometimes the plan fails with the random pose. + + // 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. + + // Set the execution times for the plans to be between the shorter and longer plan execution times + // later. + plan.solution.joint_trajectory.points.back().time_from_start.sec = 10; + plan.solution.joint_trajectory.points.back().time_from_start.nanosec = 0; + another_plan.solution.joint_trajectory.points.back().time_from_start.sec = 10; + another_plan.solution.joint_trajectory.points.back().time_from_start.nanosec = 0; + + // 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("fraction")); + + // We add two to test the prune predicate, as appropriate. + coll.insert(msg_plan_pair.second.solution, metadata); + coll.insert(msg_plan_pair.second.solution, 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 = + CartesianBestSeenExecutionTimePolicy::getSupportedFeatures(/*start_tolerance=*/0.025, + /*goal_tolerance=*/0.001, + /*min_fraction=*/0.0); + + 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, /*metadata_only=*/true); + 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]); + + // This guarantees that the longer plan will have a larger time_from_start than the shorter plan. + auto longer_plan = msg_plan_pair.second; + longer_plan.solution.joint_trajectory.points.back().time_from_start.sec = 100; + longer_plan.solution.joint_trajectory.points.back().time_from_start.nanosec = 0; + + auto shorter_plan = msg_plan_pair.second; + shorter_plan.solution.joint_trajectory.points.back().time_from_start.sec = 0; + shorter_plan.solution.joint_trajectory.points.back().time_from_start.nanosec = 100; + + // Should prune matched plan if execution time is longer than candidate. + std::string longer_prune_reason; + std::string shorter_prune_reason; + + EXPECT_FALSE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, longer_plan, policy_fetch[i], + &longer_prune_reason)); + EXPECT_TRUE(policy.shouldPruneMatchingEntry(*move_group_, msg_plan_pair.first, shorter_plan, policy_fetch[i], + &shorter_prune_reason)); + + EXPECT_FALSE(longer_prune_reason.empty()); + EXPECT_FALSE(shorter_prune_reason.empty()); + + // Should insert candidate plan if execution time is best seen. + std::string longer_insert_reason; + std::string shorter_insert_reason; + + EXPECT_FALSE(policy.shouldInsert(*move_group_, msg_plan_pair.first, longer_plan, &longer_insert_reason)); + EXPECT_TRUE(policy.shouldInsert(*move_group_, msg_plan_pair.first, shorter_plan, &shorter_insert_reason)); + + EXPECT_FALSE(longer_insert_reason.empty()); + EXPECT_FALSE(shorter_insert_reason.empty()); + } + + 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; +} diff --git a/moveit_ros/trajectory_cache/test/features/test_constant_features_with_move_group.cpp b/moveit_ros/trajectory_cache/test/features/test_constant_features_with_move_group.cpp new file mode 100644 index 0000000000..bff4b0420f --- /dev/null +++ b/moveit_ros/trajectory_cache/test/features/test_constant_features_with_move_group.cpp @@ -0,0 +1,402 @@ +// 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 "../fixtures/move_group_fixture.hpp" + +namespace +{ + +using ::geometry_msgs::msg::Point; + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit_ros::trajectory_cache::QueryOnlyEqFeature; +using ::moveit_ros::trajectory_cache::QueryOnlyGTEFeature; +using ::moveit_ros::trajectory_cache::QueryOnlyLTEFeature; +using ::moveit_ros::trajectory_cache::QueryOnlyRangeInclusiveWithToleranceFeature; + +using ::moveit_ros::trajectory_cache::MetadataOnlyFeature; + +TEST_F(MoveGroupFixture, MetadataOnlyFeature) +{ + MessageCollection coll = db_->openCollection("test_db", "test_collection"); + + Query::Ptr query = coll.createQuery(); + Metadata::Ptr metadata = coll.createMetadata(); + ASSERT_EQ(metadata->lookupFieldNames().size(), 0); + + Point msg; + + /// MetadataOnlyFeature. + + MetadataOnlyFeature string_metadata_feature("string_metadata", "test_string"); + MetadataOnlyFeature double_metadata_feature("double_metadata", 1.0); + MetadataOnlyFeature int_metadata_feature("int_metadata", 2); + MetadataOnlyFeature bool_metadata_feature("bool_metadata", true); + + // Names. + EXPECT_EQ(string_metadata_feature.getName(), "MetadataOnlyFeature.string_metadata"); + EXPECT_EQ(double_metadata_feature.getName(), "MetadataOnlyFeature.double_metadata"); + EXPECT_EQ(int_metadata_feature.getName(), "MetadataOnlyFeature.int_metadata"); + EXPECT_EQ(bool_metadata_feature.getName(), "MetadataOnlyFeature.bool_metadata"); + + // Expect no-ops. + EXPECT_EQ(string_metadata_feature.appendFeaturesAsFuzzyFetchQuery(*query, msg, *move_group_, 0.0), + moveit::core::MoveItErrorCode::SUCCESS); + EXPECT_EQ(string_metadata_feature.appendFeaturesAsExactFetchQuery(*query, msg, *move_group_, 0.0), + moveit::core::MoveItErrorCode::SUCCESS); + + // Fetch ok. + string_metadata_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_EQ(metadata->lookupFieldNames().size(), 1); + EXPECT_TRUE(metadata->lookupField("string_metadata")); + EXPECT_EQ(metadata->lookupString("string_metadata"), "test_string"); + + double_metadata_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_EQ(metadata->lookupFieldNames().size(), 2); + EXPECT_TRUE(metadata->lookupField("double_metadata")); + EXPECT_EQ(metadata->lookupDouble("double_metadata"), 1.0); + + int_metadata_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_EQ(metadata->lookupFieldNames().size(), 3); + EXPECT_TRUE(metadata->lookupField("int_metadata")); + EXPECT_EQ(metadata->lookupInt("int_metadata"), 2); + + bool_metadata_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_EQ(metadata->lookupFieldNames().size(), 4); + EXPECT_TRUE(metadata->lookupField("bool_metadata")); + EXPECT_TRUE(metadata->lookupBool("bool_metadata")); +} + +TEST_F(MoveGroupFixture, QueryOnlyEqFeature) +{ + MessageCollection coll = db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", "test_metadata"); + + Point msg; + coll.insert(msg, metadata); + + QueryOnlyEqFeature eq_feature("test_metadata", "test_metadata"); + QueryOnlyEqFeature unrelated_eq_feature("unrelated", "test_metadata"); + QueryOnlyEqFeature mismatched_eq_feature("test_metadata", "mismatched"); + + // Names. + EXPECT_EQ(eq_feature.getName(), "QueryOnlyEqFeature.test_metadata"); + EXPECT_EQ(unrelated_eq_feature.getName(), "QueryOnlyEqFeature.unrelated"); + + // Expect no-ops. + { + Metadata::Ptr noop_metadata = coll.createMetadata(); + eq_feature.appendFeaturesAsInsertMetadata(*noop_metadata, msg, *move_group_); + EXPECT_TRUE(noop_metadata->lookupFieldNames().empty()); + } + + // Match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + eq_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + eq_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + // Unrelated. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + unrelated_eq_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + unrelated_eq_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } + + // Mismatched. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + mismatched_eq_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + mismatched_eq_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } +} + +TEST_F(MoveGroupFixture, QueryOnlyGTEFeature) +{ + MessageCollection coll = db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", 5.0); + + Point msg; + coll.insert(msg, metadata); + + metadata = coll.createMetadata(); + metadata->append("unrelated", 5.0); + coll.insert(msg, metadata); + + QueryOnlyGTEFeature gte_feature("test_metadata", 4.0); + QueryOnlyGTEFeature gte_eq_feature("test_metadata", 5.0); + QueryOnlyGTEFeature unrelated_gte_feature("unrelated", 6.0); + QueryOnlyGTEFeature mismatched_gte_feature("test_metadata", 6.0); + + // Names. + EXPECT_EQ(gte_feature.getName(), "QueryOnlyGTEFeature.test_metadata"); + EXPECT_EQ(unrelated_gte_feature.getName(), "QueryOnlyGTEFeature.unrelated"); + + // Expect no-ops. + { + Metadata::Ptr metadata = coll.createMetadata(); + gte_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_TRUE(metadata->lookupFieldNames().empty()); + } + + // Match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + gte_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + gte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + { + Query::Ptr fuzzy_query = coll.createQuery(); + gte_eq_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + gte_eq_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + // Unrelated. No match. + { + Query::Ptr unrelated_fuzzy_query = coll.createQuery(); + unrelated_gte_feature.appendFeaturesAsFuzzyFetchQuery(*unrelated_fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(unrelated_fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + unrelated_gte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } + + // Mismatched. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + mismatched_gte_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + mismatched_gte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } +} + +TEST_F(MoveGroupFixture, QueryOnlyLTEFeature) +{ + MessageCollection coll = db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", 5.0); + + Point msg; + coll.insert(msg, metadata); + + QueryOnlyLTEFeature lte_feature("test_metadata", 6.0); + QueryOnlyLTEFeature lte_eq_feature("test_metadata", 5.0); + QueryOnlyLTEFeature unrelated_lte_feature("unrelated", 6.0); + QueryOnlyLTEFeature mismatched_lte_feature("test_metadata", 4.0); + + // Names. + EXPECT_EQ(lte_feature.getName(), "QueryOnlyLTEFeature.test_metadata"); + EXPECT_EQ(unrelated_lte_feature.getName(), "QueryOnlyLTEFeature.unrelated"); + + // Expect no-ops. + { + Metadata::Ptr metadata = coll.createMetadata(); + lte_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_TRUE(metadata->lookupFieldNames().empty()); + } + + // Match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + lte_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + lte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + { + Query::Ptr fuzzy_query = coll.createQuery(); + lte_eq_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + lte_eq_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + // Unrelated. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + unrelated_lte_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + unrelated_lte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } + + // Mismatched. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + mismatched_lte_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + mismatched_lte_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } +} + +TEST_F(MoveGroupFixture, QueryOnlyRangeInclusiveWithToleranceFeature) +{ + MessageCollection coll = db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", 5.0); + + Point msg; + coll.insert(msg, metadata); + + QueryOnlyRangeInclusiveWithToleranceFeature exact_range_feature("test_metadata", 5.0, 5.0); + QueryOnlyRangeInclusiveWithToleranceFeature lower_range_feature("test_metadata", 4.0, 5.0); + QueryOnlyRangeInclusiveWithToleranceFeature upper_range_feature("test_metadata", 5.0, 6.0); + QueryOnlyRangeInclusiveWithToleranceFeature over_range_feature("test_metadata", 4.5, 5.5); + + QueryOnlyRangeInclusiveWithToleranceFeature unrelated_range_feature("unrelated", 5.5, 6.0); + QueryOnlyRangeInclusiveWithToleranceFeature mismatched_range_feature("test_metadata", 5.5, 6.0); + + // Names. + EXPECT_EQ(exact_range_feature.getName(), "QueryOnlyRangeInclusiveWithToleranceFeature.test_metadata"); + EXPECT_EQ(unrelated_range_feature.getName(), "QueryOnlyRangeInclusiveWithToleranceFeature.unrelated"); + + // Expect no-ops. + { + Metadata::Ptr metadata = coll.createMetadata(); + exact_range_feature.appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_); + EXPECT_TRUE(metadata->lookupFieldNames().empty()); + } + + // Match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + exact_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + exact_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + { + Query::Ptr fuzzy_query = coll.createQuery(); + lower_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + lower_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + { + Query::Ptr fuzzy_query = coll.createQuery(); + upper_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + upper_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + { + Query::Ptr fuzzy_query = coll.createQuery(); + over_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + + Query::Ptr exact_query = coll.createQuery(); + over_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } + + // Unrelated. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + unrelated_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + unrelated_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } + + // Mismatched. No match. + { + Query::Ptr fuzzy_query = coll.createQuery(); + mismatched_range_feature.appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(fuzzy_query).empty()); + + Query::Ptr exact_query = coll.createQuery(); + mismatched_range_feature.appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0); + EXPECT_TRUE(coll.queryList(exact_query).empty()); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp b/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp new file mode 100644 index 0000000000..67a3c9bbc4 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/features/test_get_cartesian_path_request_features_with_move_group.cpp @@ -0,0 +1,118 @@ +// 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 + * + * Sanity tests for the moveit_msgs::srv::GetCartesianPath::Request feature extractors. + * + * @see get_cartesian_path_request_features.hpp + * + * WARNING: + * These tests currently do not cover the implementation details, they are just the first sanity check for + * ensuring the most basic roundtrip functionality works. + * + * For example, some features might not have any resulting changes to the metadata or query due to + * the nature of what is contained in the MotionPlanRequest passed to them. + */ + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include "../fixtures/move_group_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit_msgs::srv::GetCartesianPath; + +using ::moveit_ros::trajectory_cache::constructGetCartesianPathRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +using ::moveit_ros::trajectory_cache::CartesianMaxSpeedAndAccelerationFeatures; +using ::moveit_ros::trajectory_cache::CartesianMaxStepAndJumpThresholdFeatures; +using ::moveit_ros::trajectory_cache::CartesianPathConstraintsFeatures; +using ::moveit_ros::trajectory_cache::CartesianStartStateJointStateFeatures; +using ::moveit_ros::trajectory_cache::CartesianWaypointsFeatures; +using ::moveit_ros::trajectory_cache::CartesianWorkspaceFeatures; + +TEST_F(MoveGroupFixture, GetCartesianPathRequestRoundTrip) +{ + // Test cases. + double match_tolerance = 0.001; + + std::vector>> features_under_test; + + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + + // Setup. + std::vector waypoints = { move_group_->getRandomPose().pose, + move_group_->getRandomPose().pose }; + + GetCartesianPath::Request msg = constructGetCartesianPathRequest(*move_group_, waypoints, /*max_step=*/1.0, + /*jump_threshold=*/0.0, /*avoid_collisions=*/false); + + // Core test. + for (auto& feature : features_under_test) + { + MessageCollection coll = + db_->openCollection("test_db", feature->getName()); + + SCOPED_TRACE(feature->getName()); + + Query::Ptr fuzzy_query = coll.createQuery(); + Query::Ptr exact_query = coll.createQuery(); + Metadata::Ptr metadata = coll.createMetadata(); + + EXPECT_TRUE(feature->appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_)); + coll.insert(msg, metadata); + + EXPECT_TRUE( + feature->appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, /*exact_match_precision=*/0.0001)); + EXPECT_TRUE( + feature->appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, /*exact_match_precision=*/0.0001)); + + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp b/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp new file mode 100644 index 0000000000..71251e6bb7 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp @@ -0,0 +1,112 @@ +// 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 + * + * Sanity tests for the moveit_msgs::msg::MotionPlanRequest feature extractors. + * + * @see motion_plan_request_features.hpp + * + * WARNING: + * These tests currently do not cover the implementation details, they are just the first sanity check for + * ensuring the most basic roundtrip functionality works. + * + * For example, some features might not have any resulting changes to the metadata or query due to + * the nature of what is contained in the MotionPlanRequest passed to them. + */ + +#include + +#include +#include + +#include +#include + +#include +#include + +#include "../fixtures/move_group_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit_msgs::msg::MotionPlanRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +using ::moveit_ros::trajectory_cache::GoalConstraintsFeatures; +using ::moveit_ros::trajectory_cache::MaxSpeedAndAccelerationFeatures; +using ::moveit_ros::trajectory_cache::PathConstraintsFeatures; +using ::moveit_ros::trajectory_cache::StartStateJointStateFeatures; +using ::moveit_ros::trajectory_cache::TrajectoryConstraintsFeatures; +using ::moveit_ros::trajectory_cache::WorkspaceFeatures; + +TEST_F(MoveGroupFixture, MotionPlanRequestRoundTrip) +{ + // Test cases. + double match_tolerance = 0.001; + + std::vector>> features_under_test; + + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + + // Setup. + ASSERT_TRUE(move_group_->setPoseTarget(move_group_->getRandomPose())); + MotionPlanRequest msg; + move_group_->constructMotionPlanRequest(msg); + + // Core test. + for (auto& feature : features_under_test) + { + MessageCollection coll = db_->openCollection("test_db", feature->getName()); + + SCOPED_TRACE(feature->getName()); + + Query::Ptr fuzzy_query = coll.createQuery(); + Query::Ptr exact_query = coll.createQuery(); + Metadata::Ptr metadata = coll.createMetadata(); + + EXPECT_TRUE(feature->appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_)); + coll.insert(msg, metadata); + + EXPECT_TRUE( + feature->appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, /*exact_match_precision=*/0.0001)); + EXPECT_TRUE( + feature->appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, /*exact_match_precision=*/0.0001)); + + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/trajectory_cache/test/fixtures/gtest_with_move_group.py b/moveit_ros/trajectory_cache/test/fixtures/gtest_with_move_group.py new file mode 100644 index 0000000000..30e60803db --- /dev/null +++ b/moveit_ros/trajectory_cache/test/fixtures/gtest_with_move_group.py @@ -0,0 +1,122 @@ +import os +import unittest +import launch_testing + +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_testing.actions import ReadyToTest + +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() + ) + + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="log", + parameters=[moveit_config.to_dict()], + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="log", + ) + ] + + gtest_node = Node( + executable=PathJoinSubstitution( + [ + LaunchConfiguration("test_binary_dir"), + LaunchConfiguration("test_executable"), + ] + ), + parameters=[moveit_config.to_dict()], + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + static_tf, + robot_state_publisher, + ros2_control_node, + *load_controllers, + TimerAction(period=1.0, actions=[run_move_group_node]), + TimerAction(period=3.0, actions=[gtest_node]), + ReadyToTest(), + ] + ), { + "gtest_node": gtest_node, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, gtest_node): + self.proc_info.assertWaitForShutdown(gtest_node, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, gtest_node): + launch_testing.asserts.assertExitCodes( + proc_info, + process=gtest_node, + # Allow -11 since ros2_control or warehouse_ros sometimes segfaults + # for unrelated reasons. + allowable_exit_codes=[0, -11], + ) diff --git a/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp new file mode 100644 index 0000000000..f9a6882d1b --- /dev/null +++ b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp @@ -0,0 +1,67 @@ +// 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 "move_group_fixture.hpp" + +MoveGroupFixture::MoveGroupFixture() + : node_(rclcpp::Node::make_shared("move_group_fixture")), move_group_name_("panda_arm") +{ + node_->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + move_group_ = std::make_shared(node_, move_group_name_); +} + +MoveGroupFixture::~MoveGroupFixture() +{ + is_spinning_ = false; + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } +} + +void MoveGroupFixture::SetUp() +{ + is_spinning_ = true; + spin_thread_ = std::thread(&MoveGroupFixture::spinNode, this); + + db_ = moveit_warehouse::loadDatabase(node_); + db_->setParams(":memory:", 1); + ASSERT_TRUE(db_->connect()); +} + +void MoveGroupFixture::TearDown() +{ + db_.reset(); + is_spinning_ = false; + spin_thread_.join(); +} + +void MoveGroupFixture::spinNode() +{ + while (is_spinning_ && rclcpp::ok()) + { + rclcpp::spin_some(node_); + } +} diff --git a/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.hpp b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.hpp new file mode 100644 index 0000000000..0d4f9ad369 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.hpp @@ -0,0 +1,50 @@ +// 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 + */ + +#pragma once + +#include + +#include +#include +#include +#include + +/** @class MoveGroupFixture + * @brief Test fixture to spin up a node to start a move group with. */ +class MoveGroupFixture : public ::testing::Test +{ +public: + MoveGroupFixture(); + ~MoveGroupFixture() override; + +protected: + void SetUp() override; + void TearDown() override; + + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + std::shared_ptr move_group_; + std::string move_group_name_; + +private: + void spinNode(); + + std::thread spin_thread_; + std::atomic is_spinning_; +}; diff --git a/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp new file mode 100644 index 0000000000..3d362b0106 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp @@ -0,0 +1,64 @@ +// 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 "warehouse_fixture.hpp" + +WarehouseFixture::WarehouseFixture() : node_(rclcpp::Node::make_shared("warehouse_fixture")) +{ + node_->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); +} + +WarehouseFixture::~WarehouseFixture() +{ + is_spinning_ = false; + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } +} + +void WarehouseFixture::SetUp() +{ + is_spinning_ = true; + spin_thread_ = std::thread(&WarehouseFixture::spinNode, this); + + db_ = moveit_warehouse::loadDatabase(node_); + db_->setParams(":memory:", 1); + ASSERT_TRUE(db_->connect()); +} + +void WarehouseFixture::TearDown() +{ + db_.reset(); + is_spinning_ = false; + spin_thread_.join(); +} + +void WarehouseFixture::spinNode() +{ + while (is_spinning_ && rclcpp::ok()) + { + rclcpp::spin_some(node_); + } +} diff --git a/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.hpp b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.hpp new file mode 100644 index 0000000000..b1ca848456 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.hpp @@ -0,0 +1,45 @@ +// 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 + */ + +#pragma once + +#include +#include +#include + +/** @class WarehouseFixture + * @brief Test fixture to spin up a node to start a warehouse_ros connection with. */ +class WarehouseFixture : public ::testing::Test +{ +public: + WarehouseFixture(); + ~WarehouseFixture() override; + +protected: + void SetUp() override; + void TearDown() override; + + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + +private: + void spinNode(); + + std::thread spin_thread_; + std::atomic is_spinning_; +}; diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 425aa9cd76..69131bc52a 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -21,11 +21,13 @@ #include #include #include +#include #include #include using moveit::planning_interface::MoveGroupInterface; +using moveit_ros::trajectory_cache::constructGetCartesianPathRequest; using moveit_ros::trajectory_cache::TrajectoryCache; const std::string ROBOT_NAME = "panda"; @@ -103,11 +105,11 @@ void testGettersAndSetters(const std::shared_ptr& cache) cache->setExactMatchPrecision(1); checkAndEmit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet"); - checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case, - "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse"); - cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(1); - checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case, - "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet"); + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenPruningWorse() == 10, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenPruningWorse"); + cache->setNumAdditionalTrajectoriesToPreserveWhenPruningWorse(1); + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenPruningWorse() == 1, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenPruningWorseAfterSet"); } void testMotionTrajectories(const std::shared_ptr& move_group, @@ -585,7 +587,7 @@ void testCartesianTrajectories(const std::shared_ptr& move_g int test_jump = 2; auto test_waypoints = getDummyWaypoints(); auto cartesian_plan_req_under_test = - cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); + constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); checkAndEmit(cartesian_plan_req_under_test.waypoints == test_waypoints && static_cast(cartesian_plan_req_under_test.max_step) == test_step && @@ -600,7 +602,7 @@ void testCartesianTrajectories(const std::shared_ptr& move_g // Plain start auto waypoints = getDummyWaypoints(); - auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); + auto cartesian_plan_req = constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear(); @@ -1065,7 +1067,7 @@ int main(int argc, char** argv) options.db_path = ":memory:"; options.db_port = 0; options.exact_match_precision = 10; - options.num_additional_trajectories_to_preserve_when_deleting_worse = 10; + options.num_additional_trajectories_to_preserve_when_pruning_worse = 10; // Tests. @@ -1074,7 +1076,7 @@ int main(int argc, char** argv) testGettersAndSetters(cache); cache->setExactMatchPrecision(1e-4); - cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(0); + cache->setNumAdditionalTrajectoriesToPreserveWhenPruningWorse(0); testMotionTrajectories(move_group, cache); testCartesianTrajectories(move_group, cache); diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index 05cfcabd25..b67112b5b5 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -139,7 +139,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): launch_context, trajectory_cache_test_runner_node, lambda x: x.count("[PASS]") == 169, # All test cases passed. - timeout=30, + timeout=60, ) # Check no occurrences of [FAIL] in output @@ -147,7 +147,7 @@ def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): launch_context, trajectory_cache_test_runner_node, lambda x: "[FAIL]" in x, - timeout=10, + timeout=60, ) yield diff --git a/moveit_ros/trajectory_cache/test/utils/test_utils.cpp b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp new file mode 100644 index 0000000000..835b1de2af --- /dev/null +++ b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp @@ -0,0 +1,306 @@ +// 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 + +#include "../fixtures/warehouse_fixture.hpp" + +namespace +{ + +using ::testing::TestParamInfo; +using ::testing::ValuesIn; + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +// This test throws an exception on Humble. It is not clear why. Excluding it for now. +#if RCLCPP_VERSION_GTE(28, 3, 3) +TEST_F(WarehouseFixture, QueryAppendCenterWithToleranceWorks) +{ + MessageCollection coll = + db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", 5.0); + coll.insert(geometry_msgs::msg::Point(), metadata); + + Query::Ptr unrelated_query = coll.createQuery(); + moveit_ros::trajectory_cache::queryAppendCenterWithTolerance(*unrelated_query, "unrelated_metadata", 1.0, 10.0); + EXPECT_TRUE(coll.queryList(unrelated_query).empty()); + + Query::Ptr related_query_too_low = coll.createQuery(); + moveit_ros::trajectory_cache::queryAppendCenterWithTolerance(*related_query_too_low, "test_metadata", 4.45, 1.0); + EXPECT_TRUE(coll.queryList(related_query_too_low).empty()); + + Query::Ptr related_query_too_high = coll.createQuery(); + moveit_ros::trajectory_cache::queryAppendCenterWithTolerance(*related_query_too_high, "test_metadata", 5.55, 1.0); + EXPECT_TRUE(coll.queryList(related_query_too_high).empty()); + + Query::Ptr related_query_in_range = coll.createQuery(); + moveit_ros::trajectory_cache::queryAppendCenterWithTolerance(*related_query_in_range, "test_metadata", 5.0, 1.0); + EXPECT_EQ(coll.queryList(related_query_in_range).size(), 1); +} +#endif + +// restateInNewFrame. + +TEST(RestateInNewFrameTest, NoopOnNullTranslationAndRotation) +{ + auto tf_buffer = std::make_shared(std::make_shared(RCL_ROS_TIME)); + + moveit::core::MoveItErrorCode result = moveit_ros::trajectory_cache::restateInNewFrame( + tf_buffer, "frame_a", "frame_b", nullptr, nullptr, tf2::TimePointZero); + + EXPECT_EQ(result, moveit::core::MoveItErrorCode::SUCCESS); +} + +TEST(RestateInNewFrameTest, NoopOnSameFrame) +{ + auto tf_buffer = std::make_shared(std::make_shared(RCL_ROS_TIME)); + geometry_msgs::msg::Point translation; + translation.x = 1; + translation.y = 2; + translation.z = 3; + geometry_msgs::msg::Quaternion rotation; + rotation.x = 0; + rotation.y = 0; + rotation.z = 0; + rotation.w = 1; + + moveit::core::MoveItErrorCode result = moveit_ros::trajectory_cache::restateInNewFrame( + tf_buffer, "frame_a", "frame_a", &translation, &rotation, tf2::TimePointZero); + + EXPECT_EQ(result, moveit::core::MoveItErrorCode::SUCCESS); + EXPECT_EQ(translation.x, 1); + EXPECT_EQ(translation.y, 2); + EXPECT_EQ(translation.z, 3); + EXPECT_EQ(rotation.x, 0); + EXPECT_EQ(rotation.y, 0); + EXPECT_EQ(rotation.z, 0); + EXPECT_EQ(rotation.w, 1); +} + +TEST(RestateInNewFrameTest, FailsOnMissingTransform) +{ + auto tf_buffer = std::make_shared(std::make_shared(RCL_ROS_TIME)); + geometry_msgs::msg::Point translation; + geometry_msgs::msg::Quaternion rotation; + + moveit::core::MoveItErrorCode result = moveit_ros::trajectory_cache::restateInNewFrame( + tf_buffer, "frame_a", "frame_c", &translation, &rotation, tf2::TimePointZero); + + EXPECT_NE(result, moveit::core::MoveItErrorCode::SUCCESS); +} + +struct RestateInNewFrameTestCase +{ + std::string test_name; + bool translate; + bool rotate; + double expected_translation_x; + double expected_translation_y; + double expected_translation_z; + double expected_rotation_x; + double expected_rotation_y; + double expected_rotation_z; + double expected_rotation_w; +}; + +class RestateInNewFrameParameterizedTest : public testing::WithParamInterface, + public testing::Test +{ +}; + +INSTANTIATE_TEST_SUITE_P(RestateInNewFrameParameterizedTests, RestateInNewFrameParameterizedTest, + ValuesIn({ { + .test_name = "TranslateAndRotate", + .translate = true, + .rotate = true, + .expected_translation_x = 2, + .expected_translation_y = 4, + .expected_translation_z = 6, + .expected_rotation_x = 0, + .expected_rotation_y = 0, + .expected_rotation_z = 0.707, + .expected_rotation_w = 0.707, + }, + { + .test_name = "NullTranslationIgnoresTranslation", + .translate = false, + .rotate = true, + .expected_translation_x = 1, + .expected_translation_y = 2, + .expected_translation_z = 3, + .expected_rotation_x = 0, + .expected_rotation_y = 0, + .expected_rotation_z = 0.707, + .expected_rotation_w = 0.707, + }, + { + .test_name = "NullRotationIgnoresRotation", + .translate = true, + .rotate = false, + .expected_translation_x = 2, + .expected_translation_y = 4, + .expected_translation_z = 6, + .expected_rotation_x = 0, + .expected_rotation_y = 0, + .expected_rotation_z = 0, + .expected_rotation_w = 1, + } }), + [](const TestParamInfo& info) { + return info.param.test_name; + }); + +TEST_P(RestateInNewFrameParameterizedTest, RestateInNewFrame) +{ + auto tf_buffer = std::make_shared(std::make_shared(RCL_ROS_TIME)); + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "frame_a"; + transform.child_frame_id = "frame_b"; + transform.transform.translation.x = 1; + transform.transform.translation.y = 2; + transform.transform.translation.z = 3; + transform.transform.rotation.x = 0; + transform.transform.rotation.y = 0; + transform.transform.rotation.z = 0.707; // 90 degree rotation about the z-axis. + transform.transform.rotation.w = 0.707; + tf_buffer->setTransform(transform, "test"); + + geometry_msgs::msg::Point translation; + translation.x = 1; + translation.y = 2; + translation.z = 3; + + geometry_msgs::msg::Quaternion rotation; + rotation.x = 0; + rotation.y = 0; + rotation.z = 0; + rotation.w = 1; + + RestateInNewFrameTestCase params = GetParam(); + + moveit::core::MoveItErrorCode result = + moveit_ros::trajectory_cache::restateInNewFrame(tf_buffer, /*target_frame=*/"frame_a", /*source_frame=*/"frame_b", + params.translate ? &translation : nullptr, + params.rotate ? &rotation : nullptr, tf2::TimePointZero); + + EXPECT_EQ(result, moveit::core::MoveItErrorCode::SUCCESS); + + if (params.translate) + { + EXPECT_EQ(translation.x, params.expected_translation_x); + EXPECT_EQ(translation.y, params.expected_translation_y); + EXPECT_EQ(translation.z, params.expected_translation_z); + } + else + { + EXPECT_EQ(translation.x, 1); + EXPECT_EQ(translation.y, 2); + EXPECT_EQ(translation.z, 3); + } + + if (params.rotate) + { + EXPECT_NEAR(rotation.x, params.expected_rotation_x, 1e-3); + EXPECT_NEAR(rotation.y, params.expected_rotation_y, 1e-3); + EXPECT_NEAR(rotation.z, params.expected_rotation_z, 1e-3); + EXPECT_NEAR(rotation.w, params.expected_rotation_w, 1e-3); + } + else + { + EXPECT_EQ(rotation.x, 0); + EXPECT_EQ(rotation.y, 0); + EXPECT_EQ(rotation.z, 0); + EXPECT_EQ(rotation.w, 1); + } +} + +// Other utils. + +TEST(TestUtils, GetExecutionTimeWorks) +{ + moveit_msgs::msg::RobotTrajectory trajectory; + trajectory.joint_trajectory.points.resize(2); + trajectory.joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(1.0); + trajectory.joint_trajectory.points[1].time_from_start = rclcpp::Duration::from_seconds(2.0); + + EXPECT_EQ(moveit_ros::trajectory_cache::getExecutionTime(trajectory), 2.0); +} + +TEST(TestUtils, ConstraintSortingWorks) +{ + // Joint constraints. + { + moveit_msgs::msg::JointConstraint jc1; + jc1.joint_name = "joint2"; + moveit_msgs::msg::JointConstraint jc2; + jc2.joint_name = "joint1"; + std::vector joint_constraints = { jc1, jc2 }; + moveit_ros::trajectory_cache::sortJointConstraints(joint_constraints); + + EXPECT_EQ(joint_constraints[0].joint_name, "joint1"); + EXPECT_EQ(joint_constraints[1].joint_name, "joint2"); + } + + // Position constraints. + { + moveit_msgs::msg::PositionConstraint pc1; + pc1.link_name = "link2"; + moveit_msgs::msg::PositionConstraint pc2; + pc2.link_name = "link1"; + std::vector position_constraints = { pc1, pc2 }; + moveit_ros::trajectory_cache::sortPositionConstraints(position_constraints); + EXPECT_EQ(position_constraints[0].link_name, "link1"); + EXPECT_EQ(position_constraints[1].link_name, "link2"); + } + + // Orientation constraints. + { + moveit_msgs::msg::OrientationConstraint oc1; + oc1.link_name = "link2"; + moveit_msgs::msg::OrientationConstraint oc2; + oc2.link_name = "link1"; + std::vector orientation_constraints = { oc1, oc2 }; + moveit_ros::trajectory_cache::sortOrientationConstraints(orientation_constraints); + EXPECT_EQ(orientation_constraints[0].link_name, "link1"); + EXPECT_EQ(orientation_constraints[1].link_name, "link2"); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp b/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp new file mode 100644 index 0000000000..811e7807a8 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp @@ -0,0 +1,58 @@ +// 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 "../fixtures/move_group_fixture.hpp" + +namespace +{ + +TEST_F(MoveGroupFixture, GetWorkspaceFrameIdWorks) +{ + moveit_msgs::msg::WorkspaceParameters workspace_parameters; + EXPECT_EQ(moveit_ros::trajectory_cache::getWorkspaceFrameId(*move_group_, workspace_parameters), + move_group_->getRobotModel()->getModelFrame()); + + workspace_parameters.header.frame_id = "test_frame"; + EXPECT_EQ(moveit_ros::trajectory_cache::getWorkspaceFrameId(*move_group_, workspace_parameters), "test_frame"); +} + +TEST_F(MoveGroupFixture, GetCartesianPathRequestFrameId) +{ + moveit_msgs::srv::GetCartesianPath::Request path_request; + EXPECT_EQ(moveit_ros::trajectory_cache::getCartesianPathRequestFrameId(*move_group_, path_request), + move_group_->getPoseReferenceFrame()); + + path_request.header.frame_id = "test_frame"; + EXPECT_EQ(moveit_ros::trajectory_cache::getCartesianPathRequestFrameId(*move_group_, path_request), "test_frame"); +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp index 40afa630c6..194584f88e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/mesh_shape.cpp @@ -140,7 +140,11 @@ void MeshShape::clear() if (entity_) { entity_->detachFromParent(); - Ogre::MeshManager::getSingleton().remove(entity_->getMesh()->getName()); + const auto& mesh_name = entity_->getMesh()->getName(); + if (Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().getByName(mesh_name)) + { + Ogre::MeshManager::getSingleton().remove(mesh); + } scene_manager_->destroyEntity(entity_); entity_ = nullptr; } diff --git a/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt index d724e5a68d..498c189ccb 100644 --- a/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt @@ -5,9 +5,12 @@ find_package(ament_cmake REQUIRED) ament_package() -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} - PATTERN "setup_assistant.launch" EXCLUDE) +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +endif() + install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})