Implement motion plan cache

.github/workflows/nexus_integration_tests.yaml

@@ -40,6 +40,8 @@ jobs:
         rosdep update
         rosdep install --from-paths . -yir
     - name: build
-      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
-    - name: test
+      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+    - name: Test - Unit Tests
+      run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+
+    - name: Test - Integration Test
       run: . ./install/setup.bash && cd nexus_integration_tests && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh python3 -m unittest

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 " <<

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.")};
   }

nexus_motion_planner/CMakeLists.txt

@@ -17,6 +17,7 @@ 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)
 
@@ -27,11 +28,17 @@ set (motion_planner_server_dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
-  warehouse_ros
   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
@@ -41,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)
 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)
@@ -65,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)
@@ -73,19 +86,37 @@ 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
   )
 
-  # TODO(YV): Fix these tests.
-  # add_launch_test(
-  #   test/test_request.test.py
-  #   TIMEOUT 60
-  # )
+  ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py"
+   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
+
+  # Motion planner server test with cache mode unset
+  add_launch_test(
+    test/test_request.test.py
+    TIMEOUT 60
+  )
+
+  # Motion planner server test with cache mode set (so it uses the cache)
+  add_launch_test(
+    test/test_request_with_cache.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()

nexus_motion_planner/config/planner_params.yaml

@@ -41,18 +41,4 @@ motion_planner_server:
       # The max_z of workspace bounding box.
     workspace_max_z: 1.0
     # The seconds within which the current robot state should be valid.
-    get_state_wait_seconds: 0.01
-    # Namespaced parameters for each robot when use_namespace is true.
-    # Is important to ensure move_group, robot_state_publisher and
-    # joint_state broadcaster all have the same namespace.
-    # Note: If use_namespace is false, pass these parameters directly.
-    abb_irb1300:
-      robot_description: ""
-      robot_description_semantic: ""
-      group_name: "manipulator"
-      # Group specific kinematic properties.
-      manipulator:
-        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-        kinematics_solver_search_resolution: 0.005
-        kinematics_solver_timeout: 0.005
-        kinematics_solver_attempts: 3
+    get_state_wait_seconds: 0.01

nexus_motion_planner/config/planner_params_with_cache.yaml

@@ -0,0 +1,76 @@
+motion_planner_server:
+  ros__parameters:
+    # List of robots to generate motion plans for
+    manipulators: ["abb_irb1300"]
+    # The default moveit group for each manipulator
+    default_group_name: "manipulator"
+    # Seconds to wait to connect to an action or service server
+    timeout_duration: 5
+    # Configure the planner_server to query motion plans directly from a move_group
+    # node that is running by initializing a move_group_interface.
+    # When set to false, the planner_server will initialize planning pipelines
+    # to directly compute plans but this has not been implemented yet.
+    use_move_group_interfaces: true
+    # When planning for multiple robots, set this to true.
+    use_namespace: false
+    # Tolerance for goal position.
+    goal_tolerance: 0.005
+    # Maximum seconds to generate a plan.
+    planning_time: 1.0
+    # Maximum number of replanning attempts
+    replan_attempts: 10
+    # If true, the cartesian interpolator will check for collisions.
+    collision_aware_cartesian_path: false
+    # The value of the max_step parameter used in cartesian interpolation.
+    cartesian_max_step: 0.001
+    # The value of the jump_threshold parameter used in cartesian interpolation.
+    cartesian_jump_threshold: 0.0
+    # Set true if trajectory should be executed after a plan is generated.
+    # The execution is a blocking event.
+    execute_trajectory: false
+    # The min_x of workspace bounding box.
+    workspace_min_x: -1.0
+    # The min_y of workspace bounding box.
+    workspace_min_y: -1.0
+    # The min_z of workspace bounding box.
+    workspace_min_z: -1.0
+    # The max_x of workspace bounding box.
+    workspace_max_x: 1.0
+    # The max_y of workspace bounding box.
+    workspace_max_y: 1.0
+      # The max_z of workspace bounding box.
+    workspace_max_z: 1.0
+    # The seconds within which the current robot state should be valid.
+    get_state_wait_seconds: 0.01
+
+    ## Motion Plan Cache Parameters
+    # Planner database mode. Valid values:
+    #   - Unset: Always plan. No caching.
+    #   - TrainingOverwrite: Always plan, overwriting existing cache plans if better plan was found.
+    #   - TrainingAppendOnly: Always plan, append to cache if better plan was found (no overwriting).
+    #   - ExecuteBestEffort: Prioritize cached plans. Only plan if no cached plans found.
+    #   - ExecuteReadOnly: Only use cached plans. Fail if no cached plans found.
+    planner_database_mode: "TrainingOverwrite"
+    # Database type and location
+    cache_db_plugin: "warehouse_ros_sqlite::DatabaseConnection"
+    cache_db_host: ":memory:"
+    cache_db_port: 0  # Isn't used for SQLite3
+
+    # The cache keys cache entries on certain properties of the motion plan request's
+    # starting joint state, goal constraints, and more.
+
+    # For float comparisons, what tolerance counts as an exact match (to prevent floating point precision errors)
+    cache_exact_match_tolerance: 0.001  # ~0.05 degrees per joint
+    # Query range thresholds for matching attributes of the starting robot state for cache hits.
+    # This typically applies to joint states, per joint, in radians
+    #
+    # It should be acceptable to be more lenient on the start state, because the robot will be commanded
+    # to go to the first trajectory point from it's current start state.
+    cache_start_match_tolerance: 0.025
+    # Query range thresholds for matching attributes of the requested planning goals for cache hits.
+    # This typically applies to the x, y, z coordinates of the goal pose, in metres
+    # And also the individual quaternion components of the goal orientation
+    #
+    # Goal tolerances should be more strict so the end point of the robot remains consistent between
+    # the fetched plan and the desired goal.
+    cache_goal_match_tolerance: 0.001

nexus_motion_planner/package.xml

@@ -28,9 +28,17 @@
   <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>
+  <test_depend>robot_state_publisher</test_depend>
+  <test_depend>ros2_control</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nexus_motion_planner/src/main.cpp

@@ -27,7 +27,7 @@ int main(int argc, char** argv)
   setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 
   rclcpp::init(argc, argv);
-  auto node = std::make_shared<MotionPlannerServer>();
+  auto node = std::make_shared<nexus::motion_planner::MotionPlannerServer>();
   rclcpp::spin(node->get_node_base_interface());
   rclcpp::shutdown();
   return 0;