MAV drifting away from its takeoff position
I am working on MAV control in GPS denied environment using px4flow and Lidar sensor.While in simulation and during experiments on real vehicle, MAV doesnt stay at its position and drifts in random direction. I am using /mavros/local_position/pose msg to provide setpoints to the MAV. Currently it just have to takeoff hover and land at the same position. What might be causing this? Here is the code for your reference.
`!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point, PoseStamped
from mavros_msgs.msg import * from mavros_msgs.srv import *
class fcuModes: def __init__(self): pass
def setTakeoff(self):
rospy.wait_for_service('mavros/cmd/takeoff')
try:
takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
takeoffService(altitude = 3)
except rospy.ServiceException, e:
print "Service takeoff call failed: %s"%e
def setArm(self):
rospy.wait_for_service('mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
armService(True)
except rospy.ServiceException, e:
print "Service arming call failed: %s"%e
def setDisarm(self):
rospy.wait_for_service('mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
armService(False)
except rospy.ServiceException, e:
print "Service disarming call failed: %s"%e
def setStabilizedMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='STABILIZED')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. Stabilized Mode could not be set."%e
def setOffboardMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='OFFBOARD')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. Offboard Mode could not be set."%e
def setAltitudeMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='ALTCTL')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. Altitude Mode could not be set."%e
def setPositionMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='POSCTL')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. Position Mode could not be set."%e
def setAutoLandMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='AUTO.LAND')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. Autoland Mode could not be set."%e
class Controller:
def __init__(self):
# Drone state
self.state = State()
# Instantiate a setpoints message
self.sp = PositionTarget()
# set the flag to use position setpoints and yaw angle
self.sp.type_mask = int('010111111000', 2)
# LOCAL_NED
self.sp.coordinate_frame = 1
# We will fly at a fixed altitude for now
# Altitude setpoint, [meters]
self.ALT_SP = 1.5
# update the setpoint message with the required altitude
self.sp.position.z = self.ALT_SP
# Step size for position update
self.STEP_SIZE = 2.0
# Fence. We will assume a square fence for now
self.FENCE_LIMIT = 5.0
# A Message for the current local position of the drone
self.local_pos = Point(0.0, 0.0, 1.0)
# initial values for setpoints
self.sp.position.x = 0.0
self.sp.position.y = 0.0
# speed ...
Does it stay still in the POSCTL mode?
yes it does.
Then it is capable of holding its position. If you need to control it in offboard mode, and make it act same, inspect its local position information from mavros topics and send alike commands to make it stand still. I'm writing as answer since I need to copy your codes.