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