I am working on an autonomous vehicle project in São Paulo, Brazil. As part of the project, I am using the navsat_transform_node from the ROS robot_localization package for integrating GPS data. I have calculated the magnetic declination for my location using the NCEI site: NCEI Magnetic Calculators. The declination value is below:
Our IMU yaw rate is obtained from our vehicle's CAN data of yaw rate, and we integrate that value. We don't have a direct IMU source.
Problem:
Despite correctly setting the magnetic declination and trying various values for the yaw_offset, my robot's orientation on Mapviz is consistently incorrect. The robot's arrow points to the back instead of the front, resulting in incorrect path planning. Specifically, the robot is oriented between 90 - 180 degrees opposite to its actual heading.
Configuration Attempts: I've tried different values as you can check on my configuration file dual_ekf_navsat_params.yaml:
Magnetic Declination: -0.38201051083 radians (as calculated) yaw_offset values tried: pi, -pi, 2pi, -2pi, etc.
None of these attempts have corrected the orientation issue. Here is a snippet of my navsat_transform_node configuration:
navsat_transform:
ros__parameters:
use_sim_time: false
frequency: 30.0
delay: 3.0
magnetic_declination_radians: -0.38201051083
yaw_offset: 3.14159265359 # Example value, tried several others
zero_altitude: true
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: true
datum: [-23.66075725, -46.5925, 0.0]
Errors and Warnings:
Additionally, I am encountering the following error on the terminal, which suggests a possible issue with the UTM conversion for the southern hemisphere:
[mapviz-15] Warning: TF_OLD_DATA ignoring data from the past for frame odom at time 1719317715,336261 according to authority Authority undetectable
[mapviz-15] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
[mapviz-15] at line 292 in ./src/buffer_core.cpp
[navsat_transform_node-6] terminate called after throwing an instance of 'GeographicLib::GeographicErr'
[navsat_transform_node-6] what(): Easting 3.12708e+06km not in UTM range for S hemisphere [0km, 1000km]
[ERROR] [navsat_transform_node-6]: process has died [pid 17979, 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_n1pfb_qu -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'].
Questions:
- Magnetic Declination Usage: Is there anything I might be missing or incorrectly setting regarding the magnetic_declination_radians? Should this value be used differently?
- Yaw Offset Adjustment: How can I correctly determine the yaw_offset to align my robot's heading in the correct direction? Any specific steps or calculations to ensure the correct value?
- UTM Range Error: What could be causing the GeographicLib::GeographicErr related to UTM range, and how can it be resolved, especially for locations in the southern hemisphere?
Additional Information:
I am using ROS 2 Iron. I will attach a video and pictures demonstrating the issue for better visualization.
Mapviz Orientation Issue Video
Wrong Orientation Picture:
Desired Orientation (Robot Forward Front)
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)
After modifying my dual_ekf_navsat_params (suggested by @aimpet answer), and switching the world_frame of ekf_filter_node_odom from 'map' to 'odom' the GeographicLib UTM conversion error has mostly ceased (though it occasionally still appears). Now, the local coordinates are calculated properly, but the orientation is incorrect (negative instead of positive).
[INFO] [1720011071.646876208] [gps_wp_commander]: Converting latitude: -23.6608333333, longitude: -46.5925, yaw: 0.0
[INFO] [1720011071.648628516] [gps_wp_commander]: Converted to local coordinates: x=-7.818494231440127, y=-3.1410479824990034
The orientation issue remains, causing my robot to follow an incorrect path from the origin to the goal. The trajectory created should be a direct line to the goal (as shown by the blue line in the figure below), but instead, the goal pose falls backward robot (negative x and negative y in the Left side of figure), but it should have (positive x, and negative y, right side figure). It is very weird as well the y = -3.14, since the negative axis of y is on the left, should not fall into the right robot side, as it is.
There are GPS errors on orientation conversion, which I am not able to track why it happens on real robot and not in simulation
Any guidance would be greatly appreciated!