Skip to content

Commit 8f9603b

Browse files
mergify[bot]methylDragon
mergify[bot]
and
methylDragon
authored
Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 (#2941) (#3408)
* 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]> (cherry picked from commit a12f327) Co-authored-by: methylDragon <[email protected]>
1 parent 355cd6b commit 8f9603b

33 files changed

+7276
-1657
lines changed

‎.codespell_words‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
aas
22
ang
33
ans
4+
AppendT
45
atleast
56
ba
67
brin

‎moveit_ros/trajectory_cache/CMakeLists.txt‎

Lines changed: 60 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -28,9 +28,62 @@ set(TRAJECTORY_CACHE_DEPENDENCIES
2828
trajectory_msgs
2929
warehouse_ros)
3030

31+
set(TRAJECTORY_CACHE_LIBRARIES
32+
moveit_ros_trajectory_cache_utils_lib
33+
moveit_ros_trajectory_cache_features_lib
34+
moveit_ros_trajectory_cache_cache_insert_policies_lib
35+
moveit_ros_trajectory_cache_lib)
36+
37+
# Utils library
38+
add_library(moveit_ros_trajectory_cache_utils_lib SHARED src/utils/utils.cpp)
39+
generate_export_header(moveit_ros_trajectory_cache_utils_lib)
40+
target_include_directories(
41+
moveit_ros_trajectory_cache_utils_lib
42+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
43+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
44+
ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib
45+
${TRAJECTORY_CACHE_DEPENDENCIES})
46+
47+
# Features library
48+
add_library(
49+
moveit_ros_trajectory_cache_features_lib SHARED
50+
src/features/motion_plan_request_features.cpp
51+
src/features/get_cartesian_path_request_features.cpp)
52+
generate_export_header(moveit_ros_trajectory_cache_features_lib)
53+
target_link_libraries(moveit_ros_trajectory_cache_features_lib
54+
moveit_ros_trajectory_cache_utils_lib)
55+
target_include_directories(
56+
moveit_ros_trajectory_cache_features_lib
57+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
58+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
59+
ament_target_dependencies(moveit_ros_trajectory_cache_features_lib
60+
${TRAJECTORY_CACHE_DEPENDENCIES})
61+
62+
# Cache insert policies library
63+
add_library(
64+
moveit_ros_trajectory_cache_cache_insert_policies_lib SHARED
65+
src/cache_insert_policies/always_insert_never_prune_policy.cpp
66+
src/cache_insert_policies/best_seen_execution_time_policy.cpp)
67+
generate_export_header(moveit_ros_trajectory_cache_cache_insert_policies_lib)
68+
target_link_libraries(
69+
moveit_ros_trajectory_cache_cache_insert_policies_lib
70+
moveit_ros_trajectory_cache_features_lib
71+
moveit_ros_trajectory_cache_utils_lib)
72+
target_include_directories(
73+
moveit_ros_trajectory_cache_cache_insert_policies_lib
74+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
75+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
76+
ament_target_dependencies(moveit_ros_trajectory_cache_cache_insert_policies_lib
77+
${TRAJECTORY_CACHE_DEPENDENCIES})
78+
3179
# Trajectory cache library
3280
add_library(moveit_ros_trajectory_cache_lib SHARED src/trajectory_cache.cpp)
3381
generate_export_header(moveit_ros_trajectory_cache_lib)
82+
target_link_libraries(
83+
moveit_ros_trajectory_cache_lib
84+
moveit_ros_trajectory_cache_cache_insert_policies_lib
85+
moveit_ros_trajectory_cache_features_lib
86+
moveit_ros_trajectory_cache_utils_lib)
3487
target_include_directories(
3588
moveit_ros_trajectory_cache_lib
3689
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
@@ -39,7 +92,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib
3992
${TRAJECTORY_CACHE_DEPENDENCIES})
4093

4194
install(
42-
TARGETS moveit_ros_trajectory_cache_lib
95+
TARGETS ${TRAJECTORY_CACHE_LIBRARIES}
4396
EXPORT moveit_ros_trajectory_cacheTargets
4497
LIBRARY DESTINATION lib
4598
ARCHIVE DESTINATION lib
@@ -48,29 +101,14 @@ install(
48101
DESTINATION include/moveit_ros_trajectory_cache)
49102

50103
install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache)
51-
install(
52-
FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h
53-
DESTINATION include/moveit_ros_trajectory_cache)
54-
55-
if(BUILD_TESTING)
56-
find_package(ament_cmake_pytest REQUIRED)
57-
find_package(launch_testing_ament_cmake REQUIRED)
58-
find_package(rmf_utils REQUIRED)
59-
find_package(warehouse_ros_sqlite REQUIRED)
60-
61-
# This test executable is run by the pytest_test, since a node is required for
62-
# testing move groups
63-
add_executable(test_trajectory_cache test/test_trajectory_cache.cpp)
64-
target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib)
65-
ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite)
66-
67-
install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME})
68104

69-
ament_add_pytest_test(
70-
test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY
71-
"${CMAKE_CURRENT_BINARY_DIR}")
105+
# Install export headers for each library
106+
foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES})
107+
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h
108+
DESTINATION include/moveit_ros_trajectory_cache)
109+
endforeach()
72110

73-
endif()
111+
add_subdirectory(test)
74112

75113
ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET)
76114
ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES})

0 commit comments

Comments
 (0)