obstacles are not cleared completely in costmap

asked 2017-03-17 15:02:01 -0600

Dharmateja Kadem
190 ●9 ●16 ●20

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 :

local_costmap:
  #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

  plugins:
  # - {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
  obstacle_layer2:
    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

edit retag flag offensive close merge delete

Comments

Could it be that, when the object is out of range your sensor value is > max_range ?

Humpelstilzchen ( 2017-03-18 02:57:10 -0600 )edit

I checked that, the sensor value is not greater than max_range

Dharmateja Kadem ( 2017-03-19 22:06:41 -0600 )edit

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?

Humpelstilzchen ( 2017-03-20 02:02:58 -0600 )edit

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.

Dharmateja Kadem ( 2017-03-20 13:06:35 -0600 )edit

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?

Dharmateja Kadem ( 2017-03-20 13:09:10 -0600 )edit

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.

Dharmateja Kadem ( 2017-03-20 13:10:24 -0600 )edit

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.

Humpelstilzchen ( 2017-03-21 01:58:10 -0600 )edit

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 .

hardy ( 2017-03-21 13:41:01 -0600 )edit

see more comments

answered 2017-03-22 13:08:18 -0600

hardy
96 ●6 ●7 ●8

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);
        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;
        }
      }
      at(offset);
    }

edit flag offensive delete link

Comments

Thanks it solved my problem

Dharmateja Kadem ( 2017-03-22 14:09:51 -0600 )edit

add a comment

obstacles are not cleared completely in costmap

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

obstacles are not cleared completely in costmap edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

obstacles are not cleared completely in costmap