let’s now do the same circle mission upload using a Python script with MAVROS.
This script will:
AUTO.MISSION
mission_circle.py
#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from mavros_msgs.srv import CommandBool, SetMode, WaypointPush, WaypointClear
from mavros_msgs.msg import Waypoint
class MissionCircle(Node):
def __init__(self):
super().__init__('mission_circle')
# Initialize service clients
self.clear_cli = self.create_client(WaypointClear, '/mavros/mission/clear')
self.push_cli = self.create_client(WaypointPush, '/mavros/mission/push')
self.arm_cli = self.create_client(CommandBool, '/mavros/cmd/arming')
self.mode_cli = self.create_client(SetMode, '/mavros/set_mode')
#Creates clients for clearing missions, uploading new ones, arming, and changing flight modes, these are all one time jobs so we create clients to communicate through service
self.get_logger().info("MissionCircle node started, preparing mission...")
# upload_mission function is called every 3 seconds
self.timer = self.create_timer(3.0, self.upload_mission)
def upload_mission(self):
self.timer.cancel() #we only run this function once, so once it is called after 3 seconds just cancel the timer
# ---- 1. Clear old mission ----
if self.clear_cli.wait_for_service(timeout_sec=5.0):
self.clear_cli.call_async(WaypointClear.Request())
self.get_logger().info("Cleared old mission")
# ---- 2. Generate circle waypoints ----
lat0 = 47.397750 # reference latitude (px4본사 취리히 좌표..실제로 날릴때는 우리자표 써야함, somehow we should be able to get it from gps or mavros message
lon0 = 8.545594 # reference longitude
alt = 10.0 # altitude in meters
radius = 0.00015 # ~15m radius in GPS degrees (~1e-5 deg ≈ 1.1 m)
n_wp = 12 # number of waypoints around the circle
waypoints = [] #make a list to store way points
#the first waypoints is one just taking off
wp0 = Waypoint()#make waypoint variable with data type of Waypoint() msg
wp0.frame = 3 # MAV_FRAME_GLOBAL_REL_ALT
wp0.command = 22 # MAV_CMD_NAV_TAKEOFF
wp0.is_current = True
wp0.autocontinue = True
wp0.x_lat = lat0
wp0.y_long = lon0
wp0.z_alt = alt
waypoints.append(wp0) #make waypoint and add to waypoints list
# after the initialize waypoints, the next ones are circle waypoints
for i in range(n_wp):
theta = 2.0 * math.pi * i / n_wp
wp = Waypoint()
wp.frame = 3
wp.command = 16 # MAV_CMD_NAV_WAYPOINT
wp.is_current = False
wp.autocontinue = True
wp.x_lat = lat0 + radius * math.cos(theta)
wp.y_long = lon0 + radius * math.sin(theta)
wp.z_alt = alt
waypoints.append(wp)
self.get_logger().info(f"Generated {len(waypoints)} waypoints in a circle")
# ---- 3. Push mission ----
if self.push_cli.wait_for_service(timeout_sec=5.0):
req = WaypointPush.Request()
req.start_index = 0
req.waypoints = waypoints
future = self.push_cli.call_async(req)
future.add_done_callback(lambda f: self.get_logger().info(f"Mission upload success: {f.result().success}"))
# ---- 4. Arm the drone ----
self._arm(True)
# ---- 5. Switch to AUTO.MISSION ----
self._set_mode("AUTO.MISSION")
def _arm(self, value: bool):
if self.arm_cli.wait_for_service(timeout_sec=5.0):
req = CommandBool.Request()
req.value = value
self.arm_cli.call_async(req)
self.get_logger().info(f"ARM({value}) requested")
def _set_mode(self, mode: str):
if self.mode_cli.wait_for_service(timeout_sec=5.0):
req = SetMode.Request()
req.custom_mode = mode
self.mode_cli.call_async(req)
self.get_logger().info(f"SET_MODE({mode}) requested")
def main():
rclpy.init()
node = MissionCircle()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
AUTO.MISSION
.PX4 will now fly the polygonal circle by itself, even if you shut the node down.