Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 (#2941)

* Implement trajectory cache Signed-off-by: methylDragon <[email protected]> * Add README Signed-off-by: methylDragon <[email protected]> * Move test cpp to test directory Signed-off-by: methylDragon <[email protected]> * Clean up logging and comments Signed-off-by: methylDragon <[email protected]> * Use move_group node for time Signed-off-by: methylDragon <[email protected]> * Add and use logger Signed-off-by: methylDragon <[email protected]> * Use new move_group accessors Signed-off-by: methylDragon <[email protected]> * Coerce variable and method names to style Signed-off-by: methylDragon <[email protected]> * Formatting pass Signed-off-by: methylDragon <[email protected]> * Add docstrings Signed-off-by: methylDragon <[email protected]> * Add ability to sort in descending order Signed-off-by: methylDragon <[email protected]> * Add RFE for custom cost functions Signed-off-by: methylDragon <[email protected]> * Formatting pass Signed-off-by: methylDragon <[email protected]> * Fix build for downstream packages Signed-off-by: methylDragon <[email protected]> * Always get some workspace frame ID Signed-off-by: methylDragon <[email protected]> * Always get some cartesian path request frame ID Signed-off-by: methylDragon <[email protected]> * Fix tests Signed-off-by: methylDragon <[email protected]> * Add const qualifiers as appropriate Signed-off-by: methylDragon <[email protected]> * Add accessors, and support for preserving K plans Signed-off-by: methylDragon <[email protected]> * Edit docs and rename puts to inserts Signed-off-by: methylDragon <[email protected]> * Make clang tidy happy Signed-off-by: methylDragon <[email protected]> * Fix CMakeLists.txt Signed-off-by: methylDragon <[email protected]> * Make getters const Signed-off-by: methylDragon <[email protected]> * Clarify frame ID utils docstrings Signed-off-by: methylDragon <[email protected]> * Elaborate on trajectory cache benefits Signed-off-by: methylDragon <[email protected]> * Fix CHANGELOG, and make library shared Signed-off-by: methylDragon <[email protected]> * Add utils library with test fixtures Signed-off-by: methylDragon <[email protected]> * Add features interface with constant features Signed-off-by: methylDragon <[email protected]> * Add constraint feature extractor utils Signed-off-by: methylDragon <[email protected]> * Add RobotState.joint_state feature extractor utils Signed-off-by: methylDragon <[email protected]> * Add MotionPlanRequest features Signed-off-by: methylDragon <[email protected]> * Add GetCartesianPlanRequest features Signed-off-by: methylDragon <[email protected]> * Use namespace declarations and do cleanups Signed-off-by: methylDragon <[email protected]> * Add CacheInsertPolicyInterface and AlwaysInsertNeverPrunePolicy Signed-off-by: methylDragon <[email protected]> * Add CartesianAlwaysInsertNeverPrunePolicy Signed-off-by: methylDragon <[email protected]> * Init policy features on construction Signed-off-by: methylDragon <[email protected]> * Add execution time extraction util Signed-off-by: methylDragon <[email protected]> * Add BestSeenExecutionTimePolicy and rename methods Signed-off-by: methylDragon <[email protected]> * Add CartesianBestSeenExecutionTimePolicy Signed-off-by: methylDragon <[email protected]> * Return reason string from cache insert policy methods Signed-off-by: methylDragon <[email protected]> * Refactor TrajectoryCache to use the interfaces Signed-off-by: methylDragon <[email protected]> * Move test fixtures to their own directory Signed-off-by: methylDragon <[email protected]> * Fix bugs and build Signed-off-by: methylDragon <[email protected]> * Fix formatting and clang-tidy Signed-off-by: methylDragon <[email protected]> * Update CHANGELOG Signed-off-by: methylDragon <[email protected]> * Make clang-tidy happy Signed-off-by: methylDragon <[email protected]> * Update README Signed-off-by: methylDragon <[email protected]> * Enable unrelated query matching test Signed-off-by: methylDragon <[email protected]> * Make libraries shared Signed-off-by: methylDragon <[email protected]> * Sidestep deprecation warning for computeCartesianPath Signed-off-by: methylDragon <[email protected]> * Fix typo in trajectory cache utils test Signed-off-by: methylDragon <[email protected]> * Exclude test on humble * Add missing header * Use precrement for for loops Signed-off-by: methylDragon <[email protected]> * Use constref in range-based for loops in utils where possible Signed-off-by: methylDragon <[email protected]> * Reserve vectors in getSupportedFeatures Signed-off-by: methylDragon <[email protected]> * Fix README and CHANGELOG Signed-off-by: methylDragon <[email protected]> * Add and use restateInNewFrame util function Signed-off-by: methylDragon <[email protected]> * Attempt to fix policy test Signed-off-by: methylDragon <[email protected]> * Use .hpp instead of .h Signed-off-by: methylDragon <[email protected]> * Undo CHANGELOG changes Signed-off-by: methylDragon <[email protected]> * Use const ref strings for restateInNewFrame Signed-off-by: methylDragon <[email protected]> * Add clarificatory comment to tests and fix formatting Signed-off-by: methylDragon <[email protected]> * Fix compile error * Use constref shared_ptr in restateInNewFrame Signed-off-by: methylDragon <[email protected]> * Mitigate cartesian path tests Signed-off-by: methylDragon <[email protected]> * Mitigate test flakiness Signed-off-by: methylDragon <[email protected]> * Allow -11 for move_group gtest fixture Signed-off-by: methylDragon <[email protected]> * Make execution times in test deterministic Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> Co-authored-by: Sebastian Jahr <[email protected]> Co-authored-by: Sebastian Castro <[email protected]>
moveit · Jan 4, 2025 · a12f327 · a12f327
1 parent c3a2edc
commit a12f327
Show file tree

Hide file tree

Showing 33 changed files with 7,276 additions and 1,657 deletions.
diff --git a/.codespell_words b/.codespell_words
@@ -1,6 +1,7 @@
 aas
 ang
 ans
+AppendT
 atleast
 ba
 brin

diff --git 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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
@@ -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})
-Original file line number
+Diff line change
@@ -1,6 +1,7 @@
     aas
     ang
     ans
+    AppendT
     atleast
     ba
     brin
@@ Expand Down @@