Ask Your Question
2

Navigation 2: Sensor origin out of map bounds. The costmap can't raytrace for it.

asked 2021-08-23 11:24:35 -0500

Mk-_- gravatar image

updated 2021-08-24 11:23:04 -0500

Hi,

I've been trying to get nav2 to run for quite some time and am now facing a problem.

I am using foxy and the the navigation packages released via debian packages.

After launching slam and the base code of my physical robot, I see a map and the robot model https://drive.google.com/file/d/1D2A0...

If I now start nav2 (ros2 launch nav2_bringup navigation_launch.py use_sim_time:=False params_file:=/home/config/nav2_params.yaml) nav2 starts to run and I can also see a global costmap. However, the local costmap is not filled and the robot does not respond to any nav 2d goals. Also nothing is published from the controll server on the topic /cmd_vel.

The terminal repeatly prints:

[controller_server-1] [WARN] [1629735747.578710511] [local_costmap.local_costmap]: Sensor origin: (-0.01, -0.04, 0.80), out of map bounds. The costmap can't raytrace for it.

The used configuration is identical to the example from nav2_bringup, only I set "use_sime_time" to "false" everywhere.

Thank you in advance

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-08-26 07:46:19 -0500

Per Edwardsson gravatar image

Based on your nav2 settings from your github issue (https://github.com/ros-planning/navig...), it looks to me like the sensor data is not set up correctly. Maps don't make anything by themselves, they use plugins to fill the data, which in turn use sensor data to fill plugins. The total result is the stack of data of all plugins, from any sensor input. It's a very elegant system imo, but requires some setup.

It looks to me like you have a lidar on your robot which you want to use for mapping. Since the global map seems to work, we can use this as a template and work from there.

In your local costmap, the plugins are a VoxelLayer and an inflation layer. Voxels are used for 3d mapping, and I would think that you are looking for an ObstacleLayer, same as you use for your global map. You can copy the obstacle layer from your global costmap onto your local costmap, and remove the voxel layer. I suggest the following:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: False
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 1.0
      plugins: ["obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
       obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
         topic: /scan
         max_obstacle_height: 2.0
         clearing: True
         marking: True
         data_type: "LaserScan"
     static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

The local costmap is necessary for autonomous driving, so I hope that you can get your robot working with this fix.

edit flag offensive delete link more

Comments

Thank you so much.

Unfortunately I misunderstood the documentation

Mk-_- gravatar image Mk-_-  ( 2021-08-26 10:05:12 -0500 )edit

I'm not fully sure if I got the solution. Asking because I'm having the same issue working with a Turtlebot3 robot

Gianstiff gravatar image Gianstiff  ( 2022-02-25 05:37:09 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-08-23 11:24:35 -0500

Seen: 550 times

Last updated: Aug 26 '21