Rosanswers logo

Hi everyone,

I am converting the ultrasound sensors data to laser scan and it is used to update the local costmap. The obstacles are not cleared completely even when the obstacles moves out of frame. The ultrasound sensor readings are giving the correct data that there is no obstacle but in the costmap at some places the obstacles are not getting cleared. I went through the raytrace line code and everything seems correct.

This is the video link which shows the scenario: https://youtu.be/y8qTmAwNyMA

I am including my local_costmap_params.yaml :

  #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_combined
  robot_base_frame: base_link

  #Set the update and publish frequency of the costmap
  update_frequency: 10.0
  publish_frequency: 4.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
  always_send_full_costmap: true
  track_unknown_space: false
  width: 7.0
  height: 7.0
  resolution: 0.025
  origin_x: 0.0
  origin_y: 0.0
  inflation_radius: 0.90

  # - {name: obstacle_layer1, type: "costmap_2d::ObstacleLayer"}
  - {name: obstacle_layer2, type: "costmap_2d::ObstacleLayer"}
  # - {name: obstacle_layer3, type: "costmap_2d::ObstacleLayer"}
  # - {name: obstacle_layer4, type: "costmap_2d::ObstacleLayer"}
  # - {name: obstacle_layer5, type: "costmap_2d::ObstacleLayer"}
  #- {name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

# #Configuration for the sensors that the costmap will use to update a map
    observation_sources: US_FRT_laser_scan 

US_FRT_laser_scan: {data_type: LaserScan, sensor_frame: /US_FRT_frame, topic: /US_FRT, expected_update_rate: 0.4,
      observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 2.0, min_obstacle_height: 0.0, inf_is_valid: true} #,raytrace_range: 3.5, obstacle_range: 3.3}

Please check the video and suggest something. I am stuck with this problem from a long time. I tried almost everything. So someone please look into this problem. Thanks in Advance

Originally posted by Dharmateja Kadem on ROS Answers with karma: 190 on 2017-03-17

Post score: 3

Original comments

Comment by Humpelstilzchen on 2017-03-18:
Could it be that, when the object is out of range your sensor value is > max_range ?

Comment by Dharmateja Kadem on 2017-03-19:
I checked that, the sensor value is not greater than max_range

Comment by Humpelstilzchen on 2017-03-20:
I take it you are talking about the black dot in second 11. I noticed that your scan is orientated a bit to the right. It could be that the center of the black obstacle is outside the field of view.

May I ask why you are not using range sonar layer?

Comment by Dharmateja Kadem on 2017-03-20:
Yeah the black dot @ sec 11 is the problem. The robot is stationary in the current scenario and hence, if it is populated by the laser scan of the sensor then it should not be the case that the center of the obstacle is outside field of view.

Comment by Dharmateja Kadem on 2017-03-20:
Do you think that TF could be an issue ( in reference to the robot pose) due to which the center of the obstacle moves outside field of view?

Comment by Dharmateja Kadem on 2017-03-20:
Also, when I tried to use Range Sonar Layer for my entire suit of sensors (5 in total), some of the sensors did not populate the costmap and so i am using lasercan to simulate the sonar layer.

Comment by Humpelstilzchen on 2017-03-21:
Don't know, in the situation of the video you could check if the dot gets away when rotating your robot to the left.

I recommend to check your problem with the range layer instead.

Comment by hardy on 2017-03-21:
I also have the same problem with my robot. The costmap leaves blobs only when stationary . If clears well when the robot is turning/moving forward .

Comment by Dharmateja Kadem on 2017-03-21:
Yeah the dot gets away if i rotate the robot but that doesn't solve the problem. When the robot is stationary and if it leaves obstacles then while planning the path it says there is no path found.

Comment by Dharmateja Kadem on 2017-03-21:
I already tried the rangesensor layer it also has same problem

Comment by Procópio on 2017-03-22:
the raytracing algorithm is likely the culprit. i may mark a cell but then due to noise and slightly different readings, it cannot raytrace the original marked cell.


1 Answer 1


Rosanswers logo

I was having the same issue with my Robot. Currently, the CostMap2D uses Bresenham2D algorithm for clearing the obstacles. As per your video, the blob gets left out in almost the same location and that I think is due to the round off error of Bresenham2D algorithm. It marks a particular cell as obstacle but the next time when the sensor reading changes instantaneously, it no longer traces it through the same path and hence, the blob seem remains in the costmap .

So, for solving this problem , I amended the function below in costmap_2d.h in the navigation package of ros. I have added 2 lines of code to the Bresenham2D algorithm. This basically clears the cell to the left and right of the grid through which the Line segment constructed by the algorithm passes. This results in loosing some resolution of the map but the solution works pretty well in real life application and I have had no problems after this hacky fix which is not too bad.

template<class ActionType>
    inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
                            int offset_b, unsigned int offset, unsigned int max_length)
      unsigned int end = std::min(max_length, abs_da);
      for (unsigned int i = 0; i < end; ++i)
        at(offset+1); // **ADDED THIS LINE**
        at(offset-1); // **ADDED THIS LINE**
        offset += offset_a;
        error_b += abs_db;
        if ((unsigned int)error_b >= abs_da)
          offset += offset_b;
          error_b -= abs_da;

Originally posted by hardy with karma: 96 on 2017-03-22

This answer was ACCEPTED on the original site

Post score: 7

Original comments

Comment by Dharmateja Kadem on 2017-03-22:
Thanks it solved my problem


Your Answer

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