diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index c3d5902b063..d8a044c0b35 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -35,8 +35,8 @@ UndockRobot, ) from nav2_msgs.action import SmoothPath -from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes - +from nav2_msgs.srv import ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap +from nav2_msgs.srv import GetCostmap, LoadMap, ManageLifecycleNodes import rclpy from rclpy.action import ActionClient from rclpy.duration import Duration as rclpyDuration @@ -117,6 +117,12 @@ def __init__(self, node_name='basic_navigator', namespace=''): self.clear_costmap_local_srv = self.create_client( ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap' ) + self.clear_costmap_except_region_srv = self.create_client( + ClearCostmapExceptRegion, 'local_costmap/clear_costmap_except_region' + ) + self.clear_costmap_around_robot_srv = self.create_client( + ClearCostmapAroundRobot, 'local_costmap/clear_costmap_around_robot' + ) self.get_costmap_global_srv = self.create_client( GetCostmap, 'global_costmap/get_costmap' ) @@ -767,6 +773,26 @@ def clearGlobalCostmap(self): rclpy.spin_until_future_complete(self, future) return + def clearCostmapExceptRegion(self, reset_distance: float): + """Clear the costmap except for a specified region.""" + while not self.clear_costmap_except_region_srv.wait_for_service(timeout_sec=1.0): + self.info('ClearCostmapExceptRegion service not available, waiting...') + req = ClearCostmapExceptRegion.Request() + req.reset_distance = reset_distance + future = self.clear_costmap_except_region_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def clearCostmapAroundRobot(self, reset_distance: float): + """Clear the costmap around the robot.""" + while not self.clear_costmap_around_robot_srv.wait_for_service(timeout_sec=1.0): + self.info('ClearCostmapAroundRobot service not available, waiting...') + req = ClearCostmapAroundRobot.Request() + req.reset_distance = reset_distance + future = self.clear_costmap_around_robot_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + def getGlobalCostmap(self): """Get the global costmap.""" while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):