Handle crossing of UTM zone boundaries

[ayrton04]

If the robot crosses into a new UTM zone, we need to recompute the transform. We can add some logic here to check for a change and handle the situation accordingly.

[ayrton04]

Need to update this ROS answers question when complete.

[frontw]

Hi! Is there any update on this? It's really a problem to use this package for coordinates at zone boundaries. Could you, please, give a hint, how to possibly fix it? Thanks!

[ayrton04]

I haven't had the cycles to address it, apologies. I would start here:

https://github.com/cra-ros-pkg/robot_localization/blob/kinetic-devel/src/navsat_transform.cpp#L479

Store the UTM zone, and if it changes, you'd have to re-compute the transform. You'd also have to re-subscribe to the IMU data.

[AGummyBear]

I have a branch that avoids the UTM zone boundary issue by utilizing an internal Local Cartesian coordinate system centered on the "origin" (first good gps) rather than UTM coordinates. I configured this as a parameter, so you can switch between using UTM and the Local Cartesian. The branch is in a fork here: https://github.com/Greenzie/robot_localization/tree/agummybear/local_cartesian_option

I've tested this on some bag files, and will hopefully get to test in the real world soon.

use_local_cartesian = false

use_local_cartesian = true

[Timple]

I would be against jumping the utm transform. @ayrton04 @frontw what is your use-case for this? The utm zones allow extending outside their boundary. All is fine as long as you don't cross them by a large amount (relative to UTM grid size).

Jumping can cause unexpected behavior. Imagine a path defined in utm at 10cm parallel to an utm border. Now if the robot crosses the zone for just one sample, the utm jumps 500km to the right and the robot will (thus) try to go 500km to the right while it was actually doing pretty okay. (Said problem also occurs with paths crossing utm boundary). By not changing utm, all is well.

---

I do have an approach in mind for a while but hasn't been on top of the backlog yet. How about we specify the utm zone? So instead of utm we output a frame 32T. This also allows for more frames. A pragmatic approach would be to sample in each direction for 1° and output all zones (often just 32T, maybe sometimes 31T and 32T).

This will solve the following scenario: Image a goal being generated in utm by an external entity. If we now boot up just in the wrong zone, robot will think the whole path is 500km to the left. If both specify the frame_id in this format, all is well. The path will have 31T as frame_id and we will know where that origin is.

[ayrton04]

I've never been near a UTM boundary with a robot, so I've never been able to test/witness this directly, but I wanted to support robots that need to cross UTM boundaries by a wide margin and retain an accurate pose. We allow the user to transform the robot's EKF pose back into GPS coordinates, and I have to imagine that this becomes less accurate as we move farther from the UTM boundary.

If I had my druthers, we'd get the package to adhere to REP-105's new(ish) changes, and add an earth frame as a parent to map. Then I'd probably see if we could just move all the internal workings of the transform node into ECEF coordinates.

[Timple]

I pretty much live on one :). REP-105 doesn't mention UTM. So how would one go about that? If this package only implements REP-105 the utm transform would disappear in favor of earth. But then a new package will be required for people using UTM. So my proposal is to still output utm next to earth (probably child of earth). And this discussion still needs an answer.

[ayrton04]

So I'm afraid I simply don't have the cycles to look into this. I don't use r_l myself these days, and I have pretty much zero time to do any real development on it. I'm happy to participate in this conversation as needed, though, and will (always) accept PRs.

[Timple]

I like the REP-105 earth approach but we don't have a need for it, I consider it an nice to have if we're going to work on this.

We do have a need for this crossing UTM zones. @frontw, @AGummyBear: do you agree with my proposal to output multiple tf's if close to a boundary? (for backwards compatibility I'd keep the current utm as is)

[Jconn]

I've never been near a UTM boundary with a robot, so I've never been able to test/witness this directly

The starting GPS position can be specified in gazebo with something like this in the world sdf model

    <spherical_coordinates>
        <surface_model>EARTH_WGS84</surface_model>
        <latitude_deg>30.0</latitude_deg>
        <longitude_deg>26.0</longitude_deg>
        <elevation>0.0</elevation>
        <heading_deg>0</heading_deg>
    </spherical_coordinates>

in case anyone here uses gazebo and wants to mess with this feature

[AGummyBear]

Outputting multiple TFs when near a zone boundary seems like it would put a fairly heavy burden on the consumers of those TFs in terms of knowing when to switch and which to use at any given time. Personally, I don't have a use-case for that.

I experienced this issue first-hand with the data-set that I posted the screenshots above. We have a robotic lawnmower and one of our lawns has a UTM boundary right down the middle of it. What happens when you cross the boundary is the transform snaps about 55km to the West, to the other side of the boundary. In my screenshot above, you just see it disappear from RViz. Nothing fundamentally breaks, other than your robot doesn't have a clue where it actually is.

My approach in solving this was first to observe that, internally to navsat_transform, UTM is used only as a cartesian representation of coordinates on Earth, so that we could easily convert between the robot's world frame (which is also cartesian) and GPS. For this reason, I elected to use a Local Cartesian representation (also called Local Tangent Plane) rather than UTM. That way, the internal cartesian representation of the robot's world coordinates is centered on the location that the robot first acquires a good GPS signal, so it has the most accuracy in the local area. For robots that don't travel very far from their origin, I believe that this solution eliminates concerns with UTM boundaries. This would not be suitable for driverless cars, boats, or airplanes that need to travel very long distances and retain accurate localization estimates, but it works great for a lawnmower that will stay within a few hundred meters of where it starts.

The primary change I made was the addition of a new parameter to select using Local Cartesian. When set to TRUE, the internal representation is local cartesian rather than UTM. When set to false, UTM is used just as before. The other changes are to rename things appropriately, since many things that were previously called "utm" are now just "cartesian".

My hope was that this type of solution will ease concerns over UTM boundaries for people that have robots operating in small to medium sized areas (like me), but retains the UTM functionality for those who need it. It doesn't directly address crossing boundaries though.

[Timple]

when you cross the boundary is the transform snaps about 55km to the West

Ah, it was my understanding that the UTM zone was determined initially and never updated.

In that case, if I make multiple tf's optional but still the utm 'static' (only determined initially), your problems would also be over correct?

(Not saying Local Cartesian representations are not the way to go, I like the idea. Just trying to grasp the current situation. We need utm because of external communication)

[AGummyBear]

Edited to correct 55 km to 554 km

I may not have been clear with my previous comment. The transformed location of the robot is what snaps about 554km to the West in my dataset. The UTM zone is indeed determined initially and never updated, specifically in the method NavSatTransform::setTransformGps().

So here's what happens when the robot crosses a UTM boundary (using my data as an example, traversing East across the boundary from Zone 16 to Zone 17). Assume your origin for the transformation is about 50m East of the boundary, within Zone 16. As future GPS lat/lon updates come in, they are transformed to UTM via the NavsatConversions::LLtoUTM() method. The output of this method is a UTM_Easting (Cartesian X), a UTM_Northing (Cartesian Y) and a Zone ID. Initially, it will look something like this:

x = 554000
y = 6000
zone = 16

Once the robot's longitude crosses -84.0 deg, the output of LLtoUTM() looks something like (This is a simplified example, as the zones are not aligned quite so neatly as this):

x = 0
y = 6000
zone = 17

Because the change to zone 17 (and thus a new reference frame) isn't accounted for anywhere, the transform to the Robot's world frame now thinks that the robot has shifted -55000 meters in X.

Here's a sample of my output while crossing the boundary:

header: 
  seq: 837
  stamp: 
    secs: 1590161106
    nsecs: 276331901
  frame_id: "gps_link"
status: 
  status: 2
  service: 1
latitude: 33.9823221717
longitude: -84.0000023917
altitude: 293.809
position_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
position_covariance_type: 3
---
header: 
  seq: 838
  stamp: 
    secs: 1590161106
    nsecs: 484098911
  frame_id: "gps_link"
status: 
  status: 2
  service: 1
latitude: 33.9823233533
longitude: -83.9999998333
altitude: 293.83
position_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
position_covariance_type: 3
---

And the odometry output from navsat_transform

---
header: 
  seq: 787
  stamp: 
    secs: 1590161106
    nsecs: 276331901
  frame_id: "map"
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: 46.4685451047
      y: 21.4168349631
      z: 3.348
[removed non-relevant fields from message]
---
header: 
  seq: 788
  stamp: 
    secs: 1590161106
    nsecs: 484098911
  frame_id: "map"
child_frame_id: ''
pose: 
  pose: 
    position: 
      x: -554013.22292
      y: 16248.6161385
      z: 3.369
[removed non-relevant fields from message]
---

I've uploaded some rosbags of my data here for anyone interested in actual data of crossing a UTM boundary: UTM Boundary Bags utm_boundary_full.bag contains the sensor data and outputs of robot_localization utm_boundary_filtered.bag contains only the sensor data, so can be used to play back into `robot_localization to test different configurations or changes

[Timple]

That is a very clear explanation, thanks!

I was under the impression that the UTM zone was determined in the beginning (which it is) and always kept identical (which it is not).

So if the zone was determined initially and kept constant for the remainder of operation you would be okay as well. Not sure how much work that would be. The LLtoUTM and UTMtoLL would need zone as in input then.

Alright, looks like there are two options for your use-case. The one I described here and the Local Cartesian representation. I would prefer to fix the UTM so all data can always easier be traced back to a location independent of any random bootup location.

For us we still need to know which zone we are in so that is unrelated. If we do fix our items any time soon I'll make sure to tackle the above item as well :+1:

[jotix16]

I've uploaded some rosbags of my data here for anyone interested in actual data of crossing a UTM boundary: UTM Boundary Bags utm_boundary_full.bag contains the sensor data and outputs of robot_localization utm_boundary_filtered.bag contains only the sensor data, so can be used to play back into `robot_localization to test different configurations or changes

Hey @AGummyBear, could you provide the launch file for the bag too please?

[AGummyBear]

@jotix16 Could you clarify what you want? You should only need to run rosbag play <path-to-bag> to get it going.

[jotix16]

Sorry for the ambiguity. I wanted to know how the IMU and the GPS sensor are mounted relative to the base_link.

Edit1: I am able to play them already. I tried placing them in the same position/orientation as the base_link, but was getting weird orientations.

[AGummyBear]

I see. These are the transforms used:

  <joint name="base_link_to_sensor_housing_unit_link" type="fixed">
    <origin xyz="0.0508 0.0 0.635" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="sensor_housing_unit_link" />
  </joint>

  <joint name="base_link_to_imu_link" type="fixed">
    <origin xyz="0.10 0.0 0.91" rpy="0 0 ${M_PI_2}" />
    <parent link="base_link" />
    <child link="imu_link" />
  </joint>

  <joint name="sensor_housing_unit_link_to_gps_link" type="fixed">
    <origin xyz=".2032 0.0 .254" rpy="0 0 0" />
    <parent link="sensor_housing_unit_link" />
    <child link="gps_link" />
  </joint>

[marcusvinicius178]

Hi @Timple @ayrton04 @AGummyBear Could you please assist me with this issue I'm experiencing? I've detailed it on Robotics Stack Exchange , but here is a summary:

In simulation, navsat_transform_node worked well using the tutorial from Nav2 with GPS, which includes a correction factor for Gazebo:

<spherical_coordinates>
  <!-- currently gazebo has a bug: instead of outputting lat, long, altitude in ENU
  (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U,
  therefore we rotate the default frame 180 so that it would go back to ENU
  see: https://github.com/osrf/gazebo/issues/2022 -->
  <surface_model>EARTH_WGS84</surface_model>
  <latitude_deg>38.161479</latitude_deg>
  <longitude_deg>-122.454630</longitude_deg>
  <elevation>488.0</elevation>
  <heading_deg>180</heading_deg>
</spherical_coordinates>

For my real robot, using the same configuration file, the orientation of the robot is inverse, and when I run the interactive_waypoint_follower.py script to send the robot to a goal pose in Mapviz, or directly on terminal using the logged_waypoint_follower.py sending lat,long values through a Yaml File

I encounter these errors:

**- GeographicLib::GeographicErr'

• what(): Easting 2.5953e+06km not in UTM range for S hemisphere [0km, 1000km]
• [navsat_transform_node-6]: process has died [pid 9831, exit code -6,**

Here's a snippet of the log:

navsat_transform_node-6] terminate called after throwing an instance of 'GeographicLib::GeographicErr'
[navsat_transform_node-6]   what():  Easting 2.5953e+06km not in UTM range for S hemisphere [0km, 1000km]
[ERROR] [navsat_transform_node-6]: process has died [pid 9831, exit code -6, cmd '/opt/ros/iron/lib/robot_localization/navsat_transform_node --ros-args -r __node:=navsat_transform --params-file /home/rota_2024/nav2_gps_ws/install/nav2_bringup/share/nav2_bringup/params/real_bot_dual_ekf_navsat_params.yaml --params-file /tmp/launch_params_x2u050pd -r imu/data:=imu/data -r gps/fix:=gps/fix -r gps/filtered:=gps/filtered -r odometry/gps:=odometry/gps -r odometry/filtered:=odometry/global'].

Additionally, I encounter transform errors:

[planner_server-9] [INFO] [1719996445.946777994] [global_costmap.global_costmap]: Timed out waiting for transform from base_link to map to become available, tf error: Could not find a connection between 'map' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

Interestingly, the conversion from Lat, Long to UTM seems to be working:

[navsat_transform_node-6] [INFO] [1719996446.331761694] [navsat_transform]: Datum (latitude, longitude, altitude) is (-23.66, -46.59, 0.00)
[navsat_transform_node-6] [INFO] [1719996446.331891565] [navsat_transform]: Datum UTM coordinate is (23 south, 337589.07, 7382425.57)
[navsat_transform_node-6] [INFO] [1719996446.332153641] [navsat_transform]: Corrected for magnetic declination of -0.382011, user-specified offset of 1.5708 and meridian convergence of 0.0111569. Transform heading factor is now 1.19994

I have verified the UTM values against online converters, and they match. Therefore, I don't understand why I get the "Easting not in UTM range" error when trying to send a goal:

For this reason, I don't understand why I get this message when trying to send a close (lat,long) goal to the robot follows:

Easting 2.5953e+06km not in UTM range for S hemisphere [0km, 1000km]

Could the issue be due to playing a ROS2 bag with old sensor data (IMU, odometry, GPS) in one terminal while running the nav2 stack in another with the current timestamp (Robot Firmware)? Here are some relevant log entries about timeout:

[rviz2-1] [INFO] [1719996512.690740522] [rviz]: Message Filter dropping message: frame 'odom' at time 1719996502,288 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[mapviz-15] [ERROR] [1719996512.705628806] [mapviz]: [transform_manager]: Lookup would require extrapolation into the past.  Requested time 1719996502,287579 but the earliest data is at time 1719996503,313243, when looking up transform from frame [base_link] to frame [map]
[mapviz-15] [WARN] [1719996512.705694804] [mapviz]: [transform_manager]: Failed to get tf transform ('base_link' to 'map').  Both frames exist in tf.
[planner_server-9] [ERROR] [1719996512.751549810] [transformPoseInTargetFrame]: Extrapolation Error looking up target frame: Lookup would require extrapolation into the past.  Requested time 1719996502.287579 but the earliest data is at time 1719996503.313243, when looking up transform from frame [base_link] to frame [map]
[planner_server-9] 

[ekf_node-5] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
[ekf_node-5]          at line 292 in ./src/buffer_core.cpp
[controller_server-7] Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1719317691.585835 according to authority Authority undetectable

It's challenging to test on the real robot every time, so I use the ROS2 bag.

Additionally, why does using my magnetic declination for São Paulo, Brazil, and forcing a yaw_offset not change the orientation in Mapviz and result in an inverted orientation?

[navsat_transform:
  ros__parameters:
    use_sim_time: false
    frequency: 20.0
    delay: 3.0  # Typical value: 0.0-3.0 seconds. Accounts for delay in the sensor data. GPS usually has inherent delays
    magnetic_declination_radians: -0.38201051083 #0.0 # This is needed just if IMU provides orientation with respect to magnetic north.
    yaw_offset: 1.5707963 # Adjust the angle of IMU to be = 0 when facing East
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false #true
    wait_for_datum: true 
    #datum : [38.161479, -122.454630, 0.0] #Sonoma
    datum: [-23.66075725, -46.5925, 0.0] # Mercedes](url)

Any guidance would be greatly appreciated!

Thank you in advance!