diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst index 82ad0f2be94..09f7aae2e9d 100644 --- a/moveit_ros/trajectory_cache/CHANGELOG.rst +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -4,4 +4,4 @@ Changelog for package nexus_motion_planner 0.1.0 (2024-05-17) ------------------ -* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. \ No newline at end of file +* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 3c888c7330b..8739357edd3 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -19,45 +19,43 @@ find_package(warehouse_ros_sqlite REQUIRED) include_directories(include) -set (trajectory_cache_dependencies - geometry_msgs - moveit_ros_planning_interface - moveit_ros_warehouse - rclcpp - tf2 - tf2_ros - trajectory_msgs - warehouse_ros - warehouse_ros_sqlite -) - -#=============================================================================== -set(TRAJECTORY_CACHE_LIBRARY_NAME trajectory_cache) +set(trajectory_cache_dependencies + geometry_msgs + moveit_ros_planning_interface + moveit_ros_warehouse + rclcpp + tf2 + tf2_ros + trajectory_msgs + warehouse_ros + warehouse_ros_sqlite) + +# ============================================================================== # Motion plan cache library -add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp) -ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies}) -target_include_directories(${TRAJECTORY_CACHE_LIBRARY_NAME} PUBLIC $) +add_library(trajectory_cache src/trajectory_cache.cpp) +ament_target_dependencies(trajectory_cache ${trajectory_cache_dependencies}) +target_include_directories( + trajectory_cache + PUBLIC $) -#=============================================================================== +# ============================================================================== if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) - # This test executable is run by the pytest_test, since a node is required for testing move groups + # 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 ${TRAJECTORY_CACHE_LIBRARY_NAME}) + target_link_libraries(test_trajectory_cache trajectory_cache) - install(TARGETS - test_trajectory_cache - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) + 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}" - ) + ament_add_pytest_test( + test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY + "${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index 497f003ed97..ba5dbfd2dc9 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -54,21 +54,23 @@ If you use this package in your work, please cite it using the following: // - `warehouse_port`: The port used for the database auto traj_cache = std::make_shared(node); -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"); +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"); -if (fetched_trajectory) { +if (fetched_trajectory) +{ // Great! We got a cache hit // Do something with the plan. -} else { +} +else +{ // Plan... And put it for posterity! traj_cache->putTrajectory( - *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, /*overwrite=*/true) + *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, /*overwrite=*/true); } ``` @@ -124,8 +126,8 @@ Since this is an initial release, the following features are unsupported because - 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. + - 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. \ No newline at end of file +- The fuzzy lookup can't be configured on a per-joint basis. 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 22f6b45e641..ce38049ff1d 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 @@ -257,4 +257,4 @@ class TrajectoryCache }; } // namespace trajectory_cache -} // namespace moveit_ros \ No newline at end of file +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py index a9c32c06daf..b24ba199191 100755 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -54,8 +54,7 @@ def robot_fixture(moveit_config): 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"], + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], ) robot_state_publisher = Node( @@ -87,8 +86,7 @@ def robot_fixture(moveit_config): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner {}".format( - controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="log", ) @@ -99,7 +97,7 @@ def robot_fixture(moveit_config): static_tf, robot_state_publisher, ros2_control_node, - *load_controllers + *load_controllers, ] @@ -111,24 +109,22 @@ def trajectory_cache_test_runner_node(moveit_config): name="test_trajectory_cache_node", output="screen", cached_output=True, - parameters=[moveit_config.to_dict()] + parameters=[moveit_config.to_dict()], ) @launch_pytest.fixture def launch_description(trajectory_cache_test_runner_node, robot_fixture): return LaunchDescription( - robot_fixture + - [ - trajectory_cache_test_runner_node, - launch_pytest.actions.ReadyToTest() - ] + robot_fixture + + [trajectory_cache_test_runner_node, launch_pytest.actions.ReadyToTest()] ) def validate_stream(expected): def wrapped(output): - assert expected in output.splitlines( + assert ( + expected in output.splitlines() ), f"Did not get expected: {expected} in output:\n\n{output}" return wrapped @@ -140,7 +136,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]") == 165, # All test cases passed. - timeout=30 + timeout=30, ) # Check no occurrences of [FAIL] in output @@ -148,7 +144,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=10, ) yield