Hello everyone!
I’m facing an issue while using the DroneKit library to control a drone in the Gazebo simulator via MQTT. The drone successfully completes its mission and returns home, continuously sending telemetry to the server throughout the mission. However, there’s a peculiar issue: during the return home, telemetry transmission inexplicably stops.
This issue occurs within the while loop during the drone’s return home (the drone loses connection halfway through the return journey). In other words, the loop doesn’t terminate; there are no errors, it just stops listening and executing. Interestingly, if the drone starts the mission while already airborne, everything works perfectly – it returns and lands without any issues.
import threading
import paho.mqtt.client as mqtt
import json
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
class DroneController:
def __init__(self, drone_id, connection_string, mqtt_username, mqtt_password, mqtt_broker_address, mqtt_port):
self.drone_id = drone_id
self.connection_string = connection_string
self.vehicle = connect(connection_string, wait_ready=True)
self.client = mqtt.Client()
self.client.username_pw_set(mqtt_username, mqtt_password)
self.client.connect(mqtt_broker_address, mqtt_port, 60)
self.client.subscribe(f"{self.drone_id}_get_mission")
self.client.on_message = self.on_message
self.mission_id = 0
self.mode_status = "Waiting"
self.change_status = "Waiting"
self.coordinates_home = {"lat": -35.3632622, "lon": 149.1652375}
def send_drone_data(self):
data = {
"Altitude": self.vehicle.location.global_relative_frame.alt,
"Coordinates": {"lat": self.vehicle.location.global_relative_frame.lat, "lon": self.vehicle.location.global_relative_frame.lon},
"Battery Level": self.vehicle.battery.level,
"Heading": self.vehicle.heading,
"Speed": self.vehicle.groundspeed,
"Status": str(self.mode_status),
"mission_id": self.mission_id
}
self.client.publish(f"{self.drone_id}_data", json.dumps(data))
def execute_mission(self, mission_data):
try:
print(f"Выполнение миссии для дрона {self.drone_id}...")
self.mission_id = mission_data["missionId"]
self.client.publish(f"{self.drone_id}_mission_received", str(self.mission_id))
points = mission_data["points"]
speed = mission_data.get("speed", 10)
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.armed = True
while not self.vehicle.armed:
self.client.loop()
print(f"Ожидание включения моторов для дрона {self.drone_id}...")
time.sleep(1)
self.takeoff(points[0]["height"])
self.mode_status = 'mission accomplishment'
self.change_status = 'mission accomplishment'
self.client.publish(f"{self.drone_id}_change_status", str(self.change_status))
while not self.vehicle.mode == VehicleMode("GUIDED"):
self.client.loop()
time.sleep(1)
print("Start miss")
height_h = 20
for point in points:
lat = point["lat"]
lon = point["long"]
height = point["height"]
target_location = LocationGlobalRelative(lat, lon, height)
self.vehicle.simple_goto(target_location)
while (
abs(self.vehicle.location.global_relative_frame.lat - lat) > 0.0001 or
abs(self.vehicle.location.global_relative_frame.lon - lon) > 0.0001
):
self.client.loop()
time.sleep(1)
print("Полет до точки завершен")
self.client.publish(f"{self.drone_id}_mission_completed", str(self.mission_id))
print(f"Миссия для дрона {self.drone_id} завершена, возвращаемся домой")
self.mode_status = "returning home"
self.change_status = 'returning home'
self.client.publish(f"{self.drone_id}_change_status", str(self.change_status))
lat_home = self.coordinates_home["lat"]
lon_home = self.coordinates_home["lon"]
target_location = LocationGlobalRelative(lat_home, lon_home, height_h)
self.vehicle.simple_goto(target_location)
a = 0
while (
abs(self.vehicle.location.global_relative_frame.lat - lat_home) > 0.0001 or
abs(self.vehicle.location.global_relative_frame.lon - lon_home) > 0.0001
):
print("Inside the loop")
print(f"Latitude difference: {abs(self.vehicle.location.global_relative_frame.lat - lat)}")
print(f"Longitude difference: {abs(self.vehicle.location.global_relative_frame.lon - lon)}")
self.client.loop()
print(a)
a += 1
time.sleep(1)
print("Still inside the loop")
print("Exited the loop")
print("Садимся")
self.vehicle.mode = VehicleMode("LAND")
while not self.vehicle.mode == VehicleMode("LAND"):
self.client.loop()
time.sleep(1)
self.mode_status = "waiting"
self.change_status = 'waiting'
self.client.publish(f"{self.drone_id}_change_status", str(self.change_status))
except Exception as e:
print(f"Ошибка выполнения миссии для дрона {self.drone_id}: {e}")
finally:
print("FFFFFF")
self.vehicle.mode = VehicleMode("LAND")
time.sleep(5)
self.vehicle.armed = False
def takeoff(self, aTargetAltitude):
print(f"Взлет на высоту {aTargetAltitude} метров для дрона {self.drone_id}")
self.vehicle.simple_takeoff(aTargetAltitude)
self.mode_status = "takeoff"
self.change_status = 'takeoff'
self.client.publish(f"{self.drone_id}_change_status", str(self.change_status))
while True:
self.client.loop()
print(f"Высота для дрона {self.drone_id}: {self.vehicle.location.global_relative_frame.alt}")
if self.vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.90:
print(f"Достигнута целевая высота для дрона {self.drone_id}")
break
time.sleep(1)
def on_message(self, client, userdata, msg):
if msg.topic.endswith("_get_mission"):
try:
print(f"Received mission message for drone {self.drone_id}: {msg.payload.decode()}")
mission_data = json.loads(msg.payload.decode())
self.execute_mission(mission_data)
except json.JSONDecodeError as e:
print(f"Error decoding mission message for drone {self.drone_id}: {e}")
# Функция для запуска контроллера дрона в отдельном потоке
def start_drone_controller(drone_controller):
drone_controller.client.loop_start()
try:
while True:
drone_controller.send_drone_data()
time.sleep(1)
except KeyboardInterrupt:
print(f"Выход для дрона {drone_controller.drone_id} по запросу пользователя")
finally:
print("FFFFF")
drone_controller.vehicle.close()
drone_controller.client.loop_stop()
drone_controller.client.disconnect()
# Создаем объекты для каждого дрона
drone1 = DroneController(1, '127.0.0.1:14550', 'u_Q9DSA7', 'sYIzz6q0', 'm4.wqtt.ru', 10294)
#drone2 = DroneController(2, '127.0.0.1:14560', 'u_Q9DSA7', 'sYIzz6q0', 'm4.wqtt.ru', 10294)
#drone3 = DroneController(3, '127.0.0.1:14570', 'u_Q9DSA7', 'sYIzz6q0', 'm4.wqtt.ru', 10294)
#drone4 = DroneController(4, '127.0.0.1:14580', 'u_Q9DSA7', 'sYIzz6q0', 'm4.wqtt.ru', 10294)
#drone5 = DroneController(5, '127.0.0.1:14590', 'u_Q9DSA7', 'sYIzz6q0', 'm4.wqtt.ru', 10294)
threading.Thread(target=start_drone_controller, args=(drone1,)).start()
#threading.Thread(target=start_drone_controller, args=(drone2,)).start()
#threading.Thread(target=start_drone_controller, args=(drone3,)).start()
#threading.Thread(target=start_drone_controller, args=(drone4,)).start()
#threading.Thread(target=start_drone_controller, args=(drone5,)).start()
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print("Выход из программы по запросу пользователя")