diff --git a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp index 01ff34c968c..291d74fcdc9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_toggle_collision_monitor_service.cpp @@ -26,7 +26,7 @@ class ToggleCollisionMonitorService : public TestService { public: ToggleCollisionMonitorService() - : TestService("toggle_collision_monitor") + : TestService("/collision_monitor/toggle") {} }; @@ -105,7 +105,7 @@ TEST_P(ToggleParamTest, test_tick) R"( - diff --git a/nav2_msgs/srv/__init__.py b/nav2_msgs/srv/__init__.py index 444c4be926b..1316822eb16 100644 --- a/nav2_msgs/srv/__init__.py +++ b/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', ] diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index e84c9f4e57c..52080849820 100644 --- a/nav2_simple_commander/README.md +++ b/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. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ea93fc61711..b1a8d91d268 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/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.')