Detected object not appearing in costmap - Navigation2

asked 2022-03-31 12:59:27 -0500

ccpjBoss
46 ●3 ●6 ●9

updated 2022-03-31 13:02:03 -0500

Required Info:

Operating System:
- Ubuntu 20.04
ROS2 Version:
- Galactic

Hello,

I am working with a multi-robot package, and I am facing the following problem: The robot's costmap does not reflect what the lidar data (see image).

I will place the output from ros2 param dump, below. Any help is hugely appreciated! I can also place the outup from ros2 topic list and the tf_tree if needed!

/robot_0/global_costmap/global_costmap:
  ros__parameters:
    /bond_disable_heartbeat_timeout: true
    always_send_full_costmap: true
    clearable_layers:
    - obstacle_layer
    - voxel_layer
    - range_layer
    filters: []
    footprint: '[]'
    footprint_padding: 0.01
    global_frame: map
    height: 5
    inflation_layer:
      cost_scaling_factor: 10.0
      enabled: true
      inflate_around_unknown: false
      inflate_unknown: false
      inflation_radius: 0.66
      plugin: nav2_costmap_2d::InflationLayer
    lethal_cost_threshold: 100
    map_topic: /robot_0/map
    observation_sources: scan
    obstacle_layer:
      combination_method: 1
      enabled: true
      footprint_clearing_enabled: true
      max_obstacle_height: 2.0
      observation_sources: scan
      plugin: nav2_costmap_2d::ObstacleLayer
      scan:
        clearing: true
        data_type: LaserScan
        expected_update_rate: 10.0
        inf_is_valid: false
        marking: true
        max_obstacle_height: 2.0
        min_obstacle_height: 0.0
        observation_persistence: 1.0
        obstacle_max_range: 3.5
        obstacle_min_range: 0.0
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        sensor_frame: robot_0/ranger_0/base_scan
        topic: /robot_0/ranger_0
    origin_x: 0.0
    origin_y: 0.0
    plugins:
    - static_layer
    - obstacle_layer
    - inflation_layer
    publish_frequency: 1.0
    qos_overrides:
      /clock:
        subscription:
          depth: 1
          durability: volatile
          history: keep_last
          reliability: best_effort
    resolution: 0.05
    robot_base_frame: robot_0/base_link
    robot_radius: 0.33
    rolling_window: false
    static_layer:
      enabled: true
      map_subscribe_transient_local: true
      map_topic: ''
      plugin: nav2_costmap_2d::StaticLayer
      subscribe_to_updates: true
      transform_tolerance: 0.0
    track_unknown_space: true
    transform_tolerance: 0.3
    trinary_costmap: true
    unknown_cost_value: 255
    update_frequency: 1.0
    use_maximum: false
    use_sim_time: true
    width: 5

/robot_0/local_costmap/local_costmap:
  ros__parameters:
    /bond_disable_heartbeat_timeout: true
    always_send_full_costmap: true
    clearable_layers:
    - obstacle_layer
    - voxel_layer
    - range_layer
    filters: []
    footprint: '[]'
    footprint_padding: 0.01
    global_frame: robot_0/odom
    height: 4
    inflation_layer:
      cost_scaling_factor: 10.0
      enabled: true
      inflate_around_unknown: false
      inflate_unknown: false
      inflation_radius: 0.33
      plugin: nav2_costmap_2d::InflationLayer
    lethal_cost_threshold: 100
    map_topic: /robot_0/map
    observation_sources: scan
    obstacle_layer:
      combination_method: 1
      enabled: true
      footprint_clearing_enabled: true
      max_obstacle_height: 2.0
      observation_sources: scan
      plugin: nav2_costmap_2d::ObstacleLayer
      scan:
        clearing: true
        data_type: LaserScan
        expected_update_rate: 0.0
        inf_is_valid: false
        marking: true
        max_obstacle_height: 2.0
        min_obstacle_height: 0.0
        observation_persistence: 1.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        sensor_frame: ''
        topic: /robot_0/ranger_0
    origin_x: 0.0
    origin_y: 0.0
    plugins:
    - obstacle_layer
    - inflation_layer
    publish_frequency: 2.0
    qos_overrides:
      /clock:
        subscription:
          depth: 1
          durability: volatile
          history: keep_last
          reliability: best_effort
    resolution: 0.05
    robot_base_frame: robot_0/base_link
    robot_radius: 0.4
    rolling_window: true
    track_unknown_space: false
    transform_tolerance: 0.3
    trinary_costmap: true
    unknown_cost_value: 255
    update_frequency: 5.0
    use_maximum: false
    use_sim_time: true
    width: 4

image description

answered 2022-03-31 15:49:47 -0500

stevemacenski

8272 ●34 ●503 ●129 https://www.linkedin.c...

It doesn't look like you updated your laser scan topic, from the costmap it doesn't look like any of the laser data is being used. I would check ros2 topic list and see what topic it is being published at and update your laser scan topic in your yaml file.

edit flag offensive delete link

Comments

If I do ros2 topic info /robot_0/ranger_0 -v, which is the scan topic being viewed on RViz I get that the local_costmap_rclcpp_node and the global_costmap_rclcpp_node and subscribed to it... So I assumed that they are getting the correct sensor data.

ccpjBoss ( 2022-04-01 04:43:58 -0500 )edit

Perhaps its being rejected, do you have your TF trees properly setup by REP 105? If you set the log level to debug, do you see that the TF subscriber buffer is throwing out results because it couldn't transform them?

stevemacenski ( 2022-04-01 12:43:56 -0500 )edit

I don't see any error message telling me that it could not transform. My tf_tree is as follows: map->robot_0/odom->robot_0/base_footprint->robot_0/base_link->robot_0/base_scan. Is it not properly setup?

ccpjBoss ( 2022-04-05 06:24:21 -0500 )edit

set the log level to debug, do you see anything from the TF subscriber buffer?

stevemacenski ( 2022-04-05 13:36:29 -0500 )edit

Here is what I have related to TF in the log files: controller_server log file:

[robot0.local_costmap.local_costmap]: Activating
[robot0.local_costmap.local_costmap]: Checking transform
[robot0.local_costmap.local_costmap]: start
[robot0.local_costmap.local_costmap]: mapUpdateLoop frequency: 5.000000
[robot0.local_costmap.local_costmap]: Entering loop
[tf2_ros_message_filter]: MessageFilter [target=robot0/odom ]: Cleared
[robot0.local_costmap.local_costmap]: Updating map...
[rcl]: Initializing subscription for topic name '/robot0/ranger_0'
[rcl]: Expanded and remapped topic name '/robot0/ranger_0'
[nav2_costmap_2d]: Updating area x: [0, 60] y: [0, 60]
[robot0.local_costmap.local_costmap]: Publishing footprint
[robot0.local_costmap.local_costmap]: Map update time: 0.000329028
[robot0.local_costmap.local_costmap]: Publish costmap at local_costmap

ccpjBoss ( 2022-04-06 05:05:38 -0500 )edit

I also see this message: [tf2_ros_message_filter]: MessageFilter [target=robot0/odom ]: Added message in frame robot0/base_scan at time 7.900, count now 2

And this:

[robot0.local_costmap.local_costmap]: Updating map...
[nav2_costmap_2d]: Updating area x: [13, 45] y: [13, 45]
[robot0.local_costmap.local_costmap]: Publishing footprint
[robot0.local_costmap.local_costmap]: Map update time: 0.000261250

I see this message more than once, although it always updates in the same x and y coordinates.

ccpjBoss ( 2022-04-06 05:07:44 -0500 )edit

Hi Steve! It's me again. Looking into the Multi Robot simulation from the nav2_bringup package, I see that the only difference from my node graph to yours is that the topic on which the transformations are published are namespaced (i.e. /robot0/tf, /robot1/tf,...). In my case, I have a global /tf topic. Do you think this might be causing the problem?

EDIT1: May I add that the planner works fine and all the robots are able to navigate to their goals. It's just the costmaps that are not receiving LaserScan information

ccpjBoss ( 2022-04-12 12:59:50 -0500 )edit

The only thing i notice is that you set the sensor_frame in the global_costmap, but not the local_costmap. Is the frame_id that your scan message publishes actually robot_0/ranger_0/base_scan or is it just base_scan?

Is that scan_frame actually correct regardless? Here: tf2_ros_message_filter]: MessageFilter [target=robot0/odom ]: Added message in frame robot0/base_scan at time 7.900, count now 2 you mention robot0/base_scan not robot_0/ranger_0/base_scan

ETA you seem inconsistent in your naming, sometimes using robot_0 and sometimes robot0. You might want to stick to one and make sure it's set properly everywhere.

Joe28965 ( 2022-04-13 04:33:16 -0500 )edit

see more comments

Detected object not appearing in costmap - Navigation2

1 Answer

Comments

Question Tools

Stats

Related questions

Detected object not appearing in costmap - Navigation2 edit

1 Answer

Comments

Question Tools

Stats

Related questions

Detected object not appearing in costmap - Navigation2