Plan cache code review fixes (sans unit tests)

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

@@ -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()

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>