Extended the sensor range to avoid mistaking the max reading as an obstacle. Add a check to the obstacle grid index to prevent crashes. I thought it was just an edge case, but something more insidious is going on.
I have tried everything to fix the range / pose sync problem. A terrible hack was added: An obstacle must be detected 5 times to show up.
Not yet integrated. Graphics2d problems.