Fixing bug where gnss would always round down lat & lon
Fixed bug
When using the gnss
lib to convert LLH to XYZ poses, it uses floor()
function to round down when filtering out all digits lower than hundreds place.
https://gitlab.com/astuff/autoware.ai/common/blob/as/master/gnss/src/geo_pos_conv.cpp#L207 lad = floor(latd / 100.);
https://gitlab.com/astuff/autoware.ai/common/blob/as/master/gnss/src/geo_pos_conv.cpp#L209 lod = floor(lond / 100.);
The problem with this is that once you get to the negatives, the numbers start to round up.
Ex. floor(-01.253 / 100.) = -1 (instead of 0)
Steps to reproduce the bug (in LG Sim)
- While running ros-bridge, turn on gps messages from the LG sim.
- Receive
/nmea_sentence
by runningnmea2tfpose
node fromgnss_localizer
- Drive the car to different quadrants of XY coordinates.
Expected behavior
gnss_pose
should be reporting smooth transition between quadrants in X/Y values without any jumps
Actual behavior
gnss_pose
reporting a huge jump in X/Y values when the vehicle enters West/South regions (quadrant 2,3,4)
Steps to reproduce the bug (manually)
- Follow steps from "Steps to reproduce the bug" on core_perception!9 (merged)
Expected behavior
gnss_pose
should be reporting the following values:
For step 3:
x: -90207689.8924
y: 7519179.29268
z: 545.4
For step 4:
x: -81305263.475
y: 5958827.35862
z: 545.4
Actual behavior
gnss_pose
reporting wrong values
Additional Information
This fix affects the results in core_perception!9 (merged)
Will open another MR to update the unit test values
Fix applied
If latd or lond is a negative number, use ceil() instead to round up.