setpoint_raw/global not working?

[Changliu52]

This is only bug and feature tracker, please use it
to report bugs or request features.

---

Issue details

I was testing in gazebo simulation.
After offboard mode and arming
I send to mavros/setpoint_raw/global as
header: seq: 1135 stamp: secs: 1497020453 nsecs: 330054297 frame_id: '' coordinate_frame: 5 type_mask: 3064 latitude: 47.3977432251 longitude: 8.54559803009 altitude: 490.0 velocity: x: 0.0 y: 0.0 z: 0.0 acceleration_or_force: x: 0.0 y: 0.0 z: 0.0 yaw: -1.57079637051 yaw_rate: 0.0

while monitoring /mavros/global_position/global showing
header: seq: 7322 stamp: secs: 1497020388 nsecs: 743959040 frame_id: fcu status: status: 0 service: 1 latitude: 47.397742 longitude: 8.5455937 altitude: 488.158 position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] position_covariance_type: 1

The quad in simulation stays on the ground armed without taking off.
The same setup was working with setpoint_raw/local without problem.

MAVROS version and platform

Mavros: ?0.18.4?
ROS: indigo
Ubuntu:14.04

Autopilot type and version

[ ] ArduPilot
[X] PX4

Version: v1.5.5

Node logs

copy output of mavros_node. Usually console where you run roslaunch

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

Check ID

rosrun mavros checkid

[Changliu52]

I just found that it went into the failsafe mode after arming.
I had 20 setpoint_raw/global sent before going offboard.
This works for setpoint_raw/local...

[TSC21]

@Changliu52 PX4 doesn't handle POSITION_TARGET_GLOBAL_INT in the mavlink_receiver. That's why it doesn't work.

[Changliu52]

OK, I see, Thanks @TSC21 . The mavlink in px4 doesn't handle any position set point...

So what is the appropriate way to command GPS location in offboard mode?
Do I have to project the target global position onto the local frame and do local setpoint?
If yes, any suggestion on how to do the projection?

Thank you advance.

Chang

[TSC21]

The projection is a thing already going on in the PX4 repo. Check https://github.com/PX4/Firmware/tree/global_to_local

[TSC21]

The mavlink in px4 doesn't handle any position set point...

What do you mean? https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L210

[Changliu52]

Sorry typo the px4 v1.5.5 doesn't handle any global position set point.

[TSC21]

1.5.5? I'm mostly sure it does.

[Changliu52]

Oh, so what did I do wrong? I just need to command global set point in offboard mode.
Did you mean in the first reply that I just need to change to a different coordinate frame rather than FRAME_GLOBAL_INT=5? would the FRAME_GLOBAL_REL_ALT = 6 work?
Thanks

[TSC21]

Sorry typo the px4 v1.5.5 doesn't handle any global position set point.

Sorry I read wrong, it allows local position but not global.

[TSC21]

Did you mean in the first reply that I just need to change to a different coordinate frame rather than FRAME_GLOBAL_INT=5? would the FRAME_GLOBAL_REL_ALT = 6 work?

No. You have three options:

1. You do the projection on ROS side and send it through SET_POSITION_TARGET_LOCAL_NED. For that, Use GeographicLib tools to guarantee ROS msg def and enhance features #693 would help you since I'm integrating GeographicLib functions into MAVROS, which allows conversion from geodetic (LLA) to geocentric (local) coordinates and vice-versa. If you choose to do that, a PR to MAVROS is welcomed which implements that functionality in setpoint_position plugin (setpoint_raw is generic);
2. Do a PR to the PX4 repo that implements the support to SET_POSITION_TARGET_GLOBAL_INT, which also has to include its support on the position controllers (MC and FW);
3. You wait that https://github.com/PX4/Firmware/tree/global_to_local gets merged, which can take some time.

Will gladly review the implementation of 1 and 2 if you decide to move forward to it.

[Changliu52]

Interesting. For the 1st option, you seems to have integrated the GeographicLib successfully. So I guess all I need to do is to use some of the functions to do the conversion and use the local setpoint for control.

Which function from GeographicLib should I use to do the conversion? I am not really familiar with the library. Thanks

[TSC21]

Which function from GeographicLib should I use to do the conversion?

https://geographiclib.sourceforge.io/html/classGeographicLib_1_1Geocentric.html
You should apply the reverse calculation (.Reverse(X, Y, Z, lat, lon, h)). Just be sure you have your local frame (base_link) well defined and the origin of the map frame also. This one can be the home_position, and in that case, the home_position should also be the origin of the ENU/NED world frame.

[TSC21]

Also in addiction, in setpoint_position you can add a subscriber to a topic that publishes geographic_msgs/GeoPointStamped. Then you take the coordinates from this one, apply the conversion to geocentric coordinates, calculate the offset to the origin (if local=global, then the origin is the same) and publish the setpoints to the MAVLink stream.

[Changliu52]

Really sorry @TSC21 , having looked at the packages, It seems really difficult for me to do this at the moment, I have to make the drone follow the GPS waypoint offboard this week. I will come back for the develop in later stage. Maybe sending waypoint service request to mavros/mission/push would work?

[TSC21]

Maybe sending waypoint service request to mavros/mission/push would work?

You can only use that for geodetic (LLA) coordinates.

[TSC21]

I'm closing this for now. @Changliu52 let us know if you got a solution for your case in the meantime. Feel free to open a PR with useful changes and features regarding sending global setpoints from ROS side.