TF Tree confusion. How to configure to ROS REP 105

[Darkproduct]

What I try to accomplish:

I'm trying to configure mavros with px4 in Gazebo to run (cartographer)[http://wiki.ros.org/cartographer] (cartographer isn't running atm) and I'm looking at a TF tree which looks like this:

From REP 105 I want something like this:

graph TD;
  map-->odom;
  odom-->base_link;
  base_link-->hokuyo_laser_link;
  base_link-->imu_link;
  base_link-->rotor_0

How is the current TF tree supposed to work?

fcu = base_link (aka. flight control unit?) local_origin = odom or map?

But what is base_link_frd, map_ned and odom_ned, and why are they needed? (I know that ned should be the inverse of the original frame, am I right?)

I looked at the wiki and I couldn't find any explanation, only a description of the parameters. At the top it says Documented, but where do I find it?

I found a reference to #473 in the readme. From the document in #473:

Further, it is recommended that documentation be generated on how the config file frames relate to the REP 105 frames.

I would appreciate that. If there is already documentation available, make it more visible.

I tried so far:

I found px4_config.yaml which looks like to comply with REP 105.

But some things are odd in this configuration. Why is there a map to base_link transform? Mavros should publish an odom to base_link transform or did I miss something.

As far as my understanding goes:

• mavros should publish odom to base_link
• cartographer should publish map to odom

I tested px4_config.yaml anyway and here is what I've got: I changed tf -> send to true. The simulation starts finde, but then I can't get off the ground:

[ INFO] [1582292752.825637202, 86.068000000]: FCU: ARMED by Arm/Disarm component command [ WARN] [1582292752.833535287, 86.076000000]: CMD: Unexpected command 176, result 1 [ INFO] [1582292762.850428751, 96.084000000]: FCU: DISARMED by Auto disarm on land INFO [logger] closed logfile, bytes written: 4196634

New TF tree:

> rosrun mavros checkid 
OK. I got messages from 1:1.

---
Received 7236 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 4, 140, 141, 147, 22, 24, 26, 30, 31, 32, 33, 36, 44, 74, 331, 76, 83, 340, 87, 230, 105, 111, 241, 242, 245

Software and versions:

Ubuntu: 16.04 ROS: Kinetic Gazebo: 9.11.0 Mavros: 0.33.3 PX4: v1.9.0-rc2

[msadowski]

I'm actually looking into something similar right now. I agree that publishing map->odom->base_link would be ideal but don't know just yet how to perform this with mavros.

map->odom not necessairly has to come from Cartographer. For example when using robot_localization you would run two ekf filters:

• One would provide map->odom transform by fusing gps data (this one is not continuous)
• One would provide odom->base_link transform (no gps fused here, no pose jumps expected)

If anyone knew how to recreate a setup like this in mavros that would be appreciated!

[frapit]

Did you make any progress? I don't understand the tf tree output from mavros. Running the basic px4 hovering example with gazebo results in some disconnected tf tree:

If I understand correctly, there is some basic odometry running, and mavors publishes a pose to /mavros/local_position/pose so it should also be able to generate a map->odom transformation from EKF2 filter, shouldn't it? The pose topic looks fine in rviz.

Setting the send parameter in px4_config.yaml to true leads to the following tf tree, in my case the simulation is still working: (I added a camera to the base_link as well.) But why is the pose estimation represented by the (map->base_link) transform and doesn't include the odom frame?

As this is the basic example I guess the tf tree is like that by design? The isolated odom tree makes no sense to me and I have no clue how to setup an appropriate transform from odom->map as rviz is complaining about it.

Would be great if someone could point into the right direction or link a tutorial / working setup.

[blubbi321]

Same issue here. I would like to use the move_base node with the output of mavros .. and from what I understand it doesnt work with a TF tree like this. So thanks @RapIt for compiling the state so nicely!

@Darkproduct did you make any progress understanding what is going on?

[Rezenders]

Any updates on this?

[Darkproduct]

I finished my work on this project and moved on. I couldn't make any progress and used the map --> base_link directly also shown by @RapIt.

I hope the project maintainer decides to do something about this issue in the future. I would argue that this state is unacceptable espessialy with the poor documentation. I spend way to much hours on this.

[m1baldwin]

Just wondering; has anyone here tried publishing the map to odom transformation with the tf package static_transform_publisher? You would need to run this in a separate process than the mavros stack, but doing this you can build up the TF tree as you like.

You can change the "map" references in px4_config.yaml to "odom" and then add a new transform that you publish for map to odom. Note, This adds some complexity obviously, but judging from the issues on this topic, mavros does not quite have what is needed to fully set the tf tree up with the map -> odom -> base_link

Also, does anyone know where odom_ned is published in the mavros source code? I haven't been able to find where this is set up, and it doesn't seem like it is controlled in px4_config.yaml.

[msadowski]

Just wondering; has anyone here tried publishing the map to odom transformation with the tf package static_transform_publisher? You would need to run this in a separate process than the mavros stack, but doing this you can build up the TF tree as you like.

You can change the "map" references in px4_config.yaml to "odom" and then add a new transform that you publish for map to odom. Note, This adds some complexity obviously, but judging from the issues on this topic, mavros does not quite have what is needed to fully set the tf tree up with the map -> odom -> base_link

Be careful with this approach. I think the transform map->base_link that you get from mavros is fusing GPS data. If you change the transform to be odom->base_link then you're potentially violating REP-105 in that the robot pose in odom frame has to be continuous.

[Darkproduct]

Yes, @msadowski is right. Mavros merges several data sources, including GPS, to get the map --> base_link transform. Additionally, there is no gain here, because odom and map wouldn't be different. map --> base_link would even be more clear to what is actually going on.

Here my current understanding of the problem: Mavros does the map --> odom --> base_link transform internally and publishes a map --> base_link transform. The odom transform is hidden from us. I'm not really sure why that is. Maybe the algorithms used to fuse all the sensor information together doesn't allow to get the intermediate frame (odom) or what is more likely because mavros publishes the frames odom and odom_ned, they didn't have the time, resources, or they didn't care enough, to publish the mavros internal odom frame to tf.

[m1baldwin]

Be careful with this approach. I think the transform map->base_link that you get from mavros is fusing GPS data. If you change the transform to be odom->base_link then you're potentially violating REP-105 in that the robot pose in odom frame has to be continuous.

I agree it is important to be careful. If we change the tf link to be map to odom, then odom to base_link can still be continuous. Map to odom does not need to be continuous; odom to base_link does because that is now the odometry link.

Mavros merges several data sources, including GPS,

I think this is what the flight controller is doing, not mavros. What plugin in mavros is keeping track of the different pose estimations and fusing it into a single pose estimate? AFAIK, it just listens to the mavlink messages from the flight controller, where this fusion actually happens. Then the global_position plugin converts this from LLA to ECEF format.

Mavros does the map --> odom --> base_link transform internally and publishes a map --> base_link transform.

Maybe it does do this internally; even if it does, that means it is not exposed to the tf. Which means you won't be able to access it with a TF lookup operation, making it useless in case you want to develop on top of it. This hurts the ability to scale to larger flights or multi-UAV use cases.

Another thing; not all drones use GPS; however, whether or not the drone uses GPS, there is usually some form of a local-coordinate system it creates which usually has its origin at where it was turned on. This is what is tracked by the odom to base_link transform. This movement might be augmented by a GPS measurement if it is present, but ultimately what is coming from the fcu is a fusion of GPS and other sensors, so it is going to be continuous unless your FCU fails. I think basing the map to odom to base_link tracking off of this local coordinate system is more consistent across platforms as it will work when there is GPS along with when there isn't. You would probably need to rename the GPS frame_id's so that there is not a conflict with a frame having more than one parent

[FPSychotic]

Hi , I'm facing this issue, any insight to a work around, I renamed the TF names in the config.yalm with publish true or false, but I still brakes my TF tree and the new names are not published, maybe I did something wrong, but there is not many explanation there (into the file) but I couldn't find at the moment more explanation, I will try again.

[clydemcqueen]

I don't think that this is a bug in mavros.

It is my understanding that the REP 105 map->odom->base tree was designed to handle this specific case:

• The robot generates good odometry, typically from wheel encoders. This data can be fused with control data (inputs to the wheels) and possibly IMU data to provide a very good local pose relative to the local "odom" frame. The odom frame is typically initialized to 0 on boot. All obstacle avoidance is done in the odom frame, so the robot can move around the local area avoiding obstacles without having any idea where it really is on some global map.

• There is also some global position information relative to some known map. The global position information might come from GPS or possibly some SLAM system like cartographer, where the map is being built while the robot moves. The global position and the map can be used by the navigation system to plan routes through previously discovered obstacles. For this to work, some system needs to publish the map->odom transform for the navigation software. This calculation is basically map->odom = map->base * inverse(odom->base).

In the case of PX4 and ArduPilot, there is an onboard EKF that is fusing data from multiple sensors. Depending on the sensor suite this EKF is running in the map frame or the odom frame:

• If there is a GPS sensor involved, it is running in the map frame, and the output is map->base. If there is external nav data that is generated by a SLAM system and sent to the robot via /mavros/vision_estimate/pose then the EKF is running in the map frame (against the map generated by the SLAM system) and the output is map->base.

• If there is no internal or external source of global position data against some known map then it may be running in the odom frame, and the output would be odom->base. An example of this is a copter running indoors with an optical flow camera system and no GPS sensor. Note that a copter running indoors with just a cheap IMU will run into trouble. The noise will overwhelm the signal after a few seconds and the EKF will give up.

Note that there is only one EKF in PX4 and ArduPilot, so if you have a system with both wheel encoders and SLAM, and you want to use both the odom and map frames, then you may want to set up a second EKF to fuse sensors in the other frame. The decision depends on the sensor suite, and to some extent the limitations of MAVLink. (The onboard EKF will have fast access to frequent IMU readings, but MAVLink will add latency and I know that ArduPilot won't publish raw IMU messages more frequently than 50Hz. This reduces the usefulness of the IMU data for an external EKF.)

With this in mind, mavros has the option to publish a single transform, either map->base or odom->base. If you are using a SLAM system it might be easier to have mavros publish neither transform, and just have the SLAM system publish whatever transforms you need.

I hope this helps.

[elgarbe]

It's an old issue, I know, but I'm facing the same problem. I think that a workaround could be use robot_localization package and feed IMU and GPS data from mavros topics and add any external source of data like wheel encoder or GPS velocity directly to R_L. This way we can configure two R_L instances to get the full map->odom->baselink tf_tree