Return error if not plan was fetched for non-cartesian requests (#31)

* Return error if not plan was fetched for non-cartesian requests Signed-off-by: Yadunund <[email protected]> * Fix xml mismatch Signed-off-by: Yadunund <[email protected]> * Fix README Signed-off-by: Yadunund <[email protected]> --------- Signed-off-by: Yadunund <[email protected]>
osrf · Jan 23, 2024 · 13cc805 · 13cc805
1 parent edce5e4
commit 13cc805
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 9 deletions.
diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # NEXUS
-![](https://github.com/OpenSourceRobotics/nexus/workflows/style/badge.svg)
-![](https://github.com/OpenSourceRobotics/nexus/workflows/integration_tests/badge.svg)
+![](https://github.com/osrf/nexus/workflows/style/badge.svg)
+![](https://github.com/osrf/nexus/workflows/integration_tests/badge.svg)
 
 ![](./docs/media/nexus_architecture.png)
 
@@ -30,7 +30,7 @@ curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh
 ```bash
 mkdir ~/ws_nexus/src -p
 cd ~/ws_nexus/src/
-git clone [email protected]:OpenSourceRobotics/nexus
+git clone [email protected]:osrf/nexus
 vcs import . < nexus/abb.repos
 cd ~/ws_nexus
 rosdep install --from-paths src --ignore-src --rosdistro iron -y -r
@@ -50,7 +50,7 @@ See sample BTs for [system_orchestrator](nexus_integration_tests/config/system_o
 At present, capabilities are registered as plugins with the `orchestrator` at runtime.
 We support [these capabilities](./nexus_capabilities/src/capabilities/plugins.xml) out of the bo
 Each plugin may register one or more behavior tree nodes which can then be used to define processes as seen in the `place_on_conveyor.xml`.
-The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/OpenSourceRobotics/nexus/discussions/369) for more details.
+The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/osrf/nexus/discussions/32) for more details.
 
 ### Visualization [experimental]
 ![](./docs/media/bt_example.png)
@@ -77,7 +77,7 @@ TODO: Add a dedicated demo package.
 
 The `nexus_endpoints` package contains ROS topics, services and actions used by NEXUS. The package is generated from [nexus_endpoints.redf.yaml](./nexus_endpoints.redf.yaml) using `redf`. rust is required to generated the package, the easiest way to install rust is via [rustup](https://rustup.rs/).
 
-With rust installed, clone the redf repo at https://github.com/OpenSourceRobotics/redf, then run
+With rust installed, clone the redf repo at https://github.com/osrf/redf, then run
 ```bash
 cargo install --path .
 ```

diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp
@@ -748,7 +748,8 @@ void MotionPlannerServer::plan_with_move_group(
     interface->constructMotionPlanRequest(plan_req_msg);
 
     // 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_plan = _motion_plan_cache->fetch_best_matching_plan(
@@ -771,7 +772,8 @@ void MotionPlannerServer::plan_with_move_group(
           fetched_plan->lookupDouble("planning_time_s"));
       }
       // Fail if ReadOnly mode and no cached 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(),
@@ -839,4 +841,4 @@ void MotionPlannerServer::plan_with_move_group(
 }
 
 }  // namespace planning_interface
-}  // namespace moveit
+}  // namespace moveit
diff --git a/nexus_tree_nodes.xml b/nexus_tree_nodes.xml
@@ -142,7 +142,7 @@
       <input_port name="target_pose">The geometry_msgs::msg::PoseStamped pose at the end of the offset</input_port>
       <input_port name="time">OPTIONAL, rclcpp::Time timepoint to lookup on, if not provided, the current time is used</input_port>
       <input_port name="local">OPTIONAL, if true, return t such that `target_pose = base_pose * t`</input_port>
-      <output_port name="result">The geometry_msgs::msg::Transform offset that transforms base_pose to target_pose</input_port>
+      <output_port name="result">The geometry_msgs::msg::Transform offset that transforms base_pose to target_pose</output_port>
     </Action>
 
     <Action ID="detection.GetDetection">