Implement cartesian plan cache (#18)

* Add cartesian plan caching interfaces Signed-off-by: methylDragon <[email protected]> * Add construct_get_cartesian_plan_request Signed-off-by: methylDragon <[email protected]> * Add goal query and metadata Signed-off-by: methylDragon <[email protected]> * Add start query and metadata Signed-off-by: methylDragon <[email protected]> * Implement top level cache ops Signed-off-by: methylDragon <[email protected]> * Use motion plan cache for cartesian plans Signed-off-by: methylDragon <[email protected]> * Allow mismatched plan frames since we coerce anyway Signed-off-by: methylDragon <[email protected]> * Fix move bug Signed-off-by: methylDragon <[email protected]> * Plan cache code review fixes (sans unit tests) (#26) * Remove query appending macro Signed-off-by: methylDragon <[email protected]> * Default to warehouse_ros plugin if warehouse plugin isn't set Signed-off-by: methylDragon <[email protected]> * Return and use init result Signed-off-by: methylDragon <[email protected]> * Add todo for catching exceptions Signed-off-by: methylDragon <[email protected]> * Implement plan fetching with configurable key Signed-off-by: methylDragon <[email protected]> * Add comments for exact match tolerance Signed-off-by: methylDragon <[email protected]> * Slightly refactor put plan Signed-off-by: methylDragon <[email protected]> * Rename overwrite to delete_worse_plans Signed-off-by: methylDragon <[email protected]> * Split out motion plan cache into its own library Signed-off-by: methylDragon <[email protected]> * Sort constraints for reduced cardinality Signed-off-by: methylDragon <[email protected]> * Rename util function Signed-off-by: methylDragon <[email protected]> * Add todo for is_diff Signed-off-by: methylDragon <[email protected]> * Add unit tests for motion plan cache (#28) * Add count methods Signed-off-by: methylDragon <[email protected]> * Enable shared from this for cache class Signed-off-by: methylDragon <[email protected]> * Add unit test build rules Signed-off-by: methylDragon <[email protected]> * Add tests for motion plan cache (but not cartesian) Signed-off-by: methylDragon <[email protected]> * Fix bugs in cartesian caching Signed-off-by: methylDragon <[email protected]> * Add tests for cartesian plan cache Signed-off-by: methylDragon <[email protected]> * Exit if a test fails Signed-off-by: methylDragon <[email protected]> * Remove gtest import Signed-off-by: methylDragon <[email protected]> * Remove enable_shared_from_this Signed-off-by: methylDragon <[email protected]> * Only check for failure Signed-off-by: methylDragon <[email protected]> * Test half in-tolerance Signed-off-by: methylDragon <[email protected]> * Test different robot cache Signed-off-by: methylDragon <[email protected]> * Add force_cache_mode_execute_read_only (#29) * Add force_cache_mode_execute_read_only Signed-off-by: methylDragon <[email protected]> * Add force_cache_mode_execute_read_only input port Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]>
osrf · Dec 4, 2023 · e4d5dd6 · e4d5dd6
1 parent ca0f123
commit e4d5dd6
Show file tree

Hide file tree

Showing 12 changed files with 2,560 additions and 135 deletions.
diff --git a/nexus_capabilities/src/capabilities/plan_motion.cpp b/nexus_capabilities/src/capabilities/plan_motion.cpp
@@ -120,6 +120,17 @@ make_request()
   req->max_velocity_scaling_factor = scale_speed;
   req->max_acceleration_scaling_factor = scale_speed;
 
+  auto maybe_read_only =
+    this->getInput<bool>("force_cache_mode_execute_read_only");
+  if (maybe_read_only.has_value())
+  {
+    req->force_cache_mode_execute_read_only = maybe_read_only.value();
+  }
+  else
+  {
+    req->force_cache_mode_execute_read_only = false;
+  }
+
   RCLCPP_DEBUG_STREAM(this->_logger,
     "planning to " << req->goal_pose.pose << " at frame " <<
       req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " <<

diff --git a/nexus_capabilities/src/capabilities/plan_motion.hpp b/nexus_capabilities/src/capabilities/plan_motion.hpp
@@ -66,6 +66,9 @@ public: static BT::PortsList providedPorts()
       BT::InputPort<std::vector<moveit_msgs::msg::JointConstraint>>(
         "start_constraints",
         "OPTIONAL. If provided the the joint_constraints are used as the start condition. Else, the current state of the robot will be used as the start."),
+      BT::InputPort<bool>(
+        "force_cache_mode_execute_read_only",
+        "OPTIONAL. Set true to force cache mode to ExecuteReadOnly for this request."),
       BT::OutputPort<moveit_msgs::msg::RobotTrajectory>(
         "result", "The resulting trajectory.")};
   }

diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt
@@ -28,12 +28,17 @@ set (motion_planner_server_dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
-  warehouse_ros
-  warehouse_ros_sqlite
   tf2
   tf2_ros
 )
 
+set (motion_plan_cache_dependencies
+  moveit_ros_planning_interface
+  rclcpp
+  warehouse_ros
+  warehouse_ros_sqlite
+)
+
 set (test_request_dependencies
   moveit_msgs
   nexus_endpoints
@@ -43,12 +48,17 @@ set (test_request_dependencies
 )
 
 #===============================================================================
-set(LIBRARY_NAME motion_planner_server_core)
+set(MOTION_PLAN_CACHE_LIBRARY_NAME motion_planner_server_core)
 set(EXECUTABLE_NAME motion_planner_server)
 
+# Motion plan cache library
+add_library(${MOTION_PLAN_CACHE_LIBRARY_NAME} src/motion_plan_cache.cpp)
+ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_plan_cache_dependencies})
+
 # Server executable
-add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp src/motion_plan_cache.cpp)
+add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp)
 ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies})
+target_link_libraries(${EXECUTABLE_NAME} ${MOTION_PLAN_CACHE_LIBRARY_NAME})
 
 # Test executable
 add_executable(test_request src/test_request.cpp)
@@ -67,6 +77,7 @@ install(
 )
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
   find_package(ament_cmake_uncrustify REQUIRED)
   find_package(launch_testing_ament_cmake REQUIRED)
   find_package(rmf_utils REQUIRED)
@@ -75,19 +86,31 @@ if(BUILD_TESTING)
     NAMES "rmf_code_style.cfg"
     PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/")
 
+  add_executable(test_motion_plan_cache src/test_motion_plan_cache.cpp)
+  target_link_libraries(test_motion_plan_cache ${MOTION_PLAN_CACHE_LIBRARY_NAME})
+
+  install(TARGETS
+      test_motion_plan_cache
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+
   ament_uncrustify(
     ARGN include src test
     CONFIG_FILE ${uncrustify_config_file}
     MAX_LINE_LENGTH 80
     LANGUAGE CPP
   )
 
+  ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py"
+   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
+
   # TODO(YV): Fix these tests.
   # add_launch_test(
   #   test/test_request.test.py
   #   TIMEOUT 60
   # )
 endif()
 
-ament_export_dependencies(${motion_planner_server_dependencies})
+ament_export_dependencies(${motion_planner_server_dependencies} ${motion_plan_cache_dependencies})
 ament_package()
diff --git a/nexus_motion_planner/package.xml b/nexus_motion_planner/package.xml
@@ -28,8 +28,14 @@
   <test_depend>abb_irb1300_10_115_moveit_config</test_depend>
   <test_depend>abb_irb1300_support</test_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_resources</test_depend>
+  <test_depend>python3-pytest</test_depend>
   <test_depend>rmf_utils</test_depend>
 
   <export>