setpoint_position: accept set-position-target-global-int messages

[rmackay9]

This PR attempts to allow mavros to accept SET_POSITION_TARGET_GLOBAL_INT messages from the flight controller or GCS. This is useful because it can allow a ground station operator to pass the position target to ROS (so that it can do the path planning) without using rviz.

Below is an image of a test using Mission Planner attached to an ArduPilot Rover with ROS running on a tx2. I was able to select ROS using MP's upper-left vehicle selector and the right-mouse-button-click and select, "Fly To Here". When mavros consumes the message it converts the latitude, longitude, altitude into ENU and publishes it to /mavros/setpoint_position/cmd_pos topic

Mavros's topic can be remapped to ROS's base local planner's input (setup is described here) so that it can accept the target.

While testing I found two issues which I'm hoping the reviewers (@vooon?) can give me a hand in resolving:

• something seems incorrect with the conversion from lat/lon to ENU. The north-south values seem to be reversed. If I click to the north of the vehicle (in mission planner) the target (as viewed in rviz) appears to the south of the vehicle (or more accurately to the right of the vehicle which is facing east). The East-West calculations seem ok.
• I had to force the z-axis output to zero (see comment, "force z-axis to zero") because I'm using a rover and ROS's navigation didn't seem happy unless the target has position of zero.

Anyway, all feedback is welcome, thanks!

[TSC21]

SET_ prefixes are usually used for messages you send to the FCU, not something you receive from. I understand that what you are doing is basically expect that the data coming from the GCS gets parsed here, since what MAVROS does is nothing more than serve as a proxy to forward messages, but we should not mess how we process data that comes from the FCU and the data that comes from the GCS. This last one should be handled separately.

If the data comes from the FCU, it needs to come in POSITION_TARGET_GLOBAL_INT messages.

[TSC21]

Btw, for specific GCS data:

• https://github.com/mavlink/mavros/projects/2: still empty but can this feature can be added as an extension to it;
• GCS Bridge Plugin #1117: gives a good example on how to separate streams.

[rmackay9]

@TSC21, thanks for the feedback and links. I see what you're saying about keeping the handling of messages from the GCS separate from those received from the FCU. I was planning to actually make it possible for the vehicle to send set-position-target-local-ned messages so that ROS's navigation can help the vehicle get around some obstacles close to the vehicle.

In any case, I'll think about it a bit more and I'm still trying to find the time to resolve the frame conversion issue in this PR.

[rmackay9]

I think this PR is working now and I've done a blog on ardupilot.org including a couple of videos (auto mode, guided mode) showing how ROS's path planner can be used as an external navigation source for object avoidance.

@vooon, any chance this can be merged?

I'm happy to make changes but I probably need some more specific feedback on what needs changing.

I understand @TSC21's comments that we need to be careful of mixing up commands received from a GCS and those received from a vehicle but with these enhancements to ArduPilot Rover, the position target is now coming from the vehicle.

At the risk of overblowing this minor change, I think it a nice enhancement that allows the flight code to remain in control and only use ROS as a navigation tool when it wants to.

[TSC21]

@rmackay9 just because something is working for a particular case doesn't mean that we are doing things thr correct way. There is an API and an implementation logic that should always be respected, unless there's nothing we can do besides messing with the API logic.
As you can read on my review, I didn't just referenced the mixing of GCS and FCU messages. I also said that SET_ prefix messages are sent to the flight controller, not coming from it. You can use the counter-part message to get the data from the FCU. Using the SET_... messages here is, as I said, breaking the API and the implementation logic of this package. Doesn't make sense to have a group of messages which usage is respecting the API implementation and then have a case where this is change with no aparent reason.

[TSC21]

I'm happy to make changes but I probably need some more specific feedback on what needs changing.

As a reviewer, I gave a specific feedback on what needs to be changed :) I am happy to help you make this implementation correct (not make it work, cause it seems to be).

I understand @TSC21's comments that we need to be careful of mixing up commands received from a GCS and those received from a vehicle but with these enhancements to ArduPilot Rover, the position target is now coming from the vehicle.

I wonder if this is correct from the Mavlink API standpoint. Again, SET_ prefixes is something you send from a offboard controller to the FCU, not the other way around.

[rmackay9]

@TSC21, thanks again for the feedback. So the objection is with the SET_TARGET_POSITION_GLOBAL_INT messages coming from the FCU? I think we haven't had SET_ messages come from the FCU only because we haven't previously tried to make the flight controller the "master". I.e. the flight controller has always been the "leaf".. it's been pushed around by upper level controllers within ROS. I don't really see why there would be a restriction on a flight controller providing a position target to ROS's navigation controller... and if we accept that it is OK for a flight controller to provide a position target to ROS, I don't see how it would get it there unless it used the SET_TARGET_POSITION_GLOBAL_INT message (or the very similar SET_TARGET_POSITION_LOCAL_NED)..

.. by the way, I'm very happy to having a voice chat somewhere if that would help speed things along.. we could use AP's mumble server but skype or hangouts or whatever also works for me.

[TSC21]

@rmackay9 this is not a matter of what has been tried or not. Is what the API is set to do and changing that paradigm first needs a discussion on the Mavlink API. We can't just assume we can use a message in a certain way just becaude it works. Then we would be using all the messages the way we wanted.
You can actually read on the message description the following:

"Used by an external controller to command the vehicle (manual controller or other system)."

If one wants to update the Mavlink protocol API, then one should suggest that same change first, and not apply the message in a certain way that contradicts the API.

[vooon]

Besides really confusing message usage (with it's description),
I'd like to see that as a separate plugin. That should give more clear separation of commanding setpoints
and "guidance receiver" (better to find new name, that will not include setpoint, as they treated as "fly there" command).

[vooon]

Also perhaps that should be in extras, as it have very specific usage.