# Costmap2D failing to mark laser scan data as obstacles on one side

My obstacle layer is only marking obstacles on one half of the robot.

The raw laser scan data is correct and it is correctly visualised in RVIZ and you can see. The unmarked obstacles are always on the right-side of the robot (0 to -pi), even as it turns in the costmap. If it was an issue with the costmap itself, I would expect the error to be independent of the robot, however this is not the case.

Does this look familiar or have any suggestions?

local_costmap_params.yaml:

local_costmap:
global_frame: odom
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 7.0
height: 7.0
resolution: 0.05
transform_tolerance: 0.5

plugins:
#- {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"}
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}

sonar_layer:
topics: ['/ultrasonic1','/ultrasonic2','/ultrasonic3']
clear_threshold: 0.4
mark_threshold: 0.6


common_costmap_params.yaml:

footprint: [[0.28, 0.245], [0.28,-0.245], [-0.28,-0.245], [-0.28,0.245]]
obstacle_layer:
observation_sources: laser_scan_sensor
laser_scan_sensor:
topic: /scan
sensor_frame: /scan
data_type: LaserScan
marking: true
clearing: true
expected_update_rate: 5.0
inf_is_valid: true
obstacle_range: 6
raytrace_range: 15

inflation_layer:
cost_scaling_factor: 10.0


TF publisher from /base_link to /scan:

<node name="base_link_to_scan" pkg="tf" type="static_transform_publisher" args="0.20 0 0 3.1415 0 0 /base_link /scan 100"/>


EDIT 1

warning message on move_base bringup:

[ INFO] [1539238779.564957513]: Using plugin "obstacle_layer"
[ INFO] [1539238779.620244801]:     Subscribed to Topics: laser_scan_sensor
[ INFO] [1539238780.103866224]: Using plugin "inflation_layer"
[ WARN] [1539238780.699934704]: Illegal bounds change, was [tl: (-179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000, -179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000), br: (179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000, 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.000000)], but is now [tl: (-340282346638528859811704183484516925440.000000, -340282346638528859811704183484516925440.000000), br: (340282346638528859811704183484516925440.000000, 340282346638528859811704183484516925440.000000)]. The offending layer is local_costmap/inflation_layer
[ INFO] [1539238780.918647320]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1539238781.130984061]: Sim period is set to 0.10
[ INFO] [1539238782.598323429]: Recovery behavior will clear layer obstacles
[ INFO] [1539238782.684406268]: Recovery behavior will clear layer obstacles


EDIT 2

Local Costmap is centered on the robot:

result from echo on /scan

    header:
seq: 660
stamp:
secs: 1539183847
nsecs: 932467526
frame_id: "scan"
angle_min: -3.12413907051
angle_max: 3.14159274101
angle_increment: 0.00871450919658
time_increment: 9.44497514865e-05
scan_time: 0.0679093673825
range_min: 0.15000000596
range_max: 12.0
ranges: [3.6559998989105225, 3.635999917984009, 3.611999988555908, 3.6080000400543213, 3.0759999752044678, 3.2079999446868896, 3.7200000286102295, 3.7200000286102295, 3.6640000343322754, 3.6440000534057617, 3.6480000019073486, 2.5920000076293945, 2.615999937057495, inf, 2.6559998989105225, 2.6559998989105225, 2.6480000019073486, 2.635999917984009, 2.631999969482422, 2.635999917984009, 2.631999969482422, 2.631999969482422, 2.611999988555908, 2.6080000400543213, 2.611999988555908, 2.619999885559082, 2.628000020980835, 2.615999937057495, 2.615999937057495, 2.611999988555908, 2.611999988555908, 2.611999988555908, 2.619999885559082, 2.624000072479248, 2.624000072479248, 2.6440000534057617, 2.6480000019073486, 2.640000104904175, 2.6640000343322754, 2.6640000343322754, 2.671999931335449, 2.6679999828338623, 2.6760001182556152, 2.6760001182556152, 2.680000066757202, 2.687999963760376, 2.691999912261963, 2.691999912261963, 2.7039999961853027, 2.7239999771118164, 2.7279999256134033, 2.7279999256134033, 2.7320001125335693, 2.752000093460083, 2.75600004196167, 2.75600004196167, 2.75600004196167, 2.7679998874664307, 2.7839999198913574, 2.7839999198913574, 2.7960000038146973, 2.812000036239624, 2.808000087738037, 2.7920000553131104, 2.7920000553131104, 2.7799999713897705, 2.7960000038146973, 2.808000087738037, 2 ...
edit retag close merge delete

Sort by » oldest newest most voted

The problem was in the TF transform from odom -> base_link. I am using robot_localization to fuse IMU and wheel odometry data. When I instead used a static transform publish for the tf odom -> base_link, everything worked fine. I did see that when using robot_localization, there was small variations in the pitch and roll due to noise (or drift / inaccuracies) in the IMU.

"rosrun tf tf_echo odom base_link" gives

At time 1539332949.794
- Translation: [0.030, 0.002, 0.000]
- Rotation: in Quaternion [0.002, -0.000, 0.030, 1.000]
in RPY (radian) [0.005, -0.000, 0.061]
in RPY (degree) [0.268, -0.025, 3.494]


If you can imagine that the laser scan frame is not parallel to the 'ground' plane due to these small inaccuracies. Thus the Costmap2D was ignoring the laser scan values when the laser plane when below the ground plane.

Fix: In robot_localisation parameters.yaml file, there is a parameter called two_d_mode

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: false


setting

two_d_mode: true


fixed the problem completely. The package then assumed 2D as so sets the pitch and roll to 0. There was never any problem with the lidar data or Costmap2D configuration but instead slight planer misalignment of the base_link frame (i.e. the transform odom -> base_link).

more

Hello. I met the same problem. The obstacle layer is only marking obstacles on right hand of the robot. The original laser_scan data is correct, and the TF transform from odom to base_link is correct as well which shows like below:

• Rotation: in Quaternion [0.000, 0.000, 1.000, 0.002] in RPY (radian) [0.000, -0.000, 3.138] in RPY (degree) [0.000, -0.000, 179.812]

( 2020-11-17 01:05:50 -0500 )edit

From the param information you've provided it looks like you only have one observation source providing information for your obstacle layer while you have several sonar sensors on your robot:

obstacle_layer: observation_sources: laser_scan_sensor

A simple check might be to verify which sensors are located on the side on which you're getting obstacle data i.e. if it's the laser_scan_sensor.

more

/scan has angle_min: -3.12413907051 and angle_max: 3.14159274101 (~ -180°..180°) deg coverage. So I guess it should have full coverage.

( 2018-10-11 00:53:02 -0500 )edit

In local_costmap_params.yaml, I have commented out the sonar_layer (range_sensor_layer) so it shouldn't have any effect on the costmap. I'm also getting a warning message on launch "Illegal bounds change" but it's from Inflation layer so I don't think that is the cause. Edited original question

( 2018-10-11 01:17:30 -0500 )edit

Can you display the local costmap in rviz ? I think your the local costmap might not be centered on the robot hence the issue. Also from #q213902 check the spacing in your param files.

( 2018-10-11 01:58:25 -0500 )edit

The local Costmap is centered around the robot, I've added a picture to show this. Also using rqt_reconfigure the size of the costmap is 7.0m x 7.0m and the(origin_x, origin_y) = (0,0) which agrees with my param files

( 2018-10-11 03:15:25 -0500 )edit

Can you describe the laser you are using, how you launch it and what is its rate ? The param expected_update_rate: 5.0 seems odd to me, you should delete this line (to set it to its default value) or set it to twice the sensor publishing rate.

( 2018-10-11 04:31:55 -0500 )edit

I'm using RPLidar A2M8. I have done exactly what you said and set the expected rate to twice as fast as it's publishing. It publishes on /scan at 10hz therefore expected_update_rate should be 5hz, not 20Hz. I don't think the Lidar is at fault because the visualisation in RVIZ is correct

( 2018-10-11 05:07:01 -0500 )edit

You are right the issue is with your parameters, maybe try to use the defaults value and see what happen

( 2018-10-11 05:12:15 -0500 )edit

Reset everything back to default but no difference. However it's my robot_localization node at fault! The transform it's publishing from odom->base_link is wrong somehow. When I disabled it and used another to node publish this transform, it worked. Will edit the question.

( 2018-10-12 03:18:23 -0500 )edit