BaseVectorGeod and BaseVectorCart blocks not available in driver

[Jad-Tawil]

Am i correct in observing that the BaseVectorGeod and BaseVectorCart blocks ( the relative position of the rover w.r.t. the base station) are not retrieved and processed by the driver?

If not much trouble, can this be added?

[thomasemter]

Your observation is correct, these blocks are not yet processed by the driver.

It is not much trouble to add these. I quickly adapted the development branch of ROS1 and ROS2. It is still undocumented and untested as I have no access to corrections right now. So, if you like to test it I would be happy about your feedback.

Btw. this branch also allows you to flexibly choose sources for RTK corrections, even multiple at the same time.

[Jad-Tawil]

line 996 in communications_core.cpp

if (settings_->publish_basevectorcart)
  {
      handlers_.callbackmap_ = handlers_.insert<PosCovCartesianMsg>("4043");
  }
  if (settings_->publish_basevectorgeod)
  {
      handlers_.callbackmap_ = handlers_.insert<PosCovCartesianMsg>("4028");
  }

Should that not be:

if (settings_->publish_basevectorcart)
{
    handlers_.callbackmap_ = handlers_.insert<BaseVectorCartMsg>("4043");
}
if (settings_->publish_basevectorgeod)
{
    handlers_.callbackmap_ = handlers_.insert<BaseVectorGeodMsg>("4028");
}

[thomasemter]

You are right, thank you. This was a copy-paste error and is fixed now. It did work nonetheless as the template argument does not matter anymore.

[Jad-Tawil]

Hey Thomas,

The INS solution can be provided w.r.t. a Point of Interest (POI).
The BaseVector(Cart/Geod) is provided from antenna ARP to base station ARP.
A few questions:

The BaseVectorCart is a vector expressed in which frame exactly? If the frame's origin is the GNSS Antenna ARP, how is it oriented exactly?
Same question for BaseVectorGeod.

More generally, is it possible to have a INS solution with position expressed w.r.t. the ENU or NED frame, having it's origin at the base station?

[thomasemter]

Hi Jad,

I did not delve very deeply into the details, but as I see it, the relative coordinates of BaseVectorCart should be in the ECEF frame specified by datum. To my understanding, the orientation and BaseVectorGeod is referenced to the local tangential frame (ENU) w.r.t. to the rover.

To express the vector w.r.t. to the base in ECEF coordinates is quite easy as it is just the inverse (BaseVectorCart: dx, dy, dz). To get a vector in local ENU w.r.t to the base is more involved. You have to calculate the position of the base and the ENU frame at that position. But you need the position of the base to reference the INS solution to it anyway, right? Afterwards, you would have to calculate the vector back to the rover.

I also recommend to contact Septentrio's support, because they surely are more knowledgeable in this regard than me :-)

[Jad-Tawil]

Makes sense!

I see in the INSNavCart message that the frame_id in the header is the POI.
In order to use this with the rest of the system, I have to convert INSNavCart to nav_msgs/Odometry on my end.
The nav_msgs/Odometry has frame_id, and child_frame_id. What makes sense to me, is that the child_frame_id is the POI, which is the frame_id in the INSNavCart, and the frame_id is the name of the datum (ETRS89 in our case).
Does this logic make sense?

[thomasemter]

The nav_msgs/Odometry has frame_id, and child_frame_id. What makes sense to me, is that the child_frame_id is the POI, which is the frame_id in the INSNavCart, and the frame_id is the name of the datum (ETRS89 in our case).
Does this logic make sense?

Yes, sounds correct. However, care has to be taken with the attitude in INSNavCart, since it is not in ECEF but rather related to local NED (ENU if use_ros_axis_orientation = true). Thus, the angles have to be converted accordingly in order to obtain a correct Cartesian transformation.

[Jad-Tawil]

Oh , i did not know that... How does one do the conversion?

Or better yet, can we convert the ECEF position to local ENU / NED (with respect to the datum INSNavCart is provided for)?

If so, this link shows ENU->ECEF and vice versa:
https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates

What we need for the equations if longitude and latitude, which we would extract from the INSNavGeod message.

[thomasemter]

My first thought was also to use both INSNavCart and INSNavGeod. While it would be possible to infer latitude and longitude from from ECEF coordinates, it is quite involved and needs and iterative algorithm. Thus, it absolutely makes sense to use latitude and longitude from INSNavGeod.

The formulas on the page of esa should be valid for geodetic latitude, which should be true for GNSS if am not mistaken. I will try to have it confirmed by the experts at Septentrio.

[Jad-Tawil]

I have also contacted Septentrio about this. Will let you know their reply.

On that note, i definitely think the driver should publish a nav_msgs/Odometry, which encompasses the 6DOF pose, in either NED or ECEF (selectable). It seems to me like anyone using this driver in their robotics application would have to do that themselves (for example to fuse the output in another EKF with things like wheel odometry, visual odometry, etc..).

[thomasemter]

The driver features the possibility to output INS localization in UTM as nav_msgs/Odometry and tf. This may readily be used as input for another EKF. A localization output in ECEF is indeed a nice feature and your question has already given me the idea to add this in the future ;-).

Concerning fusion of additional sensors: With the upcoming update it will even be possible to go the other way round and input 2D odometry to the INS.

[thomasemter]

I have made a first approach for publishing localization in ECEF (ROS1/ROS2). Beware, since I have no ground-truth for reference it is still untested and unconfirmed. I am happy about any feedback.

[Jad-Tawil]

Looks great, will test and provide you a bagfile for your reference!

[Jad-Tawil]

Hey Thomas,

It seems line the Angular velocity field in the localization message is never filled.
That seems odd! What would be the reason?

[Jad-Tawil]

I see that it is not available in the INSNavCart/Geod messages, but that information is available in the AttEuler block. Is it possible to use the AttEuler to fill the localization message twist.angular?

[Jad-Tawil]

I will share the bagfile collected for a relatively straight motion, for your review.
https://drive.google.com/file/d/1sRUse80yVmzD-Tt39ZihAd7uLzIGNmp7/view?usp=share_link

[Jad-Tawil]

Hey Thomas,

This is the localization_ecef topic visualized (from the previous bagfile). I subtracted the first pose from all subsequent poses so that it is easily viewable in RVIZ.

ECEF is not locally tangential, so the robot trajectory looks erroneous when visualized.

We will need the XYZ to be expressed in ENU or NED (orientation already is), so that it can be used practical in our system.

In order to get the position in ENU (or NED) from ECEF, we need the ECEF position of the base station. Perhaps the BaseVectorGeod or BaseVectorCart can be used, since they contain the base station position w.r.t. the main antenna in ENU and ECEF coordinates respectively.

Would be great to have localization_enu / or localization_ned (depending on ROS axis true or false)!

[thomasemter]

I see that it is not available in the INSNavCart/Geod messages, but that information is available in the AttEuler block. Is it possible to use the AttEuler to fill the localization message twist.angular?

Angular velocities are just not in any SBF block. AttEuler only contains the angles from dual-antenna GNSS measurements.

[thomasemter]

Hey Jad,

Thank you so much for sharing your collected data. Although ECEF is not locally tangential, the robot's attitude should still align with the trajectory. Looking at your data I have found an error in the code already which might explain this: to calculate the transformations I accidentally converted latitude and longitude from deg to rad although the are already in rad, d'oh. If it is not too much trouble it would be great if you could try again with the fixed code.

The driver provides a localization in ENU/NED (topic localization), namely in UTM. But I am not sure if this is exactly what you want since it is a projection. A localization frame is always only locally tangential in its origin. So, if this is suitable will depend on what you exactly want to achieve.

Btw. I found a block with the position of the base station in ECEF in the firmware refence manual (#5949 BaseStation). Maybe this is what you are searching for?