Hello I am running into some issues with a SITL simulation in Gazebo. I followed the install instructions here and can run the offboard_attitude.py example script (and others) included in the MAVSDK-Python repo. Therefore, the base environment appears to be functioning correctly. For reference:
- MAVSDK-Python 0.8.0
- Python 3.8.2
- Ubuntu 20.04 LTS
- PX4 1.11.0beta
- Gazebo 9.13
- QGroundControl 4.0.7
I am trying to achieve offboard position control by adjusting the attitude of the UAV. I will be in a GPS denied environment in the future and will get positional data using a camera. My simulation works about 30% of the time otherwise I receive the following error:
Traceback (most recent call last):
File "/home/mark/Documents/UAV-Sampling-Control-System/SITL/test.py", line 130, in <module>
loop.run_until_complete(run(drone))
File "/usr/lib/python3.8/asyncio/base_events.py", line 616, in run_until_complete
return future.result()
File "/home/mark/Documents/UAV-Sampling-Control-System/SITL/test.py", line 96, in run
x, y, z = await getPos(drone, xCal=xZero, yCal=yZero)
File "/home/mark/Documents/UAV-Sampling-Control-System/SITL/test.py", line 17, in getPos
async for pos in drone.telemetry.position():
File "/home/mark/.local/lib/python3.8/site-packages/mavsdk/generated/telemetry.py", line 2841, in position
async for response in position_stream:
File "/home/mark/.local/lib/python3.8/site-packages/aiogrpc/utils.py", line 138, in __anext__
return await asyncio.shield(self._next_future, loop=self._loop)
File "/usr/lib/python3.8/concurrent/futures/thread.py", line 57, in run
result = self.fn(*self.args, **self.kwargs)
File "/home/mark/.local/lib/python3.8/site-packages/aiogrpc/utils.py", line 126, in _next
return next(self._iterator)
File "/home/mark/.local/lib/python3.8/site-packages/grpc/_channel.py", line 416, in __next__
return self._next()
File "/home/mark/.local/lib/python3.8/site-packages/grpc/_channel.py", line 706, in _next
raise self
grpc._channel._MultiThreadedRendezvous: <_MultiThreadedRendezvous of RPC that terminated with:
status = StatusCode.UNAVAILABLE
details = "Socket closed"
debug_error_string = "{"created":"@1590993102.526978113","description":"Error received from peer ipv6:[::1]:50051","file":"src/core/lib/surface/call.cc","file_line":1056,"grpc_message":"Socket closed","grpc_status":14}"
>
The same time I get the above error QGroundControl states `Failsafe enabled: no offboard’. Before I put this onto some hardware are there any ideas for this error? My best guess is that I may be flooding the autopilot. I have the main loop running at 30 Hz although the asyncio functions may cause the data to be slightly faster or slower at times.
The relevant code is posted below:
import asyncio
import time
import utm
from mavsdk import System
from mavsdk import (Attitude, AttitudeRate, OffboardError, Telemetry)
from controller import Controller
async def getAttitude(drone):
# Get UAV body attitude
async for bodyAttitude in drone.telemetry.attitude_euler():
return bodyAttitude.roll_deg, bodyAttitude.pitch_deg, bodyAttitude.yaw_deg
async def getPos(drone, xCal=0, yCal=0):
# Get GPS position data and turn into x and y -> Replace with vision in future
async for pos in drone.telemetry.position():
lat, lon, z = pos.latitude_deg, pos.longitude_deg, pos.relative_altitude_m
x, y, _, _ = utm.from_latlon(lat, lon)
return (x-xCal), (y-yCal), z
async def stabilizeLoop(stabilizeTimer):
# Set loop rate
totalLoopTime = 1/30
# Find current loop time and execute corresponding delay
timeDiff = time.time() - stabilizeTimer
if (timeDiff >= totalLoopTime):
pass
else:
await asyncio.sleep(totalLoopTime - timeDiff)
async def run(drone):
# Connect to drone
await drone.connect(system_address="udp://:14540")
# Connect to control scheme
C = Controller(0, 0, 1000)
# Connect to UAV
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
# Set message rate for attitude
await drone.telemetry.set_rate_attitude(30)
# Connect to "camera" -> GPS
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
# Arm
print("-- Arming")
await drone.action.arm()
# Zero initial position
xZero = 0
yZero = 0
for _ in range(100):
tempX, tempY, _ = await asyncio.ensure_future(getPos(drone))
xZero += tempX
yZero += tempY
xZero /= 100
yZero /= 100
# Start offboard
print("-- Starting offboard")
for _ in range(200):
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.0))
try:
await drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
# Start timers
C.timer = time.time()
startTime = time.time()
stabilizeTimer = time.time()
# Run sim for 10 secounds
while(time.time() < startTime+10):
# Get data
_, _, yaw = await getAttitude(drone)
x, y, z = await getPos(drone, xCal=xZero, yCal=yZero)
# Run control
rollControl, pitchControl, yawControl, thrustControl = C.positionControl(x*1000, y*1000, z*1000, yaw)
# Execute control
await drone.offboard.set_attitude(Attitude(rollControl, pitchControl, 0.0, thrustControl))
await drone.offboard.set_attitude_rate(AttitudeRate(0.0, 0.0, yawControl, thrustControl))
# Stabilize the sampling rate and save data
await stabilizeLoop(stabilizeTimer)
stabilizeTimer = time.time()
# Stop offboard
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(f"Stopping offboard mode failed with error code: \
{error._result.result}")
# Land
print("-- Landing")
await drone.action.land()
await asyncio.sleep(2)
if __name__ == "__main__":
# Connect to SITL
drone = System()
# Run the main program
loop = asyncio.get_event_loop()
try:
loop.run_until_complete(run(drone))
except KeyboardInterrupt:
print("Closing")
Any assistance into this issue would be greatly appreciated. Thank you in advance!