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 Speed Limits

Navigating with Speed Limits¶

  • Overview

  • Requirements

  • Tutorial Steps

Overview¶

This tutorial shows how to simply utilize Speed Filter which is designed to limit the maximum speed of robots in speed restriction areas marked on a map. This functionality is being covered by SpeedFilter 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 the 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 Speed Filter) is reading the data marked in a filter mask file. All information about filter masks, their types, detailed structure and how to make a new one is written in a Navigating with Keepout Zones tutorial at 1. Prepare filter masks chapter. The principal of drawing the filter mask for Speed Filter is the same as for Keepout Filter (to annotate a map with the requested zones), except that OccupancyGrid mask values have another meaning: these values are encoded speed limits for the areas corresponding to the cell on map.

Let’s look, how it is being decoded. As we know, OccupancyGrid values are belonging to the [0..100] range. For Speed Filter 0 value means no speed limit in the area corresponding zero-cell on mask. Values from [1..100] range are being linearly converted into a speed limit value by the following formula:

speed_limit = filter_mask_data * multiplier + base;

where:

  • filter_mask_data - is an OccupancyGrid value of the corresponding cell on mask where maximum speed should be restricted.

  • base and multiplier are coefficients taken from nav2_msgs/CostmapFilterInfo messages published by Costmap Filter Info Server (see in next chapter below).

The decoded speed_limit value may have one of two meanings:

  • Speed limit expressed in a percent from maximum robot speed.

  • Speed limit expressed in absolute values (e.g. in m/s).

The meaning used by Speed Filter is being read from nav2_msgs/CostmapFilterInfo messages. In this tutorial we will use the first type of speed restriction expressed in a percent from maximum robot speed.

Note

For speed restriction expressed in a percent, speed_limit will be used exactly as a percent belonging to [0..100] range, not [0.0..1.0] range.

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 speed_mask.pgm file. Open speed_mask.pgm in your favourite raster graphics editor and fill speed restricted areas with grey colors. In our example darker colors will indicate areas with higher speed restriction:

../../_images/drawing_speed_mask.png

Area “A” is filled with 40% gray color, area “B” - with 70% gray, that means that speed restriction will take 100% - 40% = 60% in area “A” and 100% - 70% = 30% in area “B” from maximum speed value allowed for this robot. We will use scale map mode with no thresholds. In this mode darker colors will have higher OccupancyGrid values. E.g. for area “B” with 70% of gray OccupancyGrid data will be equal to 70. So in order to hit the target, we need to choose base = 100.0 and multiplier = -1.0. This will reverse the scale OccupancyGrid values to a desired one. No thresholds (free_thresh occupied_thresh) were chosen for the convenience in the yaml file: to have 1:1 full range conversion of lightness value from filter mask -> to speed restriction percent.

Note

It is typical but not a mandatory selection of base and multiplier. For example, you can choose map mode to be raw. In this case color lightness is being directly converted into OccupancyGrid values. For masks saved in a raw mode, base and multiplier will be equal to 0.0 and 1.0 accordingly.

Another important thing is that it is not necessary to use the whole [0..100] percent scale. base and multiplier coefficients could be chosen so that the speed restriction values would belong to somewhere in the middle of percent range. E.g. base = 40.0, multiplier = 0.1 will give speed restrictions from [40.0%..50.0%] range with a step of 0.1%. This might be useful for fine tuning.

After all speed restriction areas will be filled, save the speed_mask.pgm image.

Like all other maps, the filter mask should have its own YAML metadata file. Copy turtlebot3_world.yaml to speed_mask.yaml. Open speed_mask.yaml and update the fields as shown below (as mentioned before for the scale mode to use whole color lightness range there should be no thresholds: free_thresh = 0.0 and occupied_thresh = 1.0):

image: turtlebot3_world.pgm
->
image: speed_mask.pgm

mode: trinary
->
mode: scale

occupied_thresh: 0.65
free_thresh: 0.196
->
occupied_thresh: 1.0
free_thresh: 0.0

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]). Save speed_mask.yaml and the 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 speed restricted area 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. This example shows using the main map as a base, but that is not required.

2. Configure Costmap Filter Info Publisher Server¶

Each costmap filter reads incoming meta-information (such as filter type or data conversion coefficients) in 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 masks.

In order to enable Speed Filter in your configuration, both servers should be enabled as 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', 'speed_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', 'speed_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: 1
    filter_info_topic: "/costmap_filter_info"
    mask_topic: "/speed_filter_mask"
    base: 100.0
    multiplier: -1.0
filter_mask_server:
  ros__parameters:
    use_sim_time: true
    frame_id: "map"
    topic_name: "/speed_filter_mask"
    yaml_filename: "speed_mask.yaml"

Note, that:

  • For Speed Filter setting speed restrictions in a percent from maximum speed, the type of costmap filter should be set to 1. All possible costmap filter types could be found at Map Server / Saver page.

  • 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.

  • As was described in a previous chapter, base and multiplier should be set to 100.0 and -1.0 accordingly for the purposes of this tutorial example.

Ready-to-go standalone Python launch-script, YAML-file with ROS parameters and filter mask example for Speed 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/speed_params.yaml mask:=src/navigation2_tutorials/nav2_costmap_filters_demo/maps/speed_mask.yaml

3. Enable Speed Filter¶

Costmap Filters are Costamp2D plugins. You can enable the SpeedFilter plugin in Costmap2D by adding speed_filter to the plugins parameter in nav2_params.yaml. The Speed Filter plugin should have the following parameters defined:

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

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

  • speed_limit_topic: name of topic to publish speed limit to.

Full list of parameters supported by SpeedFilter are listed at the Speed Filter Parameters page.

You can place the plugin either in the global_costmap section in nav2_params.yaml to have speed restriction mask applied to global costmap or in the local_costmap to apply speed mask to the local costmap. However, SpeedFilter plugin should never be enabled simultaneously for global and local costmaps. Otherwise, it can lead to unwanted multiple “speed restriction” - “no restriction” message chains on speed restriction boundaries, that will cause jerking of the robot or another unpredictable behaviour.

In this tutorial, we will enable Speed Filter for the global costmap. For this use the following configuration:

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["speed_filter"]
      ...
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
        speed_limit_topic: "/speed_limit"

As stated in the design, Speed Filter publishes speed restricting messages targeted for a Controller Server so that it could restrict maximum speed of the robot when it needed. Controller Server has a speed_limit_topic ROS parameter for that, which should be set to the same as in speed_filter plugin value. This topic in the map server could also be used to any number of other speed-restricted applications beyond the speed limiting zones, such as dynamically adjusting maximum speed by payload mass.

Set speed_limit_topic parameter of a Controller Server to the same value as it set for speed_filter plugin:

controller_server:
  ros__parameters:
    ...
    speed_limit_topic: "/speed_limit"

4. Run Nav2 stack¶

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

ros2 launch nav2_bringup tb3_simulation_launch.py

For better visualization of speed filter mask, in RViz in the left Displays pane unfold Map and change Topic from /map -> to /speed_filter_mask. Set the goal behind the speed restriction areas and check that the filter is working properly: robot should slow down when going through a speed restricting areas. Below is how it might look (first picture shows speed filter enabled for the global costmap, second - speed_mask.pgm filter mask):

../../_images/speed_global.gif ../../_images/speed_mask.png

© Copyright 2020.