SET_POSITION_TARGET_GLOBAL_INT not working as expected

[antonio-sc66]

Describe the bug When trying to send setpoints with MAVROS using the setpoint_position/global topic with the _geographicmsgs GeoPoseStamped message to send SET_POSITION_TARGET_GLOBAL_INT msgs, the drone tries to reach the indicated latitude and longitude but the altitude isn't working as expected. I still haven't tried to send the Mavlink msgs directly but the MAVROS ones seem well formed (https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/setpoint_position.cpp#L189 and https://github.com/mavlink/mavros/blob/master/mavros/include/mavros/setpoint_mixin.h#L80).

In case you're trying to keep the same altitude and copy the global altitude to the setpoint global altitude the drone skyrockets. If you set altitudes using relative measurements in AGL then the drone starts to descend and ends up either landing or crashing. (MAVROS sets the reference frame to MAV_FRAME_GLOBAL_INT).

During the procedure the PX4 SITL terminal throws the next warnings:

INFO  [commander] Takeoff detected
WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] Failsafe: stop and wait

I've prepared a small Python script that sends setpoints with the current position (drone should stay still) or sends setpoints to the HomePosition location so that it can be easily tested.

I would like to know if I'm doing anything wrong or if this is indeed a bug as local setpoints are working as expected.

To Reproduce Steps to reproduce the behavior:

1. Start simulation with: roslaunch px4 mavros_posix_sitl.launch (https://docs.px4.io/master/en/simulation/ros_interface.html#launching-gazebo-with-ros-wrappers)
2. Takeoff drone
3. Run program to send setpoints or use rostopic pub
4. Switch to offboard mode
5. See error

Expected behavior The drone should move to the desired latitude and longitude and maintain the indicated altitude

Drone (please complete the following information): PX4 SITL Iris Quadcopter

Additional context Tested using master branch (a88b7fc517bff7f236ca4881aaa7abda89a32260) Ubuntu 18.04 with ros-melodic and ros-melodic-mavros pkgs

Python script example to perform the test

import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import NavSatFix
from geographic_msgs.msg import GeoPoseStamped
from mavros_msgs.msg import HomePosition

RATE = 5 # Hz
current_global_pos = NavSatFix()
current_home_pos = HomePosition()

def _global_pos_cb(msg):
    global current_global_pos
    current_global_pos = msg

def _home_pos_cb(msg):
    global current_home_pos
    current_home_pos = msg

global_pos_sub = rospy.Subscriber('/mavros/global_position/global', NavSatFix, _global_pos_cb)
home_pos_sub = rospy.Subscriber('/mavros/home_position/home', HomePosition, _home_pos_cb)
pos_pub = rospy.Publisher("/mavros/setpoint_position/global", GeoPoseStamped, queue_size=1)

def main():
    rospy.init_node('setpoint_test', anonymous=False)
    rate = rospy.Rate(RATE)

    while True:
        msg = GeoPoseStamped()
        header = Header()
        header.stamp = rospy.Time.now()
        msg.header = header
        msg.pose.position.latitude = current_global_pos.latitude
        msg.pose.position.longitude = current_global_pos.longitude
        msg.pose.position.altitude = current_global_pos.altitude
        # Uncomment to go to the HomePosition and keep the current altitude
        # msg.pose.position = current_home_pos.geo

        msg.pose.orientation = current_home_pos.orientation
        pos_pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    main()

[LorenzMeier]

@TSC21 Do you have an example that you could point people to?

[TSC21]

I don't but @Jaeyoung-Lim might have some insights since he extended the setpoint_position plugin with support for sending SET_POSITION_TARGET_GLOBAL_INT messages. But the quickest thing I can think of is that the altitude being sent is the relative instead of the absolute. The plugin by default expects the absolute.

[Jaeyoung-Lim]

@antonio-sc66 Could you try out https://github.com/PX4/PX4-Autopilot/pull/16514 ? This should fix your problem

[antonio-sc66]

Hi, @Jaeyoung-Lim, I will try the fix this weekend

[Jaeyoung-Lim]

@antonio-sc66 Any updates?