Ask Your Question
0

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

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

mcarr gravatar image

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

2 Answers

Sort by » oldest newest most voted
0

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

mcarr gravatar image

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 more
0

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

rikee93 gravatar image

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 more

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 gravatar imageHumpelstilzchen ( 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 gravatar imagemcarr ( 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 gravatar imageDelb ( 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 gravatar imagemcarr ( 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 gravatar imageDelb ( 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 gravatar imagemcarr ( 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 gravatar imageDelb ( 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 gravatar imagemcarr ( 2018-10-12 03:18:23 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-10-10 10:24:37 -0500

Seen: 243 times

Last updated: Oct 12 '18