0
$\begingroup$

Rosanswers logo

Hi,

I'm using move_base_node in stage, and I've found that local costmap is empty. In this picture from rviz I can see that global costmap is fine but local costmap (the small square) is empty. The red dots are the laser scans from robot (green footprint) with which the local costmap should be generated. Is something wrong with this? Because although of this I can set goals and the robot can reach them.

EDIT 1:

Although the robot can reach the goals, it does it at a very low speed, even if I increase the value of max_vel_x parameter of local_planner. I don't know if this happens due to the lack of local_costmap.

EDIT 2:

If I change

local_costmap:
      observation_sources: base_scan
      base_scan: {
        topic: /car_0/base_scan
...

for

local_costmap:
          observation_sources: base_scan
          base_scan: {
            topic: /base_0/base_scan
    ...

where /base_0/base_scan is another LaserScan topic, some black areas start to appear in local_costmap until origin of sensor its outside it.

EDIT 3:

This is my TF tree; base_* are "robots" on each corner of the intersection; I say "robots" because I use them as static lasers. And car_* are robots navigating as vehicles, in this case car_0 is the green polygon.



image description

Here are the parameters I use:

costmap_common file:

#This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d

#For this example we'll configure the costmap in voxel-grid mode
map_type: voxel

#Voxel grid specific parameters
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 9
mark_threshold: 0

#Set the tolerance we're willing to have for tf transforms
transform_tolerance: 0.3

#Obstacle marking parameters
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0

#The footprint of the robot and associated padding
# included in both global and local costmaps params
#footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.3, 0.0], [0.25, -0.25]]
footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.3, 0.0], [0.25, -0.25]]
footprint_padding: 0.05

#Cost function parameters
# included in both global and local costmaps params
inflation_radius: 0.35
cost_scaling_factor: 10.0

#The cost at which a cell is considered an obstacle when a map is read from the map_server
lethal_cost_threshold: 100

#Configuration for the sensors that the costmap will use to update a map
# included in both global and local costmaps params
#observation_sources: base_scan
#base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
#  observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

global_costmap file:

#Independent settings for the global planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d

global_costmap:
  observation_sources: "base_0 base_1 base_2 base_3"
  base_0: {
    topic: /base_0/base_scan,
    data_type: LaserScan, 
    expected_update_rate: 0.4,
    observation_persistence: 0.0, 
    marking: true, 
    clearing: true, 
    max_obstacle_height: 0.4, 
    min_obstacle_height: 0.08
    }
  
  base_1: {
    topic: /base_1/base_scan,
    data_type: LaserScan, 
    expected_update_rate: 0.4,
    observation_persistence: 0.0, 
    marking: true, 
    clearing: true, 
    max_obstacle_height: 0.4, 
    min_obstacle_height: 0.08
    }
    
  base_2: {
    topic: /base_2/base_scan,
    data_type: LaserScan, 
    expected_update_rate: 0.4,
    observation_persistence: 0.0, 
    marking: true, 
    clearing: true, 
    max_obstacle_height: 0.4, 
    min_obstacle_height: 0.08
    }
    
  base_3: {
    topic: /base_3/base_scan,
    data_type: LaserScan, 
    expected_update_rate: 0.4,
    observation_persistence: 0.0, 
    marking: true, 
    clearing: true, 
    max_obstacle_height: 0.4, 
    min_obstacle_height: 0.08
    }
    
  #inflation_radius: 0.05
  #cost_scaling_factor: 10.0
  #Set the global and robot frames for the costmap
  global_frame: /map
  robot_base_frame: base_link

  #Set the update and publish frequency of the costmap
  update_frequency: 5.0
  publish_frequency: 2.0

  #We'll use a map served by the map_server to initialize this costmap
  static_map: true
  rolling_window: false

  footprint_padding: 0.02

local_costamp file:

#Independent settings for the local planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d

local_costmap:
  observation_sources: base_scan
  base_scan: {
    topic: /car_0/base_scan,
    data_type: LaserScan, 
    expected_update_rate: 0.4,
    observation_persistence: 0.0, 
    marking: true, 
    clearing: true, 
    max_obstacle_height: 0.4, 
    min_obstacle_height: 0.08
    }
  #The footprint of the robot and associated padding
  #footprint: [[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.3, 0.0], [0.2, -0.2]]
  #footprint_padding: 0.01
  #Cost function parameters
  #inflation_radius: 0.35
  #cost_scaling_factor: 10.0
  #We'll publish the voxel grid used by this costmap
  publish_voxel_map: true

  #Set the global and robot frames for the costmap
  global_frame: odom
  robot_base_frame: base_link

  #Set the update and publish frequency of the costmap
  update_frequency: 5.0
  publish_frequency: 2.0

  #We'll configure this costmap to be a rolling window... meaning it is always
  #centered at the robot
  static_map: false
  rolling_window: true
  width: 8.0
  height: 8.0
  resolution: 0.025
  origin_x: 0.0
  origin_y: 0.0

Thanks in advance


Originally posted by gustavo.velascoh on ROS Answers with karma: 756 on 2014-01-17

Post score: 1


Original comments

Comment by Tim Sweet on 2014-01-17:
What is the output of move_base? Any errors? Also, I'm not sure what the defined behavior is when you don't have a footprint defined (it's commented out in your file). Is it possible that your robot has no footprint, hence it can go anywhere?

Comment by gustavo.velascoh on 2014-01-17:
Hi @Tim-sweet, parameter in costmap_common file are common to global and local costmaps, there is the footprint: 'footprint: [[-0.25, -0.25], [-0.25, 0.25], [0.25, 0.25], [0.3, 0.0], [0.25, -0.25]]'

Comment by Tim Sweet on 2014-01-17:
Oh I missed that, my bad. Does move_base produce any errors? It might be helpful to set the logger level to debug (you can do that in rqt_console).

Comment by gustavo.velascoh on 2014-01-30:
move_base doesn't produce errors, just warnings about control rate.

Comment by David Lu on 2014-02-11:
What does your TF tree look like? Also are the /base_*/base_scan lasers not on the robot? I'm confused to your setup.

Comment by gustavo.velascoh on 2014-02-12:
HI @David_lu, I updated my question with a link to my TF tree and a brief explanation about my setup. Thanks

Comment by David Lu on 2014-02-12:
When you run the non-working configuration, rostopic info /car_0/base_scan show the proper connections?

Comment by gustavo.velascoh on 2014-02-12:
Yes, /car_0/base_scan is connected to /car_0/move_base_node. If you need to test the code is on https://www.github.com/gustavovelascoh/vintersim and https://www.github.com/gustavovelascoh/vinter_sim

Comment by Mani on 2014-09-30:
Any updates on this question? I am experiencing a similar issue.

Comment by David Lu on 2014-09-30:
I was never able to replicate the behavior, although I don't think I ever downloaded the sims.

Comment by Mani on 2014-10-01:
I am investigating a similar issue. I either will create another questions if I can't solve it or share the results here if it gets fixed.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I faced a similar issue in a much simpler setup. I found out ObstacleLayer of Costmap2D will not put any obstacles from a sensor in the costmap, if there is negative z-offset between the sensor_frame plane and map plane (assuming a planar lidar as sensor). I could get around this by setting a negative value for min_obstacle_height (less than the z offset between two planes).


Originally posted by Mani with karma: 1704 on 2014-10-02

This answer was ACCEPTED on the original site

Post score: 4


Original comments

Comment by Karsten on 2016-01-27:
You saved me! Ran into similar problems and it was exactly this negative z-offset! Thx

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.