Navigation 2 Logo
latest
  • Getting Started
    • Installation
    • Running the Example
    • Navigating
  • Build and Install
    • Install
    • Build
      • For Released Distributions
        • Install ROS
        • Build Nav2
      • For Main Branch Development
        • Build ROS 2 Main
        • Build Nav2 Main
    • Docker
      • Building Docker Container
      • Using DockerHub Container
    • Generate Doxygen
    • Help
      • Build Troubleshooting Guide
        • Common Nav2 Dependencies Build Failures
        • Reporting Issue
  • Navigation Concepts
    • ROS 2
      • Action Server
      • Lifecycle Nodes and Bond
    • Behavior Trees
    • Navigation Servers
      • Planner, Controller, Smoother and Recovery Servers
      • Planners
      • Controllers
      • Behaviors
      • Smoothers
      • Waypoint Following
    • State Estimation
      • Standards
      • Global Positioning: Localization and SLAM
      • Odometry
    • Environmental Representation
      • Costmaps and Layers
      • Costmap Filters
      • Other Forms
    • Nav2 Academic Overview
  • First-Time Robot Setup Guide
    • Setting Up Transformations
      • Transforms Introduction
      • Static Transform Publisher Demo
      • Transforms in Navigation2
      • Conclusion
    • Setting Up The URDF
      • URDF and the Robot State Publisher
      • Setting Up the Environment
      • Writing the URDF
      • Build and Launch
      • Visualization using RVIZ
      • Adding Physical Properties
      • Conclusion
    • Setting Up Odometry
      • Odometry Introduction
      • Setting Up Odometry on your Robot
      • Simulating an Odometry System using Gazebo
        • Setup and Prerequisites
        • Adding Gazebo Plugins to a URDF
        • Launch and Build Files
        • Build, Run and Verification
      • Robot Localization Demo
        • Configuring Robot Localization
        • Launch and Build Files
        • Build, Run and Verification
      • Conclusion
    • Setting Up Sensors
      • Sensor Introduction
        • Common Sensor Messages
      • Simulating Sensors using Gazebo
        • Adding Gazebo Plugins to a URDF
        • Launch and Build Files
        • Build, Run and Verification
      • Mapping and Localization
      • Costmap 2D
        • Configuring nav2_costmap_2d
        • Build, Run and Verification
      • Conclusion
    • Setting Up the Robot’s Footprint
      • Footprint Introduction
      • Configuring the Robot’s Footprint
      • Build, Run and Verification
      • Visualizing Footprint in RViz
      • Conclusion
    • Setting Up Navigation Plugins
      • Planner and Controller Servers
      • Selecting the Algorithm Plugins
        • Planner Server
        • Controller Server
      • Conclusion
  • General Tutorials
    • Navigating with a Physical Turtlebot 3
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Setup Your Enviroment Variables
        • 1- Launch Turtlebot 3
        • 2- Launch Nav2
        • 3- Launch RVIZ
        • 4- Initialize the Location of Turtlebot 3
        • 5- Send a Goal Pose
    • (SLAM) Navigating While Mapping
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Launch Robot Interfaces
        • 1- Launch Navigation2
        • 2- Launch SLAM
        • 3- Working with SLAM
        • 4- Getting Started Simplification
    • (STVL) Using an External Costmap Plugin
      • Overview
      • Costmap2D and STVL
      • Tutorial Steps
        • 0- Setup
        • 1- Install STVL
        • 1- Modify Navigation2 Parameter
        • 2- Launch Navigation2
        • 3- RVIZ
    • Groot - Interacting with Behavior Trees
      • Overview
      • Visualize Behavior Trees
      • Edit Behavior Trees
      • Adding A Custom Node
    • Camera Calibration
      • Overview
      • Requirements
      • Tutorial Steps
    • Get Backtrace in ROS 2 / Nav2
      • Overview
      • Preliminaries
      • From a Node
      • From a Launch File
      • From Large Project
      • From Nav2 Bringup
      • Automatic backtrace on crash
    • Profiling in ROS 2 / Nav2
      • Overview
      • Preliminaries
      • Profile from a Node
      • Profile from a Launch File
      • From Nav2 Bringup
      • Interpreting Results
    • Dynamic Object Following
      • Overview
      • Tutorial Steps
        • 0- Create the Behavior Tree
        • 1- Setup Rviz clicked point
        • 2- Run Dynamic Object Following in Nav2 Simulation
    • Navigating with Keepout Zones
      • Overview
      • Requirements
      • Tutorial Steps
        • 1. Prepare filter mask
        • 2. Configure Costmap Filter Info Publisher Server
        • 3. Enable Keepout Filter
        • 4. Run Nav2 stack
    • Navigating with Speed Limits
      • Overview
      • Requirements
      • Tutorial Steps
        • 1. Prepare filter mask
        • 2. Configure Costmap Filter Info Publisher Server
        • 3. Enable Speed Filter
        • 4. Run Nav2 stack
    • Using Rotation Shim Controller
      • Overview
      • What is the Rotation Shim Controller?
      • Configuring Rotation Shim Controller
      • Configuring Primary Controller
      • Demo Execution
    • Adding a Smoother to a BT
      • Overview
      • Requirements
      • Tutorial Steps
        • 0- Familiarization with the Smoother BT Node
        • 1- Specifying a Smoother Plugin
        • 2- Modifying your BT XML
    • Using Collision Monitor
      • Overview
      • Requirements
      • Configuring Collision Monitor
      • Preparing Nav2 stack
      • Demo Execution
    • Adding a New Nav2 Task Server
      • Lifecycle Nodes
      • Composition
      • Error codes
      • Conclusion
  • Plugin Tutorials
    • Writing a New Costmap2D Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Write a new Costmap2D plugin
        • 2- Export and make GradientLayer plugin
        • 3- Enable the plugin in Costmap2D
        • 4- Run GradientLayer plugin
    • Writing a New Planner Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new Planner Plugin
        • 2- Exporting the planner plugin
        • 3- Pass the plugin name through params file
        • 4- Run StraightLine plugin
    • Writing a New Controller Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Create a new Controller Plugin
        • 2- Exporting the controller plugin
        • 3- Pass the plugin name through the params file
        • 4- Run Pure Pursuit Controller plugin
    • Writing a New Behavior Tree Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new BT Plugin
        • 2- Exporting the planner plugin
        • 3- Add plugin library name to config
        • 4- Run Your Custom plugin
    • Writing a New Behavior Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Creating a new Behavior Plugin
        • 2- Exporting the Behavior Plugin
        • 3- Pass the plugin name through params file
        • 4- Run Behavior Plugin
    • Writing a New Navigator Plugin
      • Overview
      • Requirements
      • Tutorial Steps
        • 1- Create a new Navigator Plugin
        • 2- Exporting the navigator plugin
        • 3- Pass the plugin name through the params file
        • 4- Run plugin
  • Configuration Guide
    • Behavior-Tree Navigator
      • Parameters
      • Example
    • Behavior Tree XML Nodes
      • Action Plugins
        • Wait
        • Spin
        • BackUp
        • DriveOnHeading
        • AssistedTeleop
        • ComputePathToPose
        • FollowPath
        • NavigateToPose
        • ClearEntireCostmap
        • ClearCostmapExceptRegion
        • ClearCostmapAroundRobot
        • ReinitializeGlobalLocalization
        • TruncatePath
        • TruncatePathLocal
        • PlannerSelector
        • ControllerSelector
        • SmootherSelector
        • GoalCheckerSelector
        • NavigateThroughPoses
        • ComputePathThroughPoses
        • RemovePassedGoals
        • CancelControl
        • CancelBackUp
        • CancelSpin
        • CancelWait
        • CancelDriveOnHeading
        • CancelAssistedTeleop
        • SmoothPath
      • Condition Plugins
        • GoalReached
        • TransformAvailable
        • DistanceTraveled
        • GoalUpdated
        • GloballyUpdatedGoal
        • InitialPoseReceived
        • IsStuck
        • TimeExpired
        • IsBatteryLow
        • IsPathValid
        • PathExpiringTimer
        • AreErrorCodesPresent
        • WouldAControllerRecoveryHelp
        • WouldAPlannerRecoveryHelp
        • WouldASmootherRecoveryHelp
      • Control Plugins
        • PipelineSequence
        • RoundRobin
        • RecoveryNode
      • Decorator Plugins
        • RateController
        • DistanceController
        • SpeedController
        • GoalUpdater
        • PathLongerOnApproach
        • SingleTrigger
      • Example
    • Costmap 2D
      • Costmap2D ROS Parameters
      • Default Plugins
      • Plugin Parameters
        • Static Layer Parameters
        • Inflation Layer Parameters
        • Obstacle Layer Parameters
        • Voxel Layer Parameters
        • Range Sensor Parameters
      • Costmap Filters Parameters
        • Keepout Filter Parameters
        • Speed Filter Parameters
        • Binary Filter Parameters
      • Example
    • Lifecycle Manager
      • Parameters
      • Example
    • Planner Server
      • Parameters
      • Default Plugins
      • Example
    • NavFn Planner
      • Parameters
      • Example
    • Smac Planner
      • Provided Plugins
        • Smac 2D Planner
        • Smac Hybrid-A* Planner
        • Smac State Lattice Planner
      • Description
    • Theta Star Planner
      • Parameters
      • Example
    • Controller Server
      • Parameters
      • Provided Plugins
        • SimpleProgressChecker
        • SimpleGoalChecker
        • StoppedGoalChecker
      • Default Plugins
      • Example
    • DWB Controller
      • Controller
        • DWB Controller
        • XYTheta Iterator
        • Kinematic Parameters
        • Publisher
      • Plugins
        • LimitedAccelGenerator
        • StandardTrajectoryGenerator
      • Trajectory Critics
        • BaseObstacleCritic
        • GoalAlignCritic
        • GoalDistCritic
        • ObstacleFootprintCritic
        • OscillationCritic
        • PathAlignCritic
        • PathDistCritic
        • PreferForwardCritic
        • RotateToGoalCritic
        • TwirlingCritic
      • Example
    • Regulated Pure Pursuit
      • Regulated Pure Pursuit Parameters
      • Example
    • Model Predictive Path Integral Controller
      • MPPI Parameters
        • Trajectory Visualization
        • Path Handler
        • Ackermann Motion Model
        • Constraint Critic
        • Goal Angle Critic
        • Goal Critic
        • Obstacles Critic
        • Path Align Critic
        • Path Angle Critic
        • Path Follow Critic
        • Prefer Forward Critic
        • Twirling Critic
      • Example
      • Notes to Users
        • General Words of Wisdom
        • Prediction Horizon, Costmap Sizing, and Offsets
        • Obstacle, Inflation Layer, and Path Following
    • Rotation Shim Controller
      • Rotation Shim Controller Parameters
      • Example
    • Map Server / Saver
      • Map Saver Parameters
      • Map Server Parameters
      • Costmap Filter Info Server Parameters
      • Example
    • AMCL
      • Parameters
      • Example
    • Behavior Server
      • Behavior Server Parameters
      • Default Plugins
      • Spin Behavior Parameters
      • BackUp Behavior Parameters
      • DriveOnHeading Behavior Parameters
      • AssistedTeleop Behavior Parameters
      • Example
    • Smoother Server
      • Smoother Server Parameters
      • Example
    • Simple Smoother
      • Simple Smoother Parameters
      • Example
    • Savitzky-Golay Smoother
      • Savitzky-Golay Smoother Parameters
      • Example
    • Constrained smoother
      • Smoother Server Parameters
      • Example
    • Velocity Smoother
      • Velocity Smoother Parameters
      • Example
    • Collision Monitor
      • Features
      • Parameters
        • Polygons parameters
        • Observation sources parameters
      • Example
    • Waypoint Follower
      • Parameters
      • Provided Plugins
        • WaitAtWaypoint
        • PhotoAtWaypoint
        • InputAtWaypoint
      • Default Plugin
      • Example
  • Tuning Guide
    • Inflation Potential Fields
    • Robot Footprint vs Radius
    • Rotate in Place Behavior
    • Planner Plugin Selection
    • Controller Plugin Selection
    • Caching Obstacle Heuristic in Smac Planners
    • Nav2 Launch Options
    • Other Pages We’d Love To Offer
  • Nav2 Behavior Trees
    • Introduction To Nav2 Specific Nodes
      • Action Nodes
      • Condition Nodes
      • Decorator Nodes
      • Control: PipelineSequence
      • Control: Recovery
      • Control: RoundRobin
    • Detailed Behavior Tree Walkthrough
      • Overview
      • Prerequisites
      • Navigate To Pose With Replanning and Recovery
      • Navigation Subtree
      • Recovery Subtree
    • Navigate To Pose
    • Navigate Through Poses
    • Navigate To Pose and Pause Near Goal-Obstacle
    • Navigate To Pose With Consistent Replanning And If Path Becomes Invalid
    • Follow Dynamic Point
    • Odometry Calibration
  • Navigation Plugins
    • Behavior-Tree Navigators
    • Costmap Layers
    • Costmap Filters
    • Controllers
    • Planners
    • Smoothers
    • Behaviors
    • Waypoint Task Executors
    • Goal Checkers
    • Progress Checkers
    • Behavior Tree Nodes
  • Migration Guides
    • Dashing to Eloquent
      • New Packages
      • New Plugins
      • Navigation2 Architectural Changes
    • Eloquent to Foxy
      • General
      • Server Updates
      • New Plugins
      • Map Server Re-Work
      • New Particle Filter Messages
      • Selection of Behavior Tree in each navigation action
      • FollowPoint Capability
      • New Costmap Layer
    • Foxy to Galactic
      • NavigateToPose Action Feedback updates
      • NavigateToPose BT-node Interface Changes
      • NavigateThroughPoses and ComputePathThroughPoses Actions Added
      • ComputePathToPose BT-node Interface Changes
      • ComputePathToPose Action Interface Changes
      • BackUp BT-node Interface Changes
      • BackUp Recovery Interface Changes
      • Nav2 Controllers and Goal Checker Plugin Interface Changes
      • FollowPath goal_checker_id attribute
      • Groot Support
      • New Plugins
      • Costmap Filters
      • SmacPlanner
      • ThetaStarPlanner
      • RegulatedPurePursuitController
      • Costmap2D current_ Usage
      • Standard time units in parameters
      • Ray Tracing Parameters
      • Obstacle Marking Parameters
      • Recovery Action Changes
      • Default Behavior Tree Changes
      • NavFn Planner Parameters
      • New ClearCostmapExceptRegion and ClearCostmapAroundRobot BT-nodes
      • New Behavior Tree Nodes
      • sensor_msgs/PointCloud to sensor_msgs/PointCloud2 Change
      • ControllerServer New Parameter failure_tolerance
      • Removed BT XML Launch Configurations
      • Nav2 RViz Panel Action Feedback Information
    • Galactic to Humble
      • Major improvements to Smac Planners
      • Simple (Python) Commander
      • Reduce Nodes and Executors
      • API Change for nav2_core
      • Extending the BtServiceNode to process Service-Results
      • Including new Rotation Shim Controller Plugin
      • Spawning the robot in Gazebo
      • Recovery Behavior Timeout
      • New parameter use_final_approach_orientation for the 3 2D planners
      • SmacPlanner2D and Theta*: fix goal orientation being ignored
      • SmacPlanner2D, NavFn and Theta*: fix small path corner cases
      • Change and fix behavior of dynamic parameter change detection
      • Dynamic Parameters
      • BT Action Nodes Exception Changes
      • BT Navigator Groot Multiple Navigators
      • Removed Kinematic Limiting in RPP
      • Added Smoother Task Server
      • Removed Use Approach Velocity Scaling Param in RPP
      • Refactored AMCL motion models as plugins
      • Dropping Support for Live Groot Monitoring of Nav2
      • Replanning Only if Path is Invalid
      • Fix CostmapLayer clearArea invert param logic
      • Dynamic Composition
      • BT Cancel Node
      • BT PathLongerOnApproach Node
      • BT TruncatePathLocal Node
      • Constrained Smoother
      • Replanning at a Constant Rate and if the Path is Invalid
      • Euclidean Distance 2D
      • Recovery To Behavior
      • Respawn Support in Launch and Lifecycle Manager
      • New Nav2 Velocity Smoother
      • Goal Checker API Changed
      • Added Assisted Teleop
    • Humble to Iron
      • New Behavior-Tree Navigator Plugins
      • Added Collision Monitor
      • Removed use_sim_time from yaml
      • Run-time Speed up of Smac Planner
      • Recursive Refinement of Smac and Simple Smoothers
      • Simple Commander Python API
      • Smac Planner Start Pose Included in Path
      • Parameterizable Collision Checking in RPP
      • Expanded Planner Benchmark Tests
      • Smac Planner Path Tolerances
      • costmap_2d_node default constructor
      • Feedback for Navigation Failures
      • Costmap Filters
      • Savitzky-Golay Smoother
      • Changes to Map yaml file path for map_server node in Launch
      • SmootherSelector BT Node
      • Publish Costmap Layers
      • Give Behavior Server Access to Both Costmaps
      • New Model Predictive Path Integral Controller
      • Behavior Tree Uses Error Codes
      • Load, Save and Loop Waypoints from the Nav2 Panel in RViz
      • DWB Forward vs Reverse Pruning
      • More stable regulation on curves for long lookahead distances
  • Simple Commander API
    • Overview
    • Commander API
    • Costmap API
    • Footprint Collision Checker API
    • Examples and Demos
  • Roadmaps
    • Iron Roadmap
    • Humble Roadmap
  • Getting Involved
    • Getting Involved
    • Process
    • Licensing
    • Developer Certification of Origin (DCO)
  • About and Contact
    • Related Projects
    • ROS to ROS 2 Navigation
    • About
    • Contact
  • Robots Using
Navigation 2
Edit
  • General Tutorials
  • Navigating with Keepout Zones

Navigating with Keepout Zones¶

  • Overview

  • Requirements

  • Tutorial Steps

Overview¶

This tutorial shows how to simply utilize keep-out/safety zones where robots can’t enter and preferred lanes for robots moving in industrial environments and warehouses. All this functionality is being covered by KeepoutFilter costmap filter plugin which will be enabled and used in this document.

Requirements¶

It is assumed that ROS 2, Gazebo and TurtleBot3 packages are installed or built locally. Please make sure that Nav2 project is also built locally as it was made in Build and Install.

Tutorial Steps¶

1. Prepare filter mask¶

As was written in Navigation Concepts, any Costmap Filter (including Keepout Filter) are reading the data marked in a filter mask file. Filter mask - is the usual Nav2 2D-map distributed through PGM, PNG or BMP raster file with its metadata containing in a YAML file. The following steps help to understand how to make a new filter mask:

Create a new image with a PGM/PNG/BMP format: copy turtlebot3_world.pgm main map which will be used in a world simulation from a Nav2 repository to a new keepout_mask.pgm file.

Open keepout_mask.pgm in your favourite raster graphics editor (as an example could be taken GIMP editor). The lightness of each pixel on the mask means an encoded information for the specific costmap filter you are going to use. Color lightness of each pixel belongs to the [0..255] range (or [0..100] in percent scale), where 0 means black color and 255 - white. Another term “darkness” will be understood as the exact opposite of lightness. In other words color_darkness = 100% - color_lightness.

In the GIMP lightness is expressed through color components value (e.g. R in percent scale) and might be set by moving L slider in color changing tool:

../../_images/ligtness_in_GIMP.png

The incoming mask file is being read by the Map Server and converted into OccupancyGrid values from [0..100] range (where 0 means free cell, 100 - occupied, anything in between - less or more occupied cells on map) or be equal to -1 for unknown value. In Nav2 stack each map has mode attribute which could be trinary, scale or raw. Depending on mode selected, the color lightness of PGM/PNG/BMP is being converted to OccupancyGrid by one of the following principles:

  • trinary (default mode): Darkness >= occupied_thresh means that map occupied (100). Darkness <= free_thresh - map free (0). Anything in between - unknown status on map (-1).

  • scale: Alpha < 1.0 - unknown. Darkness >= occupied_thresh means that map occupied (100). Darkness <= free_thresh - map free (0). Anything in between - linearly interpolate to nearest integer from [0..100] range.

  • raw: Lightness = 0 (dark color) means that map is free (0). Lightness = 100 (in absolute value) - map is occupied (100). Anything in between - OccupancyGrid value = lightness. Lightness >= 101 - unknown (-1).

where free_thresh and occupied_thresh thresholds are expressed in percentage of maximum lightness/darkness level (255). Map mode and thresholds are placed in YAML metadata file (see below) as mode, free_thresh and occupied_thresh fields.

Note

There is another parameter in a YAML metadata file called negate. By default it is set to false. When it is set to true, blacker pixels will be considered as free, whiter pixels - as occupied. In this case we should count color lightness instead of darkness for trinary and scale modes. negate has no effect on raw mode.

For Keepout Filter OccupancyGrid value is proportional to the passibility of area corresponting to this cell: higher values means more impassable areas. Cells with occupied values covers keep-out zones where robot will never enter or pass through. KeepoutFilter can also act as a “weighted areas layer” by setting the OccupancyGrid to something between [1-99] non-occupied values. Robot is allowed to move in these areas, however its presence there would be “undesirable” there (the higher the value, the sooner planners will try to get the robot out of this area).

Keepout Filter also covers preferred lanes case, where robots should moving only on pre-defined lanes and permitted areas e.g. in warehouses. To use this feaure you need to prepare the mask image where the lanes and permitted areas will be marked with free values while all other areas will be occupied. TIP for drawing the mask in a trinary or scale mode: typically, amount of pixels belonging to lanes are much less than pixels covering other areas. In this case initially all lanes data might be drawn with a black pencil over white background and then (just before saving a PGM) “color inversion” tool in a image raster editor might be used.

For simplicity, in the example fill the areas with black color (in trinary mode this means occupied map) that you are going to mark as a keep-out zones:

../../_images/drawing_keepout_mask.png

After all keepout areas will be filled save the keepout_mask.pgm image.

Like all other maps, filter mask should have its own YAML metadata file. Copy turtlebot3_world.yaml to keepout_mask.yaml. Open keepout_mask.yaml and correct image field to a newly made PGM mask:

image: turtlebot3_world.pgm
->
image: keepout_mask.pgm

Since filter mask image was created as a copy of main map, other fields of YAML-file do not need to be changed. Save keepout_mask.yaml and new filter mask is ready to use.

Note

World map itself and filter mask could have different sizes, origin and resolution which might be useful e.g. for cases when filter mask is covering smaller areas on maps or when one filter mask is used repeatedly many times (like annotating a keepout zone for same shape rooms in the hotel). For this case, you need to correct resolution and origin fields in YAML as well so that the filter mask is correctly laid on top of the original map.

Note

Another important note is that since Costmap2D does not support orientation, the last third “yaw” component of the origin vector should be equal to zero. For example: origin: [1.25, -5.18, 0.0].

2. Configure Costmap Filter Info Publisher Server¶

Each costmap filter reads incoming meta-information (such as filter type or data conversion coefficients) in a messages of nav2_msgs/CostmapFilterInfo type. These messages are being published by Costmap Filter Info Publisher Server. The server is running as a lifecycle node. According to the design document, nav2_msgs/CostmapFilterInfo messages are going in a pair with OccupancyGrid filter mask topic. Therefore, along with Costmap Filter Info Publisher Server there should be enabled a new instance of Map Server configured to publish filter mask.

In order to enable Keepout Filter in your configuration, both servers should be enabled as a lifecycle nodes in Python launch-file. For example, this might look as follows:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml


def generate_launch_description():
    # Get the launch directory
    costmap_filters_demo_dir = get_package_share_directory('nav2_costmap_filters_demo')

    # Create our own temporary YAML files that include substitutions
    lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']

    # Parameters
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    autostart = LaunchConfiguration('autostart')
    params_file = LaunchConfiguration('params_file')
    mask_yaml_file = LaunchConfiguration('mask')

    # Declare the launch arguments
    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Top-level namespace')

    declare_use_sim_time_cmd = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true')

    declare_autostart_cmd = DeclareLaunchArgument(
        'autostart', default_value='true',
        description='Automatically startup the nav2 stack')

    declare_params_file_cmd = DeclareLaunchArgument(
            'params_file',
            default_value=os.path.join(costmap_filters_demo_dir, 'params', 'keepout_params.yaml'),
            description='Full path to the ROS 2 parameters file to use')

    declare_mask_yaml_file_cmd = DeclareLaunchArgument(
            'mask',
            default_value=os.path.join(costmap_filters_demo_dir, 'maps', 'keepout_mask.yaml'),
            description='Full path to filter mask yaml file to load')

    # Make re-written yaml
    param_substitutions = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file}

    configured_params = RewrittenYaml(
        source_file=params_file,
        root_key=namespace,
        param_rewrites=param_substitutions,
        convert_types=True)

    # Nodes launching commands
    start_lifecycle_manager_cmd = Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_costmap_filters',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': autostart},
                        {'node_names': lifecycle_nodes}])

    start_map_server_cmd = Node(
            package='nav2_map_server',
            executable='map_server',
            name='filter_mask_server',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[configured_params])

    start_costmap_filter_info_server_cmd = Node(
            package='nav2_map_server',
            executable='costmap_filter_info_server',
            name='costmap_filter_info_server',
            namespace=namespace,
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[configured_params])

    ld = LaunchDescription()

    ld.add_action(declare_namespace_cmd)
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_autostart_cmd)
    ld.add_action(declare_params_file_cmd)
    ld.add_action(declare_mask_yaml_file_cmd)

    ld.add_action(start_lifecycle_manager_cmd)
    ld.add_action(start_map_server_cmd)
    ld.add_action(start_costmap_filter_info_server_cmd)

    return ld

where the params_file variable should be set to a YAML-file having ROS parameters for Costmap Filter Info Publisher Server and Map Server nodes. These parameters and their meaning are listed at Map Server / Saver page. Please, refer to it for more information. The example of params_file could be found below:

costmap_filter_info_server:
  ros__parameters:
    use_sim_time: true
    type: 0
    filter_info_topic: "/costmap_filter_info"
    mask_topic: "/keepout_filter_mask"
    base: 0.0
    multiplier: 1.0
filter_mask_server:
  ros__parameters:
    use_sim_time: true
    frame_id: "map"
    topic_name: "/keepout_filter_mask"
    yaml_filename: "keepout_mask.yaml"

Note, that:

  • For Keepout Filter the type of costmap filter should be set to 0.

  • Filter mask topic name should be the equal for mask_topic parameter of Costmap Filter Info Publisher Server and topic_name parameter of Map Server.

  • According to the Costmap Filters design, OccupancyGrid values are being linearly transformed into feature map in a filter space. For a Keepout Filter these values are directly passed as a filter space values without a linear conversion. Even though base and multiplier coefficients are not used in Keepout Filter, they should be set to 0.0 and 1.0 accordingly in order to explicitly show that we have one-to-one conversion from OccupancyGrid values -> to a filter value space.

Ready-to-go standalone Python launch-script, YAML-file with ROS parameters and filter mask example for Keepout Filter could be found in a nav2_costmap_filters_demo directory of navigation2_tutorials repository. To simply run Filter Info Publisher Server and Map Server tuned on Turtlebot3 standard simulation written at Getting Started, build the demo and launch costmap_filter_info.launch.py as follows:

$ mkdir -p ~/tutorials_ws/src
$ cd ~/tutorials_ws/src
$ git clone https://github.com/ros-planning/navigation2_tutorials.git
$ cd ~/tutorials_ws
$ colcon build --symlink-install --packages-select nav2_costmap_filters_demo
$ source ~/tutorials_ws/install/setup.bash
$ ros2 launch nav2_costmap_filters_demo costmap_filter_info.launch.py params_file:=src/navigation2_tutorials/nav2_costmap_filters_demo/params/keepout_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/keepout_mask.yaml

3. Enable Keepout Filter¶

Costmap Filters are Costamp2D plugins. You can enable the KeepoutFilter plugin in Costmap2D by adding keepout_filter to the plugins parameter in nav2_params.yaml. You can place it in the global_costmap for planning with keepouts and local_costmap to make sure the robot won’t attempt to drive through a keepout zone. The KeepoutFilter plugin should have the following parameters defined:

  • plugin: type of plugin. In our case nav2_costmap_2d::KeepoutFilter.

  • filter_info_topic: filter info topic name. This need to be equal to filter_info_topic parameter of Costmap Filter Info Publisher Server from the chapter above.

Full list of parameters supported by KeepoutFilter are listed at Keepout Filter Parameters page.

It is important to note that enabling KeepoutFilter for global_costmap only will cause the path planner to build plans bypassing keepout zones. Enabling KeepoutFilter for local_costmap only will cause the robot to not enter keepout zones, but the path may still go through them. So, the best practice is to enable KeepoutFilter for global and local costmaps simultaneously by adding it both in global_costmap and local_costmap in nav2_params.yaml. However it does not always have to be true. In some cases keepout zones don’t have to be the same for global and local costmaps, e.g. if the robot doesn’t allowed to intentionally go inside keepout zones, but if its there, the robot can drive in and out really quick if it clips an edge or corner. For this case, there is not need to use extra resources of the local costmap copy.

To enable KeepoutFilter with same mask for both global and local costmaps, use the following configuration:

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      ...
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
...
local_costmap:
  local_costmap:
    ros__parameters:
      ...
      plugins: ["voxel_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      ...
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"

Note

All costmap filters should be enabled through a filters parameter – though it is technically possible to include in the layered costmap itself. This is separated from the layer plugins to prevent interference in the layers, particularly the inflation layer.

4. Run Nav2 stack¶

After Costmap Filter Info Publisher Server and Map Server were launched and Keepout Filter was enabled for global/local costmaps, run Nav2 stack as written in Getting Started:

ros2 launch nav2_bringup tb3_simulation_launch.py

And check that filter is working properly as in the pictures below (first picture shows keepout filter enabled for the global costmap, second - differently-sized keepout_mask.pgm filter mask):

../../_images/keepout_global.gif ../../_images/keepout_mask.png

© Copyright 2020.