YOLO, I’m writing code to publish Gotrun’s cv2 screen to Ros Detection in real time, then subscribe to that image and use mavsdk to issue commands to control the drone based on the size and position of the bounding box.
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(p, r, t, y))
When I give this control command, the drone turns on and off again. I’m wondering why?
import asyncio
from mavsdk import System
from std_msgs.msg import String
from mavsdk.offboard import (VelocityBodyYawspeed,VelocityNedYaw, PositionNedYaw, OffboardError)
import rospy
class Navigation:
def __init__(self):
self.detection_info = ""
self.centerx = 240
self.centery = 220
self.height = 100
self.tolx = 20
self.toly = 20
self.tolh = 20
self.drone = System()
def detection_cb(self, msg):
self.detection_info = msg.data
async def setup_and_takeoff(self):
await self.drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in self.drone.core.connection_state():
if state.is_connected:
print(f"-- Connected to drone!")
break
print("Waiting for drone to have a global position estimate...")
async for health in self.drone.telemetry.health():
if health.is_global_position_ok and health.is_home_position_ok:
print("-- Global position estimate OK")
break
print("-- Arming")
await self.drone.action.arm()
print("-- Setting initial setpoint")
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await self.drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await self.drone.action.disarm()
await self.control_loop()
async def control_loop(self):
while True :
cx, cy, h = self.detection_info.split()
cx = int(float(cx))
cy = int(float(cy))
h = int(float(h))
p,r,t,y = 0.0, 0.0, 0.0, 0.0
if abs(cx - self.centerx) > self.tolx:
if cx - self.centerx <= 0:
r = 1.0
else:
r = -1.0
if abs(cy - self.centery) > self.toly:
if cy - self.centery > 0:
t = -5.0
else:
t = 5.0
if abs(h - self.height) > self.tolh:
if h - self.height > 0:
p = -1.0
else :
p = 1.0
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(p, r, t, y))
await asyncio.sleep(5)
if __name__ == "__main__":
rospy.init_node("detection", anonymous=False)
navigation = Navigation()
detection_sub = rospy.Subscriber("detection_info", String, callback=navigation.detection_cb)
loop = asyncio.get_event_loop()
loop.run_until_complete(navigation.setup_and_takeoff())