Add toggle cm to simple commander API (#5559)

* Add toggle cm to simple comander API, fix default toggle cm service in test for consistency

Signed-off-by: David G <[email protected]>

* Fix pre commit, ament flake, add readme

Signed-off-by: David G <[email protected]>

* add toggle msg to __init__.py

Signed-off-by: David G <[email protected]>

* add error handling

Signed-off-by: David G <[email protected]>

* add error handling for clear costmap functions

Signed-off-by: David G <[email protected]>

---------

Signed-off-by: David G <[email protected]>

Author: DavidG-Develop

nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp

@@ -26,7 +26,7 @@ class ToggleCollisionMonitorService : public TestService<nav2_msgs::srv::Toggle>
 {
 public:
   ToggleCollisionMonitorService()
-  : TestService("toggle_collision_monitor")
+  : TestService("/collision_monitor/toggle")
   {}
 };
 
@@ -105,7 +105,7 @@ TEST_P(ToggleParamTest, test_tick)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <ToggleCollisionMonitor service_name="toggle_collision_monitor" enable=")")
+            <ToggleCollisionMonitor service_name="/collision_monitor/toggle" enable=")")
     +
     std::string(enable ? "true" : "false") +
     R"(" />

nav2_msgs/srv/__init__.py

@@ -10,6 +10,7 @@
 from nav2_msgs.srv._reload_dock_database import ReloadDockDatabase
 from nav2_msgs.srv._save_map import SaveMap
 from nav2_msgs.srv._set_initial_pose import SetInitialPose
+from nav2_msgs.srv._toggle import Toggle
 
 __all__ = [
     'ClearCostmapAroundRobot',
@@ -24,4 +25,5 @@
     'ReloadDockDatabase',
     'SaveMap',
     'SetInitialPose',
+    'Toggle',
 ]

nav2_simple_commander/README.md

@@ -40,6 +40,7 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
 | clearGlobalCostmap()              | Clears the global costmap.                                                 |
 | getGlobalCostmap()                | Returns the global costmap, `nav2_msgs/Costmap`                            |
 | getLocalCostmap()                 | Returns the local costmap, `nav2_msgs/Costmap`                             |
+| toggleCollisionMonitor(enable)    | Toggles the collision monitor on (`True`) or off (`False`).                |
 | waitUntilNav2Active(navigator='bt_navigator, localizer='amcl') | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. Custom navigator and localizer nodes can be specified  |
 | lifecycleStartup()                | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. |
 | lifecycleShutdown()               | Sends a request to all lifecycle management servers to shut them down.     |

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -31,7 +31,7 @@
 from nav2_msgs.msg import Route
 from nav2_msgs.srv import (ClearCostmapAroundPose, ClearCostmapAroundRobot,
                            ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap,
-                           ManageLifecycleNodes)
+                           ManageLifecycleNodes, Toggle)
 from nav_msgs.msg import Goals, OccupancyGrid, Path
 import rclpy
 from rclpy.action import ActionClient
@@ -251,6 +251,11 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N
             self.create_client(
             GetCostmap, 'local_costmap/get_costmap'
         )
+        self.toggle_collision_monitor_srv: Client[
+            Toggle.Request, Toggle.Response] = \
+            self.create_client(
+            Toggle, 'collision_monitor/toggle'
+        )
 
     def destroyNode(self) -> None:
         self.destroy_node()
@@ -1056,6 +1061,11 @@ def clearLocalCostmap(self) -> None:
         req = ClearEntireCostmap.Request()
         future = self.clear_costmap_local_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear local costmap request failed!')
+
         return
 
     def clearGlobalCostmap(self) -> None:
@@ -1065,6 +1075,11 @@ def clearGlobalCostmap(self) -> None:
         req = ClearEntireCostmap.Request()
         future = self.clear_costmap_global_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear global costmap request failed!')
+
         return
 
     def clearCostmapExceptRegion(self, reset_distance: float) -> None:
@@ -1075,6 +1090,11 @@ def clearCostmapExceptRegion(self, reset_distance: float) -> None:
         req.reset_distance = reset_distance
         future = self.clear_costmap_except_region_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear costmap except region request failed!')
+
         return
 
     def clearCostmapAroundRobot(self, reset_distance: float) -> None:
@@ -1085,6 +1105,11 @@ def clearCostmapAroundRobot(self, reset_distance: float) -> None:
         req.reset_distance = reset_distance
         future = self.clear_costmap_around_robot_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear costmap around robot request failed!')
+
         return
 
     def clearLocalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None:
@@ -1096,6 +1121,11 @@ def clearLocalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float)
         req.reset_distance = reset_distance
         future = self.clear_local_costmap_around_pose_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear local costmap around pose request failed!')
+
         return
 
     def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float) -> None:
@@ -1107,6 +1137,11 @@ def clearGlobalCostmapAroundPose(self, pose: PoseStamped, reset_distance: float)
         req.reset_distance = reset_distance
         future = self.clear_global_costmap_around_pose_srv.call_async(req)
         rclpy.spin_until_future_complete(self, future)
+
+        result = future.result()
+        if result is None:
+            self.error('Clear global costmap around pose request failed!')
+
         return
 
     def getGlobalCostmap(self) -> OccupancyGrid:
@@ -1140,6 +1175,21 @@ def getLocalCostmap(self) -> OccupancyGrid:
 
         return result.map
 
+    def toggleCollisionMonitor(self, enable: bool) -> None:
+        """Toggle the collision monitor."""
+        while not self.toggle_collision_monitor_srv.wait_for_service(timeout_sec=1.0):
+            self.info('Toggle collision monitor service not available, waiting...')
+        req = Toggle.Request()
+        req.enable = enable
+        future = self.toggle_collision_monitor_srv.call_async(req)
+
+        rclpy.spin_until_future_complete(self, future)
+        result = future.result()
+        if result is None:
+            self.error('Toggle collision monitor request failed!')
+
+        return
+
     def lifecycleStartup(self) -> None:
         """Startup nav2 lifecycle system."""
         self.info('Starting up lifecycle nodes based on lifecycle_manager.')