Add force_cache_mode_execute_read_only

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/src/motion_planner_server.cpp

@@ -615,7 +615,8 @@ void MotionPlannerServer::plan_with_move_group(
       _collision_aware_cartesian_path);
 
     // Fetch if in execute mode.
-    if (cache_mode_is_execute(_cache_mode))
+    if (cache_mode_is_execute(_cache_mode)
+      || req.force_cache_mode_execute_read_only)
     {
       auto fetch_start = this->now();
       auto fetched_cartesian_plan =
@@ -640,7 +641,8 @@ void MotionPlannerServer::plan_with_move_group(
           fetched_cartesian_plan->lookupDouble("planning_time_s"));
       }
       // Fail if ReadOnly mode and no cached cartesian plan was fetched.
-      else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly)
+      else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly
+        || req.force_cache_mode_execute_read_only)
       {
         RCLCPP_ERROR(
           this->get_logger(),

nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv

@@ -52,6 +52,11 @@ string payload
 float64 max_velocity_scaling_factor
 float64 max_acceleration_scaling_factor
 
+# Set to true in order to force the server to use the motion cache as if the
+# cache mode were set to ExecuteReadOnly, regardless of the original setting
+# for this request.
+bool force_cache_mode_execute_read_only
+
 ---
 
 # Motion planning result

nexus_tree_nodes.xml

@@ -52,6 +52,7 @@
       <input_port name="scale_speed">An optional fraction [0,1] to scale the acceleraton and speed of the generated trajectory</input_port>
       <input_port name="cartesian">True if a cartesian plan is required</input_port>
       <input_port name="start_constraints">If provided the joint_constraints will be used as the start state of the robot. By default the current state is set as start</input_port>
+      <input_port name="force_cache_mode_execute_read_only">True to force cache mode to ExecuteReadOnly for this request.</input_port>
       <output_port name="result">The generated motion plan</output_port>
     </Action>