search

ros-navigation / navigation2_tutorials

Tutorial code referenced in https://docs.nav2.org/
180 stars 124 forks source link

Activating keepout and speed filter together. #66

Closed dharanish-nh closed 1 year ago

dharanish-nh commented 1 year ago

Hello,

I'm working on a project that requires the use of two costmap filters at the same time: the Keepout filter and the Speed filter. I've been looking through the documentation and I'm having trouble figuring out how to activate both filters at once. Could someone please provide guidance on how to do this?

Thank you for your help.

I tried to make some changes to the code however it does not work.

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_keepout = LaunchConfiguration('params_file_keepout')
    params_file_speed = LaunchConfiguration('params_file_speed')
    mask_yaml_file_keepout = LaunchConfiguration('mask_keepout')
    mask_yaml_file_speed = LaunchConfiguration('mask_speed')

    # 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_keepout_cmd = DeclareLaunchArgument(
            'params_file_keepout',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'params', 'keepout_params.yaml'),
            description='Full path to the ROS2 parameters file for the Keepout filter')

    declare_params_file_speed_cmd = DeclareLaunchArgument(
            'params_file_speed',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'params', 'speed_params.yaml'),
            description='Full path to the ROS2 parameters file for the Speed filter')

    declare_mask_yaml_file_keepout_cmd = DeclareLaunchArgument(
            'mask_keepout',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'maps', 'fullmap_keepout.yaml'),
            description='Full path to filter mask yaml file to load for the Keepout filter')

    declare_mask_yaml_file_speed_cmd = DeclareLaunchArgument(
            'mask_speed',
            default_value=os.path.join(get_package_share_directory('nav2_costmap_filters_demo'), 'maps', 'speed.yaml'),
            description='Full path to filter mask yaml file to load for the Speed filter')

    # Make re-written yaml for Keepout filter
    param_substitutions_keepout = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file_keepout}

    configured_params_keepout = RewrittenYaml(
        source_file=params_file_keepout,
        root_key=namespace,
        param_rewrites=param_substitutions_keepout,
        convert_types=True)

    # Make re-written yaml for Speed filter
    param_substitutions_speed = {
        'use_sim_time': use_sim_time,
        'yaml_filename': mask_yaml_file_speed}

    configured_params_speed = RewrittenYaml(
        source_file=params_file_speed,
        root_key=namespace,
        param_rewrites=param_substitutions_speed,
        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_keepout])

    start_costmap_filter_info_server_cmd = Node(
        package='nav2_map_server',
        executable='costmap_filter_info_server',
        name='costmap_filter_info_server_1',
        namespace=namespace,
        output='screen',
        emulate_tty=True,
        parameters=[configured_params_speed])

    start_costmap_filter_info_server_cmd2 = Node(
        package='nav2_map_server',
        executable='costmap_filter_info_server',
        name='costmap_filter_info_server_2',
        namespace=namespace,
        output='screen',
        emulate_tty=True,
        parameters=[configured_params_keepout])

    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_keepout_cmd)
    ld.add_action(declare_params_file_speed_cmd)
    ld.add_action(declare_mask_yaml_file_keepout_cmd)
    ld.add_action(declare_mask_yaml_file_speed_cmd)

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

    return ld

running the above code give the below output

ros2 launch nav2_costmap_filters_demo costmap_filter_info_2.launch.py                                                                                     
[INFO] [launch]: All log files can be found below /home/bot/.ros/log/2023-05-04-01-30-16-163378-bot-22263
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [lifecycle_manager-1]: process started with pid [22264]
[INFO] [map_server-2]: process started with pid [22266]
[INFO] [costmap_filter_info_server-3]: process started with pid [22268]
[INFO] [costmap_filter_info_server-4]: process started with pid [22270]
[lifecycle_manager-1] [INFO] [1683156616.663989370] [lifecycle_manager_costmap_filters]: Creating
[costmap_filter_info_server-4] [INFO] [1683156616.667593196] [costmap_filter_info_server]: This is costmap filter info publisher
[costmap_filter_info_server-3] [INFO] [1683156616.668954028] [costmap_filter_info_server]: This is costmap filter info publisher
[costmap_filter_info_server-4] [INFO] [1683156616.676255188] [costmap_filter_info_server_2]: 
[costmap_filter_info_server-4]  costmap_filter_info_server_2 lifecycle node launched. 
[costmap_filter_info_server-4]  Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-4]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-2] [INFO] [1683156616.687254944] [filter_mask_server]: 
[map_server-2]  filter_mask_server lifecycle node launched. 
[map_server-2]  Waiting on external lifecycle transitions to activate
[map_server-2]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-2] [INFO] [1683156616.687365271] [filter_mask_server]: Creating
[costmap_filter_info_server-3] [INFO] [1683156616.746267633] [costmap_filter_info_server_1]: 
[costmap_filter_info_server-3]  costmap_filter_info_server_1 lifecycle node launched. 
[costmap_filter_info_server-3]  Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-3]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-1] [INFO] [1683156616.773070866] [lifecycle_manager_costmap_filters]: Creating and initializing lifecycle service clients
[lifecycle_manager-1] [INFO] [1683156618.847922028] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156620.848138293] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156622.848298710] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156624.848502387] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156626.848733649] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156628.848942898] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156630.849106236] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156632.849306747] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156634.849465813] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
[lifecycle_manager-1] [INFO] [1683156636.849632568] [lifecycle_manager_costmap_filters]: Waiting for service costmap_filter_info_server/get_state...
SteveMacenski commented 1 year ago

Simply follow the instructions for both individually, they should not conflict in any way ( @AlexeyMerzlyakov )

AlexeyMerzlyakov commented 1 year ago

Most probably, the problem is that lifecycle manager does not know about two simultaneously running filter info servers, and thus can not change their state which seem in logs:

lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server']

Try to change this to:

lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server_1', 'costmap_filter_info_server_2']
dharanish-nh commented 1 year ago

I made those changes to the code. The code works however I am not receiving the map. Again if run them individually it works. should I make any changes to the YAML files as of now I have not made any changes.

output log of costmap_filter_info_2.launch.py

ros2 launch nav2_costmap_filters_demo costmap_filter_info_2.launch.py 
[INFO] [launch]: All log files can be found below /home/bot/.ros/log/2023-05-04-13-30-51-100544-bot-28556
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [lifecycle_manager-1]: process started with pid [28560]
[INFO] [map_server-2]: process started with pid [28562]
[INFO] [costmap_filter_info_server-3]: process started with pid [28564]
[INFO] [costmap_filter_info_server-4]: process started with pid [28566]
[lifecycle_manager-1] [INFO] [1683199851.312745679] [lifecycle_manager_costmap_filters]: Creating
[lifecycle_manager-1] [INFO] [1683199851.314517811] [lifecycle_manager_costmap_filters]: Creating and initializing lifecycle service clients
[costmap_filter_info_server-3] [INFO] [1683199851.317287383] [costmap_filter_info_server]: This is costmap filter info publisher
[costmap_filter_info_server-4] [INFO] [1683199851.318580692] [costmap_filter_info_server]: This is costmap filter info publisher
[map_server-2] [INFO] [1683199851.326218103] [filter_mask_server]: 
[map_server-2]  filter_mask_server lifecycle node launched. 
[map_server-2]  Waiting on external lifecycle transitions to activate
[map_server-2]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_server-2] [INFO] [1683199851.326313268] [filter_mask_server]: Creating
[costmap_filter_info_server-3] [INFO] [1683199851.327711099] [costmap_filter_info_server_1]: 
[costmap_filter_info_server-3]  costmap_filter_info_server_1 lifecycle node launched. 
[costmap_filter_info_server-3]  Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-3]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[costmap_filter_info_server-4] [INFO] [1683199851.330852765] [costmap_filter_info_server_2]: 
[costmap_filter_info_server-4]  costmap_filter_info_server_2 lifecycle node launched. 
[costmap_filter_info_server-4]  Waiting on external lifecycle transitions to activate
[costmap_filter_info_server-4]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-1] [INFO] [1683199851.519095705] [lifecycle_manager_costmap_filters]: Starting managed nodes bringup...
[lifecycle_manager-1] [INFO] [1683199851.519176336] [lifecycle_manager_costmap_filters]: Configuring filter_mask_server
[map_server-2] [INFO] [1683199851.519815644] [filter_mask_server]: Configuring
[map_server-2] [INFO] [map_io]: Loading yaml file: /home/bot/tutorials_ws/install/nav2_costmap_filters_demo/share/nav2_costmap_filters_demo/maps/fullmap_keepout.yaml
[map_server-2] [DEBUG] [map_io]: resolution: 0.05
[map_server-2] [DEBUG] [map_io]: origin[0]: -26.8
[map_server-2] [DEBUG] [map_io]: origin[1]: -3.56
[map_server-2] [DEBUG] [map_io]: origin[2]: 0
[map_server-2] [DEBUG] [map_io]: free_thresh: 0.196
[map_server-2] [DEBUG] [map_io]: occupied_thresh: 0.65
[map_server-2] [DEBUG] [map_io]: mode: trinary
[map_server-2] [DEBUG] [map_io]: negate: 0
[map_server-2] [INFO] [map_io]: Loading image_file: /home/bot/tutorials_ws/install/nav2_costmap_filters_demo/share/nav2_costmap_filters_demo/maps/restricted_zone.pgm
[map_server-2] [DEBUG] [map_io]: Read map /home/bot/tutorials_ws/install/nav2_costmap_filters_demo/share/nav2_costmap_filters_demo/maps/restricted_zone.pgm: 1207 X 591 map @ 0.05 m/cell
[lifecycle_manager-1] [INFO] [1683199851.652817587] [lifecycle_manager_costmap_filters]: Configuring costmap_filter_info_server_1
[costmap_filter_info_server-3] [INFO] [1683199851.653118615] [costmap_filter_info_server_1]: Configuring
[lifecycle_manager-1] [INFO] [1683199851.655315007] [lifecycle_manager_costmap_filters]: Configuring costmap_filter_info_server_2
[costmap_filter_info_server-4] [INFO] [1683199851.655602376] [costmap_filter_info_server_2]: Configuring
[lifecycle_manager-1] [INFO] [1683199851.658408431] [lifecycle_manager_costmap_filters]: Activating filter_mask_server
[map_server-2] [INFO] [1683199851.658632930] [filter_mask_server]: Activating
[map_server-2] [INFO] [1683199851.659224932] [filter_mask_server]: Creating bond (filter_mask_server) to lifecycle manager.
[lifecycle_manager-1] [INFO] [1683199851.762686287] [lifecycle_manager_costmap_filters]: Server filter_mask_server connected with bond.
[lifecycle_manager-1] [INFO] [1683199851.762727316] [lifecycle_manager_costmap_filters]: Activating costmap_filter_info_server_1
[costmap_filter_info_server-3] [INFO] [1683199851.763032972] [costmap_filter_info_server_1]: Activating
[costmap_filter_info_server-3] [INFO] [1683199851.763110993] [costmap_filter_info_server_1]: Creating bond (costmap_filter_info_server_1) to lifecycle manager.
[lifecycle_manager-1] [INFO] [1683199851.867724324] [lifecycle_manager_costmap_filters]: Server costmap_filter_info_server_1 connected with bond.
[lifecycle_manager-1] [INFO] [1683199851.867771251] [lifecycle_manager_costmap_filters]: Activating costmap_filter_info_server_2
[costmap_filter_info_server-4] [INFO] [1683199851.868342757] [costmap_filter_info_server_2]: Activating
[costmap_filter_info_server-4] [INFO] [1683199851.868430797] [costmap_filter_info_server_2]: Creating bond (costmap_filter_info_server_2) to lifecycle manager.
[lifecycle_manager-1] [INFO] [1683199851.973993801] [lifecycle_manager_costmap_filters]: Server costmap_filter_info_server_2 connected with bond.
[lifecycle_manager-1] [INFO] [1683199851.974034737] [lifecycle_manager_costmap_filters]: Managed nodes are active
[lifecycle_manager-1] [INFO] [1683199851.974052180] [lifecycle_manager_costmap_filters]: Creating bond timer...

partial output log of nav2

[controller_server-18] [INFO] [1683199843.254859347] [local_costmap.local_costmap]: start
[controller_server-18] [INFO] [1683199843.255343592] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[controller_server-18] [INFO] [1683199843.305885195] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[INFO] [spawner-8]: process has finished cleanly [pid 28056]
[INFO] [spawner-4]: process has finished cleanly [pid 28046]
[lifecycle_manager-25] [INFO] [1683199843.408933167] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-25] [INFO] [1683199843.408979985] [lifecycle_manager_navigation]: Activating smoother_server
[smoother_server-19] [INFO] [1683199843.409252247] [smoother_server]: Activating
[smoother_server-19] [INFO] [1683199843.409288466] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
[INFO] [spawner-9]: process has finished cleanly [pid 28061]
[INFO] [spawner-5]: process has finished cleanly [pid 28048]
[INFO] [spawner-7]: process has finished cleanly [pid 28053]
[lifecycle_manager-25] [INFO] [1683199843.512089567] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
[lifecycle_manager-25] [INFO] [1683199843.512129360] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-20] [INFO] [1683199843.512418734] [planner_server]: Activating
[planner_server-20] [INFO] [1683199843.512462528] [global_costmap.global_costmap]: Activating
[planner_server-20] [INFO] [1683199843.512480634] [global_costmap.global_costmap]: Checking transform
[planner_server-20] [INFO] [1683199843.512540766] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 0.560000 but the earliest data is at time 1.319000, when looking up transform from frame [base_link] to frame [map]
[planner_server-20] [INFO] [1683199844.012572542] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Lookup would require extrapolation into the past.  Requested time 1.040000 but the earliest data is at time 1.319000, when looking up transform from frame [base_link] to frame [map]
[amcl-16] [INFO] [1683199844.083085860] [amcl]: Message Filter dropping message: frame 'base_laser_link' at time 0.127 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199844.092781163] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.127 for reason 'discarding message because the queue is full'
[amcl-16] [INFO] [1683199844.183792279] [amcl]: Message Filter dropping message: frame 'base_laser_link' at time 0.204 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199844.188933641] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.204 for reason 'discarding message because the queue is full'
[rviz2-26] [INFO] [1683199844.316104098] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.319 for reason 'discarding message because the queue is full'
[rviz2-26] [INFO] [1683199844.412180496] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.415 for reason 'discarding message because the queue is full'
[planner_server-20] [INFO] [1683199844.512617097] [global_costmap.global_costmap]: start
[planner_server-20] [INFO] [1683199844.513039475] [global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[planner_server-20] [INFO] [1683199844.513554580] [global_costmap.global_costmap]: SpeedFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[rviz2-26] [INFO] [1683199844.893623324] [rviz2]: Trying to create a map of size 80 x 80 using 1 swatches
[controller_server-18] [WARN] [1683199845.061240908] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199845.531435601] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199845.531517133] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[planner_server-20] [INFO] [1683199845.564278911] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[planner_server-20] [INFO] [1683199845.564812490] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[rviz2-26] [INFO] [1683199845.566532733] [rviz2]: Trying to create a map of size 1207 x 591 using 1 swatches
[rviz2-26] [INFO] [1683199845.569413534] [rviz2]: Trying to create a map of size 1207 x 591 using 1 swatches
[lifecycle_manager-25] [INFO] [1683199845.667523302] [lifecycle_manager_navigation]: Server planner_server connected with bond.
[lifecycle_manager-25] [INFO] [1683199845.667563851] [lifecycle_manager_navigation]: Activating behavior_server
[behavior_server-21] [INFO] [1683199845.667867812] [behavior_server]: Activating
[behavior_server-21] [INFO] [1683199845.667897702] [behavior_server]: Activating assisted_teleop
[behavior_server-21] [INFO] [1683199845.667916931] [behavior_server]: Activating spin
[behavior_server-21] [INFO] [1683199845.667935262] [behavior_server]: Activating backup
[behavior_server-21] [INFO] [1683199845.667951418] [behavior_server]: Activating drive_on_heading
[behavior_server-21] [INFO] [1683199845.667967203] [behavior_server]: Activating wait
[behavior_server-21] [INFO] [1683199845.667985804] [behavior_server]: Creating bond (behavior_server) to lifecycle manager.
[lifecycle_manager-25] [INFO] [1683199845.770628113] [lifecycle_manager_navigation]: Server behavior_server connected with bond.
[lifecycle_manager-25] [INFO] [1683199845.770664356] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-22] [INFO] [1683199845.770965171] [bt_navigator]: Activating
[bt_navigator-22] [INFO] [1683199845.818738010] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-25] [INFO] [1683199845.922000508] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
[lifecycle_manager-25] [INFO] [1683199845.922038972] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-23] [INFO] [1683199845.922363316] [waypoint_follower]: Activating
[waypoint_follower-23] [INFO] [1683199845.922404948] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
[lifecycle_manager-25] [INFO] [1683199846.025118482] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
[lifecycle_manager-25] [INFO] [1683199846.025179561] [lifecycle_manager_navigation]: Activating velocity_smoother
[velocity_smoother-24] [INFO] [1683199846.025475462] [velocity_smoother]: Activating
[velocity_smoother-24] [INFO] [1683199846.025528597] [velocity_smoother]: Creating bond (velocity_smoother) to lifecycle manager.
[lifecycle_manager-25] [INFO] [1683199846.128683773] [lifecycle_manager_navigation]: Server velocity_smoother connected with bond.
[lifecycle_manager-25] [INFO] [1683199846.128719895] [lifecycle_manager_navigation]: Managed nodes are active
[lifecycle_manager-25] [INFO] [1683199846.128738606] [lifecycle_manager_navigation]: Creating bond timer...
[controller_server-18] [WARN] [1683199847.261242606] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199848.513243074] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199848.513304697] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199849.461361343] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199851.513274821] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199851.513323552] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199851.661419911] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[controller_server-18] [INFO] [1683199851.763161665] [local_costmap.local_costmap]: KeepoutFilter: Received filter info from /costmap_filter_info topic.
[controller_server-18] [INFO] [1683199851.763201531] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "filter_mask" topic for filter mask...
[planner_server-20] [INFO] [1683199851.763781454] [global_costmap.global_costmap]: KeepoutFilter: Received filter info from /costmap_filter_info topic.
[planner_server-20] [INFO] [1683199851.763820271] [global_costmap.global_costmap]: KeepoutFilter: Subscribing to "filter_mask" topic for filter mask...
[planner_server-20] [INFO] [1683199851.764266507] [global_costmap.global_costmap]: SpeedFilter: Received filter info from /costmap_filter_info topic.
[planner_server-20] [ERROR] [1683199851.764290021] [global_costmap.global_costmap]: SpeedFilter: Mode is not supported
[controller_server-18] [WARN] [1683199851.868466904] [local_costmap.local_costmap]: KeepoutFilter: New costmap filter info arrived from /costmap_filter_info topic. Updating old filter info.
[planner_server-20] [WARN] [1683199851.868548125] [global_costmap.global_costmap]: KeepoutFilter: New costmap filter info arrived from /costmap_filter_info topic. Updating old filter info.
[planner_server-20] [INFO] [1683199851.869115094] [global_costmap.global_costmap]: KeepoutFilter: Subscribing to "filter_mask" topic for filter mask...
[planner_server-20] [INFO] [1683199851.869486690] [global_costmap.global_costmap]: SpeedFilter: Received filter info from /costmap_filter_info topic.
[planner_server-20] [ERROR] [1683199851.869508402] [global_costmap.global_costmap]: SpeedFilter: Mode is not supported
[controller_server-18] [INFO] [1683199851.872762199] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "filter_mask" topic for filter mask...
[rviz2-26] [INFO] [1683199852.710419254] [rviz]: Message Filter dropping message: frame 'odom' at time 0.320 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199852.811376046] [rviz]: Message Filter dropping message: frame 'odom' at time 0.500 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.018758316] [rviz]: Message Filter dropping message: frame 'odom' at time 0.636 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.018834009] [rviz]: Message Filter dropping message: frame 'odom' at time 0.700 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.224561329] [rviz]: Message Filter dropping message: frame 'odom' at time 0.900 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.437909621] [rviz]: Message Filter dropping message: frame 'odom' at time 1.100 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.650208907] [rviz]: Message Filter dropping message: frame 'odom' at time 1.280 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.650298149] [rviz]: Message Filter dropping message: frame 'odom' at time 1.223 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199853.852129157] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.518 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[controller_server-18] [WARN] [1683199853.861691999] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[rviz2-26] [INFO] [1683199853.955424813] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.617 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.059346917] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.713 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.164293215] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.814 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.268034345] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 0.916 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.370942077] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 1.013 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.474231104] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 1.114 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[planner_server-20] [WARN] [1683199854.513218244] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199854.513255503] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[rviz2-26] [INFO] [1683199854.577602530] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 1.216 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-26] [INFO] [1683199854.679869386] [rviz]: Message Filter dropping message: frame 'base_laser_link' at time 1.318 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[controller_server-18] [WARN] [1683199856.061494953] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199857.513199976] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199857.513239553] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199858.261332273] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199860.461346685] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199860.513390991] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199860.513435528] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199862.661251778] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199863.513294699] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199863.513337844] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
[controller_server-18] [WARN] [1683199864.861485278] [local_costmap.local_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199866.513113897] [global_costmap.global_costmap]: KeepoutFilter: Filter mask was not received
[planner_server-20] [WARN] [1683199866.513147583] [global_costmap.global_costmap]: SpeedFilter: Filter mask was not received
AlexeyMerzlyakov commented 1 year ago

@dharanish-nh, you have underconfigured system, therefore filters are not operating well. In order to make speed and keepout filters are working altogether, please make sure that:

  1. Along with two costmap filters info server in the nav2_costmap_filters_demo/launch/costmap_filter_info.launch.py there are two different map servers are also set (which I did not seen in the launch-file from above). The rule is simple: one costmap filter info and one map server per each cosmap filter. Please also make sure, that their names are also different, to keep namespaces to be consistent.
  2. You have correctly configured both params/speed_params.yaml and params/keepout_params.yaml params-files setting proper namespaces for each costmap filter info and map server.

  3. In nav2_bringup/params/nav2_params.yaml both filters were set and configured correctly each one for its own filter info topic:

    diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 019bbb39..bf32a249 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -116,6 +116,7 @@ controller_server:
 min_y_velocity_threshold: 0.5
 min_theta_velocity_threshold: 0.001
 failure_tolerance: 0.3
+    speed_limit_topic: "/speed_limit"
 progress_checker_plugin: "progress_checker"
 goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
 controller_plugins: ["FollowPath"]
@@ -232,6 +233,7 @@ global_costmap:
   resolution: 0.05
   track_unknown_space: true
   plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      filters: ["keepout_filter", "speed_filter"]
   obstacle_layer:
     plugin: "nav2_costmap_2d::ObstacleLayer"
     enabled: True
@@ -253,6 +255,15 @@ global_costmap:
     plugin: "nav2_costmap_2d::InflationLayer"
     cost_scaling_factor: 3.0
     inflation_radius: 0.55
+      keepout_filter:
+        plugin: "nav2_costmap_2d::KeepoutFilter"
+        enabled: True
+        filter_info_topic: "/keepout_filter_info"
+      speed_filter:
+        plugin: "nav2_costmap_2d::SpeedFilter"
+        enabled: True
+        filter_info_topic: "/speed_filter_info"
+        speed_limit_topic: "/speed_limit"
   always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.

For the reference, attaching zipped demonstration sources that work fine for me: nav2_costmap_filters_demo.zip

I've enabled keepout and speed filter simultaneously for the global_costmap and they did work altogether as follows from the picture below: Screenshot_2023-05-05_09-05-43_cr

dharanish-nh commented 1 year ago

Thank you for all the help. It works for me as well. But I have observed one behavior when implementing keepout filter. While planning its path, if the path generated is close to the keepout zone, there is higher chance robot base gets stuck in the zone and causes the robot to stop navigation. How can i improve to avoid this situation?

filter_error