Implement trajectory cache

Signed-off-by: methylDragon <[email protected]>

moveit_ros/trajectory_cache/CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_motion_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.0 (2024-05-17)
+------------------
+* Add ``moveit_ros_trajectory_cache`` package for trajectory caching.

moveit_ros/trajectory_cache/CMakeLists.txt

@@ -0,0 +1,63 @@
+cmake_minimum_required(VERSION 3.8)
+project(moveit_ros_trajectory_cache)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_ros_warehouse REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(warehouse_ros REQUIRED)
+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)
+
+# Motion plan cache library
+add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp)
+ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies})
+
+#===============================================================================
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(launch_testing_ament_cmake REQUIRED)
+  find_package(rmf_utils REQUIRED)
+
+  add_executable(test_trajectory_cache src/test_trajectory_cache.cpp)
+  target_link_libraries(test_trajectory_cache ${TRAJECTORY_CACHE_LIBRARY_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}"
+  )
+
+endif()
+
+ament_export_dependencies(${trajectory_cache_dependencies})
+ament_package()

moveit_ros/trajectory_cache/README.md

@@ -0,0 +1 @@
+# Trajectory Cache

moveit_ros/trajectory_cache/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>moveit_ros_trajectory_cache</name>
+  <version>0.1.0</version>
+  <description>A trajectory cache for MoveIt 2 motion plans and cartesian plans.</description>
+  <maintainer email="[email protected]">Brandon Ong</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>tf2_ros</depend>
+  <depend>trajectory_msgs</depend>
+
+  <exec_depend>moveit_ros</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>warehouse_ros_sqlite</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_cmake_uncrustify</test_depend>
+  <test_depend>launch_pytest</test_depend>
+  <test_depend>launch_testing_ament_cmake</test_depend>
+
+  <test_depend>moveit_configs_utils</test_depend>
+  <test_depend>moveit_planners_ompl</test_depend>
+  <test_depend>moveit_resources</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>rmf_utils</test_depend>
+  <test_depend>robot_state_publisher</test_depend>
+  <test_depend>ros2_control</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>