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?