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

asked 2018-10-10 10:24:37 -0500

mcarr
145 ●5 ●10 ●17

updated 2018-10-11 03:26:40 -0500

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

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
  robot_base_frame: base_link
  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
    clear_on_max_reading: true
    no_readings_timeout: 0.0

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:
  inflation_radius: 0.35
  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:

image description

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 ...

(more)

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2018-10-12 03:48:47 -0500

mcarr
145 ●5 ●10 ●17

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).

edit flag offensive delete link

Comments

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]

I have no idea about this problem.

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

add a comment

answered 2018-10-10 16:53:29 -0500

rikee93
1 ●1

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.

edit flag offensive delete link

Comments

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

Humpelstilzchen ( 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

mcarr ( 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.

Delb ( 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

mcarr ( 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.

Delb ( 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

mcarr ( 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

Delb ( 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.

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

see more comments

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

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

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

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

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