2
$\begingroup$

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:

Declination,latlong data

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:

  1. Magnetic Declination Usage: Is there anything I might be missing or incorrectly setting regarding the magnetic_declination_radians? Should this value be used differently?
  2. 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?
  3. 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:

Wrong orientation

Desired Orientation (Robot Forward Front)

Desired Orientation

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:

image

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.

Incorrect Goal Pose vs Correct Goal Pose

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!

$\endgroup$

3 Answers 3

2
$\begingroup$

[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.

Right now it seems that both of your ekf instances are publishing the base_footprint to map transform. One should be used for the map<->odom tf. Have you disabled all other nodes from publishing the same transforms? I had the same issue when i was publishing duplicate odom<->base_link transforms. Disable every other possible node from publishing these transforms even if they provide data for the ekfs.

Unfortunately, I also haven't solved the orienatation issue, or in better terms the mismatch in orientation and gps location with respect to the local map frame. I'm also currently investigating it and my main problem is that the orientation of the ekf output doesnt seem to get the navsat corrections. referral to initial issue (localization/orientation issue while running robot localization with navsat transform node for fusing GPS + IMU in rviz visualization)

$\endgroup$
8
  • $\begingroup$ Hi @aimpet thanks for the answer/comment. I change the ekf_filter_node_odom to odom instead map: i.imgur.com/Ha70oT5.png However at least the error stopped: "what(): Easting 2.5953e+06km not in UTM range for S hemisphere [0km, 1000km]" It seems the local coordinates now are correctly transformed: "Point in wgs84: -46.5926,-23.6608" BUT STILL IN WRONG ORIENTATION i.imgur.com/0e18dzy.png I was using for world_frame 'map' instead 'odom' (differing from doc) because on sim and even real robot, it's delaying a lot to establish costmaps Is wrong use 'map' in both ekfs nodes? $\endgroup$ Commented Jul 3 at 12:27
  • $\begingroup$ @aimpet I thought that maybe my IMU could be not calibrated, however converting quaternion (from /imu topic) to Euler angles using a script, I got that the Imu orientation (yaw) is correct (close to 0 degrees of robot front): i.imgur.com/yytwt4Z.png It seems that using the magnetic declination and yaw_offset on the configuration file is not making any modification to correct the orientation: i.imgur.com/loCXh4v.png Did you get some different result modifying these parameters? $\endgroup$ Commented Jul 3 at 12:37
  • $\begingroup$ @MarcusVinicius i think the way the navsat is supposed to work is that the IMU operates in relation to the ENU frame. So, i am using a madgwick filter for the phdiget IMU and now it shows 0 at north. As a result with the configurations of magnetic declination and yaw offset i would expect that my final odometry/filtered_map orientation output would give me a yaw of 0, when facing at the east. But unfortunately this is not the case. I think you need a relation between your IMU readings and ENU frame. $\endgroup$
    – aimpet
    Commented Jul 3 at 13:59
  • $\begingroup$ @MarcusVinicius - You are welcome. Please note that comments are ephemeral in nature and tend to get deleted, so any important info in the above comments should also go into your question (if not done already). You can flag this comment as no longer needed, once you have read it, in order to keep things tidy. Thanks. $\endgroup$ Commented Jul 3 at 14:16
  • $\begingroup$ @MarcusVinicius Sorry for writing here, but i still cant post a comment under your 2nd answer. So i think you need some kind of ENU reference in order for the navsat transform to work. As a result I think that if your IMU doesnt provide a magnetometer and it doesnt give you any reference with respect to the ENU there is an option of letting your track move for some meters and then only from the GPS coordinates make a conclusion about your heading wrt the ENU. The navsat only needs one instance (the first) of an IMU/heading information and then doenst subscribe at all to the IMU topic. $\endgroup$
    – aimpet
    Commented Jul 4 at 9:05
2
$\begingroup$

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.

Sorry, are you saying that you don't have a magnetometer that gives you an earth-referenced yaw value?

If so, then navsat_transform_node simply won't work. There's no way to create the necessary transform between your robot's world frame and the earth frame if we don't know your robot's orientation with respect to the earth.

EDIT in response to comments:

Could you confirm if my understanding is correct that to use navsat_transform_node correctly, I need to provide an earth-referenced yaw value, and the current IMU plugin setup in Gazebo does not do this by default?

I cannot comment on the Gazebo plugin, apologies. I don't know how it works and don't use it (or if I ever did, it was many years ago, and I've forgotten). But in the real world, you need a magnetometer to supply an eart-referenced heading.

I mean I was not expecting that for a real robot, I would need to align the imu data within the earth-referenced yaw (the first time I am transitioning to a real robot)

You don't have to do anything. This is handled for you, provided you set the parameters correctly. Your IMU will report a yaw value via the magnetometer. This value will typically have two issues:

  1. It will report magnetic north. To get the value for true north, we need the magnetic declination for the location where the node is being run. So you have to set the magnetic_declination_radians parameter.
  2. However, the node doesn't want true north; it wants true east (i.e., the direction of +X on the UTM grid). So we need to account for the difference between true north and true east. This is the purpose of the yaw_offset parameter (this is typically pi/2).

Assuming you set those correctly, navsat_transform_node will work.

If you instead want to get the earth-referenced orientation from your COG, that should be fine, so long as it reports 0 facing "true east" and you publish it in the right message type (sensor_msgs/Imu).

$\endgroup$
4
  • $\begingroup$ As far as I understand, the IMU data provided by the plugin is relative to the robot's frame within the simulation world, and not inherently aligned with the Earth's magnetic field (true north). In the simulation, I assumed the orientation was already aligned with North, but after reviewing the IMU plugin code, it seems there is no transformation from the earth frame to the world frame: acesse.dev/TTkQ0 $\endgroup$ Commented Jul 24 at 14:58
  • $\begingroup$ Could you confirm if my understanding is correct that to use navsat_transform_node correctly, I need to provide an earth-referenced yaw value, and the current IMU plugin setup in Gazebo does not do this by default? Additionally, could you recommend an IMU that is robust, supports CAN integration, and is compatible with the navsat_transform_node application? $\endgroup$ Commented Jul 24 at 14:59
  • $\begingroup$ I mean I was not expecting that for a real robot, I would need to align the imu data within the earth-referenced yaw (the first time I am transitioning to a real robot). But it seems that is the case right? In simulation is this alignment already done? $\endgroup$ Commented Jul 24 at 15:06
  • $\begingroup$ I have a final question. I have COG (course over ground) from NMEA and I am acquiring HDT (GPHDT) from Trimble. Could I create a program that continuously subtracts the current IMU yaw data (from CAN, which is the truck's own frame) from the HDT data to obtain a new earth-referenced yaw? In other words, can I use the following formula to achieve an earth-referenced yaw? Earth-referenced yaw = IMU yaw (truck frame) − HDT (heading from True North) Earth-referenced yaw=IMU yaw (truck frame)−HDT (heading from True North) Is this approach correct? $\endgroup$ Commented Jul 24 at 16:50
0
$\begingroup$

Adjusting Yaw Offset and Goal Pose Calculation

  1. Correcting Yaw Offset:
  • I followed the suggestion from @aimpet to align my IMU orientation to the ENU (East-North-Up) frame. Using the Google Earth compass icon, I determined the true north direction.

  • My truck was initially facing west, which meant my yaw was 0 degrees. To align this with the ENU frame, I had to set a yaw_offset of -90 degrees

Clocwise 90 degrees in yaw_offset.

Updated Configuration

navsat_transform:
  ros__parameters:
    use_sim_time: false
    frequency: 20.0
    delay: 3.0
    magnetic_declination_radians: -0.38201051083
    yaw_offset: -1.5707963  # Adjust to 0 degrees to facing North
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false
    wait_for_datum: true 

Adjusting Goal Pose Calculation:

  • The goal pose was initially too close to the truck due to the base_link being positioned in the middle of the truck’s length.

planner_origin_base_link

  • I added an offset of 4.8025 meters to the x-coordinate of the goal pose to account for the truck’s length.

-Script Adjustment:

pose_stamped.pose.position.x = response.map_point.x + 4.8025  # Offset for half of truck length (planner is starting at origin of base_link behind the truck's cabin bumper)

-Resulting Coordinates:

[INFO] [1720063873.346257752] [gps_wp_commander]: Converting latitude: -23.6608333333, longitude: -46.5925, yaw: 0.0
[INFO] [1720063873.347771130] [gps_wp_commander]: Converted to local coordinates: x=12.62099439907819, y=3.141047563403845

Making this specific adjustment for my case (I don't wish to correct urdf to send the base_link to the truck's bumper because it is too much work) I got reasonable (correct) conversation of latitude, longitude to ros2 local coordinates (x,y) in terms of pose and orientation.

Correct Orientation and Pose Conversion

Clarification on Y Direction:

I initially misunderstood the Y direction. The resulting y = + to the left is correct according to REP 105 REP 103 standards.

Doubt

However, I am concerned about the practicality of manually adjusting the yaw_offset each time the truck starts in a different direction (e.g., facing west, east, or south). It seems impractical to manually change the configuration file (yaw_offset) every time the orientation changes.

Is there a recommended way to dynamically calculate and apply the yaw_offset based on the truck's initial orientation? I am happy with the comments in this regard.

$\endgroup$
5
  • $\begingroup$ "However, I am concerned about the practicality of manually adjusting the yaw_offset each time the truck starts in a different direction (e.g., facing west, east, or south). It seems impractical to manually change the configuration file (yaw_offset) every time the orientation changes." That's not how the node was designed. The offset is meant to capture the difference between "magnetic east" and "true east" for your location. $\endgroup$
    – automatom
    Commented Jul 24 at 9:38
  • $\begingroup$ docs.ros.org/en/melodic/api/robot_localization/html/… $\endgroup$
    – automatom
    Commented Jul 24 at 9:40
  • $\begingroup$ @automatom Thank you for the clarification. Based on your response and the documentation, I understand that: The yaw_offset parameter corrects the IMU's zero heading to align with true east, accounting for the difference between magnetic and true east ? For my location (São Paulo), I need to use the magnetic declination (-0.38 radians) to correct the heading data? $\endgroup$ Commented Jul 24 at 14:59
  • $\begingroup$ @automatom What confused me was the documentation's example of using a yaw_offset to correct from north to east " If your IMU does not conform to this standard and instead reports zero when facing north, you can still use the yaw_offset parameter to correct this. In this case, the value for yaw_offset would be π/2" , which made me unsure whether to use north or east as the reference. To find true east (to compare with /imu topic values), I can use a compass and apply the magnetic declination, or use Google Earth for an accurate reference. Is this understanding correct? $\endgroup$ Commented Jul 24 at 15:00
  • $\begingroup$ In order to work, the node needs to compute where "true east" is, i.e., the direction that aligns with +X on the UTM grid. But IMUs have two issues: (a) the values they report need to be adjusted by magnetic declination (which is the difference between true north and magnetic north - we have a magnetic_declination_radians parameter for that), and (b) even with magnetic declination, the IMU will now report 0 at true north. We want a 0 value at true east, so we have a yaw_offset parameter for that. $\endgroup$
    – automatom
    Commented Sep 24 at 9:46

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.