search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

Local position plugin not accounting for translational offset to "map" frame? #1372

Open tstastny opened 4 years ago

tstastny commented 4 years ago

Issue details

Maybe I'm missing something (definitely possible) - but it seems that the mavros local position is published with (converted to ENU) values directly from the local position mavlink message from PX4, however, defining these values as within the "map" frame, which as I understand it, is defined with origin at home position, which is offset translationally from the actual PX4 local position origin (defined in the GPS_GLOBAL_ORIGIN msg).

Is there something going on behind the scene that I am not seeing with the tf's? Or is this a bug?

Mavros frame logic described in the global position plugin here: https://github.com/mavlink/mavros/blob/a1dd03c6db26107f914b4526305abca9733f7486/mavros/src/plugins/global_position.cpp#L329-L333

Local position plugin refs:

MAVROS version and platform

Mavros: Current Master branch ROS: Melodic Ubuntu: 18.04

Autopilot type and version

PX4

TSC21 commented 4 years ago

which is offset translationally from the actual PX4 local position origin (defined in the GPS_GLOBAL_ORIGIN msg).

As far as I know there's nothing on the definition that obliges the local position data to suffer a translation. The PX4 local origin is when when the estimator starts, and we don't even use GPS_GLOBAL_ORIGIN, so the navigation frame will always end up being the same, with the same origin. Maybe using the odom frame could make sense, but for that you already have the odom plugin, which publishes the odometry data coming the flight controller (which basically is the local_position pose + velocities + covariances).

TSC21 commented 4 years ago

For your own implementation, if you are not happy with the frame_id, you can always change it here https://github.com/mavlink/mavros/blob/master/mavros/launch/px4_config.yaml#L69.

tstastny commented 4 years ago
  1. The HOME_POSITION msg from PX4 arrives with PX4 defined local position coordinates (yes, with local origin at estimator start). https://mavlink.io/en/messages/common.html#HOME_POSITION But the home position itself is *not necessarily at that "estimator start". The true PX4 local origin is placed at the GPS_GLOBAL_ORIGIN (in lat/lon) https://mavlink.io/en/messages/common.html#GPS_GLOBAL_ORIGIN
  2. These home position *local coordinates are then set as the origin for the "map" frame in the MAVROS global position plugin. (see link in above comment)
  3. Local position msg from PX4 arrives with position data also in PX4 local frame, and is set to the MAVROS local position msg with frame_id="map" and child_frame_id="baselink". (see again link in comment above). "baselink" sounds fine as a child frame.. I have no issue with this. But the problem here is that defining the tf from "map" to "baselink" here means that we are saying the position translation from "map" origin to "baselink" is whatever the PX4 local position coordinates are, which is not true, because "map" is shifted from the PX4 origin by the translation to the home position.

Maybe I am misunderstanding how tf operates - please advise if this is the case.

Otherwise, yes - I could re-define frames for my own custom implementation.. but I brought this up here because I thought this would be of interest to anyone else using MAVROS for offboard applications, where e.g. this would potentially cause problems when syncing with other e.g. vision/laser based maps a MAV should be operating in. A couple meters translational offset could mean the difference between crashing.

TSC21 commented 4 years ago

The true PX4 local origin is placed at the GPS_GLOBAL_ORIGIN (in lat/lon) https://mavlink.io/en/messages/common.html#GPS_GLOBAL_ORIGIN

And I say again: that assumption is wrong! We don't use the GPS_GLOBAL_ORIGIN. And the home position has nothing to do with the local position origin, as you as well understood. So, as far you might be concerned regarding the PX4 architecture, you should assume that the map frame matches the odom frame. We don't propagate translational offsets between a supposed global origin and a local origin. We just simply apply simple math to do re-projection to and from LLA coordinates.

tstastny commented 4 years ago

Yes I understand MAVROS doesn't use the GPS_GLOBAL_ORIGIN. I was only mentioning it because this mavlink msg is populated with the lat/lon of the PX4 local position origin (which is 0,0,0 for the mavlink local_position_ned msg positions) .. at least this is what mavlink/px4 documentation states.

But anyway, I believe I realized our mixed signals here -- I've been talking about the local position plugin this whole time.. I've now checked global position plugin again and see that the "local" subhandle published from this plugin *does consider whatever map origin is defined during the conversion from GPS readings to local enu -- as you say (when home position is received, it is home position) https://github.com/mavlink/mavros/blob/a1dd03c6db26107f914b4526305abca9733f7486/mavros/src/plugins/global_position.cpp#L356

So I will use /mavros/global_position/local to grab local positions in the map frame.. and all is ok, no translational problems.

However - I believe this is still a problem in the local position plugin, if someone were to use the positions in this message, the similar consideration of the map origin is not taken into account in the local position plugin as it is in the global position plugin. Meaning there would still be that offset from home position.. as the raw PX4 local position values are published without the corresponding subtraction of the "map" origin. Meaning the frame_id of "map" is not currently correct for the local position plugin**.

TSC21 commented 4 years ago

Yes I understand MAVROS doesn't use the GPS_GLOBAL_ORIGIN. I was only mentioning it because this mavlink msg is populated with the lat/lon of the PX4 local position origin (which is 0,0,0 for the mavlink local_position_ned msg positions) .. at least this is what mavlink/px4 documentation states.

Where exactly on the PX4 documentation you see this documented? Where do you see it being populated on the PX4 Firmware?

tstastny commented 4 years ago

Where exactly on the PX4 documentation you see this documented? Where do you see it being populated on the PX4 Firmware?

https://mavlink.io/en/messages/common.html#GPS_GLOBAL_ORIGIN

TSC21 commented 4 years ago

Where exactly on the PX4 documentation you see this documented? Where do you see it being populated on the PX4 Firmware?

https://mavlink.io/en/messages/common.html#GPS_GLOBAL_ORIGIN

That doesn't answer my question :)

TSC21 commented 4 years ago

I think you are mixing Mavlink with PX4. They are not the same. The spec on the Mavlink message is one thing, what PX4 uses from the spec is another thing. And I rephrase again: we do not use that message on the PX4 Firmware since we do not propagate two different navigation frames on the architecture. Hope this answers your doubts.

TSC21 commented 4 years ago

However - I believe this is still a problem in the local position plugin, if someone were to use the positions in this message, the similar consideration of the map origin is not taken into account in the local position plugin as it is in the global position plugin. Meaning there would still be that offset from home position.. as the raw PX4 local position values are published without the corresponding subtraction of the "map" origin. Meaning the frame_id of "map" is not currently correct for the local position plugin**.

I don't agree with this. According to https://www.ros.org/reps/rep-0105.html, map doesn't have to be globally referenced, just the pose of a mobile platform, relative to the map frame, should not significantly drift over time. You can read that it's well suited for a global reference, but then, on the REP, you can also read that map coordinate frames can either be referenced globally or to an application specific position. This is applicable to pose estimation which main sour is GPS. Since the default sensor setup expected to be used on the autopilot is GPS, the default data is expected to be globally referenced, so map is perfectly well suited as the default frame of reference. If the user/developer expects an pose estimate that drifts over time, then it should use the odom frame.

tstastny commented 4 years ago

I think you are mixing Mavlink with PX4

Fair point. However, I regret mentioning GPS_GLOBAL_ORIGIN at all now, as it doesn't really have much to do with my main point here. I've compiled sources (in PX4 firmware) of data for the respective mavlink msgs (when using PX4) below to hopefully better illustrate my primary point of concern.

In the local position plugin, the xyz positions from the LOCAL_POSITION_NED mavlink msg are read in and (after a flip to ENU) directly set as xyz for the MAVROS local position (specifically here I'm talking about those published from the local position plugin.. not the global position plugin subhandle "local"). At the same time, the parent frame is set to "map", which implies that these position values are describing the translation from "map" origin (where ever it may be) to the "baselink" frame, in this case.

In general, you are right, it doesn't really matter where the "map" origin is.. but if we want to consider multiple topics defined in a "map" frame (e.g. for visualization in rviz), we need to have a common/consistent definition. At least one origin definition is given in the global position plugin, again if home position is received, it references all new lla readings to this.

The problem comes only now, that if the "map" origin is referenced to home position, as defined in the global position plugin, then these local positions from the local position plugin must be incorrect. As you previously stated, the PX4 internally defined local positions are referenced to estimator start, not to home position, this can be seen here: https://github.com/PX4/Firmware/blob/13a9b552c5358a64202272f1ee77ca6d059bc45b/src/modules/ekf2/ekf2_main.cpp#L1274-L1275 This uorb then fills out the mavlink LOCAL_POSITION_NED here: https://github.com/PX4/Firmware/blob/13a9b552c5358a64202272f1ee77ca6d059bc45b/src/modules/mavlink/mavlink_messages.cpp#L2616-L2618

So the point here - is that e.g. if one were to now visualize /mavros/local_position/pose at the same time as /mavros/global_position/local in rviz, the positions would be offset from each other by whatever the offset from estimator start to home position is.

Does anyone actually use both at the same time? I dont know, probably not. But the point is that tagging both messages with the "map" parent frame is inconsistent in this case. This could potentially further have repercussions in e.g. visualizing waypoints etc from a mission along side position trajectories if the definitions are not kept common. But I'll note that I have not looked at these plugins, this is just for sake of argument for a common definition of a given frame.

If "map" is intended to be loosely defined like this, and only consistent with one given plugin at a time, and it is up to the user to know what data is coming through, that's ok.. but I feel like this could definitely be confusing for people (it was for me) and should probably be clearly stated.

If I'm still misunderstanding something significant here.. I'd be happy to move this conversation to slack if you like -- as maybe we are getting too much into the weeds for what anyone would actually be interested to read here on the issue thread. -- Thanks for your patience and dialogue.