Mavros setpoint_position/local acceptance

I’m using mavros to guide my quadrotor between a set of waypoints. The most effective way I’ve found to do this while keeping some fine-grained control over the waypoint behavior is to use the setpoint APIs. Specifically, I use the setpoint_position/local topic to publish the waypoint I want the quad to move to and the local_position/pose topic to listen to the current location of the drone to determine if the waypoint has been reached. I calculate this acceptance by measuring an acceptance radius away from the given PoseStamped point.

Here’s the general idea in code:

Goto position:

local_setposition_publisher = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
local_setposition_publisher.publish(waypoint)

Then monitor the quad for waypoint acceptance:

def updatePosition(localposition):
    if distance(self.waypoint.pose.position, localposition.pose.position) < self.distanceThreshold:
        print "Success!"
        self.stop()

rospy.Subscriber("/mavros/local_position/pose",  PoseStamped,  updatePosition)

This works well but sometimes fails. When it fails, the quadrotor approaches a point and stops, sometimes meters away from the desired location. Am I missing something? Does the setpoint_position and local_position work against the same tf frame of reference? Should I be doing something else or take something else into account to determine waypoint acceptance?

1 Like

Any updates on this?