Skip to content

Fixing bug where gnss would always round down lat & lon

Steven requested to merge astuff/autoware.ai/common:cherry-pick-09b81432 into master

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)

  1. While running ros-bridge, turn on gps messages from the LG sim.
  2. Receive /nmea_sentence by running nmea2tfpose node from gnss_localizer
  3. 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)

  1. Follow steps from "Steps to reproduce the bug" on astuff/autoware.ai/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

Fix applied

If latd or lond is a negative number, use ceil() instead to round up.

Additional Information

This fix affects the results in astuff/autoware.ai/core_perception!9 (merged)
Will open another MR to update the unit test values Associated downstream MR: astuff/autoware.ai/common!4 (merged)

Edited by Steven

Merge request reports