|
1 | 1 | """ |
2 | | -Copyright (C) Microsoft Corporation. |
| 2 | +Copyright (C) Microsoft Corporation. |
3 | 3 | Copyright (C) 2025 IAMAI Consulting Corp. |
4 | 4 | MIT License. All rights reserved. |
5 | 5 |
|
6 | | -Demonstrates flying a quadrotor drone with camera sensors. |
| 6 | +Secondary controller+viewer: flies the drone AND subscribes to camera topics to show images. |
7 | 7 | """ |
8 | 8 |
|
9 | 9 | import asyncio |
| 10 | +import signal |
10 | 11 |
|
11 | 12 | from projectairsim import ProjectAirSimClient, Drone, World |
12 | 13 | from projectairsim.utils import projectairsim_log |
13 | 14 | from projectairsim.image_utils import ImageDisplay |
14 | 15 |
|
15 | | -# Async main function to wrap async drone commands |
| 16 | + |
16 | 17 | async def main(): |
17 | | - # Create a Project AirSim client |
18 | 18 | client = ProjectAirSimClient() |
19 | | - |
20 | | - # Initialize an ImageDisplay object to display camera sub-windows |
21 | 19 | image_display = ImageDisplay() |
22 | 20 |
|
| 21 | + # Graceful shutdown with Ctrl+C |
| 22 | + stop_event = asyncio.Event() |
23 | 23 | try: |
24 | | - # Connect to simulation environment |
25 | | - client.connect() |
26 | | - |
27 | | - # Create a World object to interact with the sim world and load a scene |
28 | | - world = World(client, "scene_two_drones.jsonc", delay_after_load_sec=2, actual_load=False) |
29 | | - |
30 | | - # Create a Drone object to interact with a drone in the loaded sim world |
31 | | - drone = Drone(client, world, "Drone2") |
32 | | - |
33 | | - # ------------------------------------------------------------------------------ |
| 24 | + signal.signal(signal.SIGINT, lambda *_: stop_event.set()) |
| 25 | + except Exception: |
| 26 | + pass |
34 | 27 |
|
| 28 | + try: |
| 29 | + # 1) Connect to the running simulation (primary already loaded the scene) |
| 30 | + client.connect() |
35 | 31 |
|
36 | | - # Set the drone to be ready to fly |
| 32 | + # 2) Attach to current world without loading a new scene |
| 33 | + world = World(client) |
| 34 | + |
| 35 | + # 3) Attach to the existing drone |
| 36 | + drone = Drone(client, world, "Drone1") |
| 37 | + |
| 38 | + # ----------------- SUBSCRIPTIONS (viewer) ----------------- |
| 39 | + # Downward RGB |
| 40 | + rgb_win = "RGB-Image (Secondary)" |
| 41 | + image_display.add_image(rgb_win, subwin_idx=0) |
| 42 | + try: |
| 43 | + down_rgb_topic = drone.sensors["DownCamera"]["scene_camera"] |
| 44 | + except Exception: |
| 45 | + down_rgb_topic = "/Sim/SceneBasicDrone/robots/Drone1/sensors/DownCamera/scene_camera" |
| 46 | + client.subscribe(down_rgb_topic, lambda _, msg: image_display.receive(msg, rgb_win)) |
| 47 | + |
| 48 | + # Depth |
| 49 | + depth_win = "Depth-Image (Secondary)" |
| 50 | + image_display.add_image(depth_win, subwin_idx=2) |
| 51 | + try: |
| 52 | + down_depth_topic = drone.sensors["DownCamera"]["depth_camera"] |
| 53 | + except Exception: |
| 54 | + down_depth_topic = "/Sim/SceneBasicDrone/robots/Drone1/sensors/DownCamera/depth_camera" |
| 55 | + client.subscribe(down_depth_topic, lambda _, msg: image_display.receive(msg, depth_win)) |
| 56 | + |
| 57 | + # Chase cam |
| 58 | + chase_win = "ChaseCam (Secondary)" |
| 59 | + image_display.add_chase_cam(chase_win) |
| 60 | + try: |
| 61 | + chase_topic = drone.sensors["Chase"]["scene_camera"] |
| 62 | + except Exception: |
| 63 | + chase_topic = "/Sim/SceneBasicDrone/robots/Drone1/sensors/Chase/scene_camera" |
| 64 | + client.subscribe(chase_topic, lambda _, msg: image_display.receive(msg, chase_win)) |
| 65 | + |
| 66 | + # Start viewer windows |
| 67 | + image_display.start() |
| 68 | + |
| 69 | + # ----------------- FLIGHT SEQUENCE (control) ----------------- |
| 70 | + # Keep the same behavior you had: enable, arm, takeoff, move up/down, land |
37 | 71 | drone.enable_api_control() |
38 | 72 | drone.arm() |
39 | 73 |
|
40 | | - # ------------------------------------------------------------------------------ |
41 | | - |
42 | 74 | projectairsim_log().info("takeoff_async: starting") |
43 | | - takeoff_task = ( |
44 | | - await drone.takeoff_async() |
45 | | - ) # schedule an async task to start the command |
46 | | - |
47 | | - # Example 1: Wait on the result of async operation using 'await' keyword |
| 75 | + takeoff_task = await drone.takeoff_async() |
48 | 76 | await takeoff_task |
49 | 77 | projectairsim_log().info("takeoff_async: completed") |
50 | 78 |
|
51 | | - # ------------------------------------------------------------------------------ |
52 | | - |
53 | | - # Command the drone to move up in NED coordinate system at 1 m/s for 4 seconds |
| 79 | + # Move up 1 m/s for 4 s |
54 | 80 | move_up_task = await drone.move_by_velocity_async( |
55 | 81 | v_north=0.0, v_east=0.0, v_down=-1.0, duration=4.0 |
56 | 82 | ) |
57 | 83 | projectairsim_log().info("Move-Up invoked") |
58 | | - |
59 | 84 | await move_up_task |
60 | 85 | projectairsim_log().info("Move-Up completed") |
61 | 86 |
|
62 | | - # ------------------------------------------------------------------------------ |
63 | | - |
64 | | - # Command the Drone to move down in NED coordinate system at 1 m/s for 4 seconds |
| 87 | + # Move down 1 m/s for 4 s |
65 | 88 | move_down_task = await drone.move_by_velocity_async( |
66 | 89 | v_north=0.0, v_east=0.0, v_down=1.0, duration=4.0 |
67 | | - ) # schedule an async task to start the command |
| 90 | + ) |
68 | 91 | projectairsim_log().info("Move-Down invoked") |
69 | | - |
70 | | - # Example 2: Wait for move_down_task to complete before continuing |
71 | 92 | while not move_down_task.done(): |
72 | 93 | await asyncio.sleep(0.005) |
73 | 94 | projectairsim_log().info("Move-Down completed") |
74 | 95 |
|
75 | | - # ------------------------------------------------------------------------------ |
76 | | - |
77 | 96 | projectairsim_log().info("land_async: starting") |
78 | 97 | land_task = await drone.land_async() |
79 | 98 | await land_task |
80 | 99 | projectairsim_log().info("land_async: completed") |
81 | 100 |
|
82 | | - # ------------------------------------------------------------------------------ |
| 101 | + # Keep windows open until Ctrl+C (optional) |
| 102 | + projectairsim_log().info("Viewer running. Press Ctrl+C to quit.") |
| 103 | + await stop_event.wait() |
83 | 104 |
|
84 | | - # Shut down the drone |
| 105 | + # Disarm/disable after viewing (optional) |
85 | 106 | drone.disarm() |
86 | 107 | drone.disable_api_control() |
87 | 108 |
|
88 | | - # ------------------------------------------------------------------------------ |
89 | | - |
90 | | - # logs exception on the console |
91 | 109 | except Exception as err: |
92 | 110 | projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) |
93 | | - |
94 | 111 | finally: |
95 | | - # Always disconnect from the simulation environment to allow next connection |
96 | 112 | client.disconnect() |
97 | | - |
98 | 113 | image_display.stop() |
99 | 114 |
|
100 | 115 |
|
101 | 116 | if __name__ == "__main__": |
102 | | - asyncio.run(main()) # Runner for async main function |
| 117 | + asyncio.run(main()) |
0 commit comments