From 5d52df2bedf3855fa59f02b0ada713ae643ee80b Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 26 May 2026 14:56:39 -0300 Subject: [PATCH] One parameter world constructor (#9) enables running script after scripts and multiple service-only scripts --- .../hello_drone_secondary.py | 69 ++++++ .../two_drones_split_drone1.py | 88 +++++++ .../two_drones_split_drone2.py | 90 +++++++ .../projectairsim/src/projectairsim/client.py | 101 ++++++-- .../projectairsim/src/projectairsim/drone.py | 223 +++++++++++++++--- .../projectairsim/src/projectairsim/world.py | 43 +++- .../projectairsim/tests/test_api_services.py | 15 ++ core_sim/src/actor/robot.cpp | 34 ++- core_sim/src/scene.cpp | 13 +- core_sim/src/simulator.cpp | 5 + core_sim/src/topic_manager.cpp | 33 ++- core_sim/src/topic_manager.hpp | 2 + 12 files changed, 631 insertions(+), 85 deletions(-) create mode 100644 client/python/example_user_scripts/hello_drone_secondary.py create mode 100644 client/python/example_user_scripts/two_drones_split_drone1.py create mode 100644 client/python/example_user_scripts/two_drones_split_drone2.py diff --git a/client/python/example_user_scripts/hello_drone_secondary.py b/client/python/example_user_scripts/hello_drone_secondary.py new file mode 100644 index 00000000..593469fe --- /dev/null +++ b/client/python/example_user_scripts/hello_drone_secondary.py @@ -0,0 +1,69 @@ +""" +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI Consulting Corp. +MIT License. All rights reserved. + +Secondary controller: flies the drone using only service calls (no topic subscriptions). +""" + +import asyncio + +from projectairsim import ProjectAirSimClient, Drone, World +from projectairsim.utils import projectairsim_log + + +async def main(): + client = ProjectAirSimClient() + + try: + # 1) Connect to the running simulation (primary already loaded the scene) + client.connect() + + # 2) Attach to current world without loading a new scene + world = World(client) + + # 3) Attach to the existing drone + drone = Drone(client, world, "Drone1") + + # ----------------- FLIGHT SEQUENCE (control) ----------------- + drone.enable_api_control() + drone.arm() + + projectairsim_log().info("takeoff_async: starting") + takeoff_task = await drone.takeoff_async() + await takeoff_task + projectairsim_log().info("takeoff_async: completed") + + # Move up 1 m/s for 4 s + move_up_task = await drone.move_by_velocity_async( + v_north=0.0, v_east=0.0, v_down=-1.0, duration=4.0 + ) + projectairsim_log().info("Move-Up invoked") + await move_up_task + projectairsim_log().info("Move-Up completed") + + # Move down 1 m/s for 4 s + move_down_task = await drone.move_by_velocity_async( + v_north=0.0, v_east=0.0, v_down=1.0, duration=4.0 + ) + projectairsim_log().info("Move-Down invoked") + await move_down_task + projectairsim_log().info("Move-Down completed") + + projectairsim_log().info("land_async: starting") + land_task = await drone.land_async() + await land_task + projectairsim_log().info("land_async: completed") + + # Disarm/disable at the end + drone.disarm() + drone.disable_api_control() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + finally: + client.disconnect() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/example_user_scripts/two_drones_split_drone1.py b/client/python/example_user_scripts/two_drones_split_drone1.py new file mode 100644 index 00000000..85663fc4 --- /dev/null +++ b/client/python/example_user_scripts/two_drones_split_drone1.py @@ -0,0 +1,88 @@ +""" +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Control script for Drone1. +This script loads the two-drone scene and controls only Drone1. +""" + +import asyncio + +from projectairsim import Drone, ProjectAirSimClient, World +from projectairsim.image_utils import ImageDisplay +from projectairsim.utils import projectairsim_log + + +def imu_callback_drone1(_, imu_msg): + print(f"[Drone1][IMU] {imu_msg}") + + +async def fly_box_pattern(drone: Drone, label: str): + """Fly a simple box-like path for one drone.""" + cmd_duration_sim_sec = 3 + velocity_mps = 1 + + projectairsim_log().info(f"[{label}] Move Up") + task = await drone.move_by_velocity_async(0, 0, -velocity_mps, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move North") + task = await drone.move_by_velocity_async(velocity_mps, 0, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move West") + task = await drone.move_by_velocity_async(0, -velocity_mps, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move South") + task = await drone.move_by_velocity_async(-velocity_mps, 0, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move East") + task = await drone.move_by_velocity_async(0, velocity_mps, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move Down") + task = await drone.move_by_velocity_async(0, 0, velocity_mps, cmd_duration_sim_sec) + await task + + +async def main(): + client = ProjectAirSimClient() + image_display = ImageDisplay() + + try: + client.connect() + + # Drone1 script is the one that loads the scene. + world = World(client, "scene_two_drones.jsonc", delay_after_load_sec=2) + drone1 = Drone(client, world, "Drone1") + + chase_cam_window = "Drone1-ChaseCam" + image_display.add_chase_cam(chase_cam_window) + client.subscribe( + drone1.sensors["Chase"]["scene_camera"], + lambda _, chase: image_display.receive(chase, chase_cam_window), + ) + client.subscribe(drone1.sensors["IMU1"]["imu_kinematics"], imu_callback_drone1) + image_display.start() + + drone1.enable_api_control() + drone1.arm() + + await fly_box_pattern(drone1, "Drone1") + + drone1.disarm() + drone1.disable_api_control() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + client.disconnect() + image_display.stop() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/example_user_scripts/two_drones_split_drone2.py b/client/python/example_user_scripts/two_drones_split_drone2.py new file mode 100644 index 00000000..f49386ad --- /dev/null +++ b/client/python/example_user_scripts/two_drones_split_drone2.py @@ -0,0 +1,90 @@ +""" +Copyright (C) Microsoft Corporation. +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Control script for Drone2. +This script does NOT reload the scene; it attaches to the current world with World(client). +Run this after the Drone1 script has already loaded the scene. +""" + +import asyncio + +from projectairsim import Drone, ProjectAirSimClient, World +from projectairsim.utils import projectairsim_log + + +def imu_callback_drone2(_, imu_msg): + print(f"[Drone2][IMU] {imu_msg}") + + +async def fly_box_pattern(drone: Drone, label: str): + """Fly a simple box-like path for one drone.""" + cmd_duration_sim_sec = 3 + velocity_mps = 1 + + projectairsim_log().info(f"[{label}] Move Up") + task = await drone.move_by_velocity_async(0, 0, -velocity_mps, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move North") + task = await drone.move_by_velocity_async(velocity_mps, 0, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move West") + task = await drone.move_by_velocity_async(0, -velocity_mps, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move South") + task = await drone.move_by_velocity_async(-velocity_mps, 0, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move East") + task = await drone.move_by_velocity_async(0, velocity_mps, 0, cmd_duration_sim_sec) + await task + + projectairsim_log().info(f"[{label}] Move Down") + task = await drone.move_by_velocity_async(0, 0, velocity_mps, cmd_duration_sim_sec) + await task + + +async def main(): + client = ProjectAirSimClient() + + try: + client.connect() + + # Drone2 script attaches to the already-loaded scene. + world = World(client) + drone2 = Drone(client, world, "Drone2") + + if not client.is_service_only_mode(): + imu_topic = drone2.sensors.get("IMU1", {}).get("imu_kinematics") + if imu_topic: + client.subscribe(imu_topic, imu_callback_drone2) + else: + projectairsim_log().warning( + "[Drone2] IMU topic was not found; continuing without topic subscription." + ) + else: + projectairsim_log().warning( + "[Drone2] Client is running in service-only mode; skipping topic subscription." + ) + + drone2.enable_api_control() + drone2.arm() + + await fly_box_pattern(drone2, "Drone2") + + drone2.disarm() + drone2.disable_api_control() + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + client.disconnect() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/projectairsim/src/projectairsim/client.py b/client/python/projectairsim/src/projectairsim/client.py index 89f537ff..2b0b6c44 100644 --- a/client/python/projectairsim/src/projectairsim/client.py +++ b/client/python/projectairsim/src/projectairsim/client.py @@ -43,14 +43,11 @@ def __init__(self, address="127.0.0.1", port_topics=8989, port_services=8990): self.socket_topics = None self.socket_services = None self.request_id = None + self.topics_enabled = False def connect(self): """Connects to the server""" projectairsim_log().info(f"Connecting to simulation server at {self.address}") - # Socket for comm via ProjectAirSim topics - self.socket_topics = pynng.Pair0( - send_timeout=1000, # Timeout after 1 second - ) # Socket for comm via ProjectAirSim services # TODO: Revisit timeout for service methods # TODO: Enable resend_time & handle on the server-side to prevent cancellation @@ -62,28 +59,61 @@ def connect(self): ) self.request_id = self.request_id_generator() if "win" in platform: - # todo check linux API for socket.set_int_option() - self.socket_topics.dial( - f"tcp://{self.address}:{self.port_topics}".encode(), block=True - ) self.socket_services.dial( f"tcp://{self.address}:{self.port_services}".encode(), block=True ) if "linux" in platform: - self.socket_topics.dial( - address=f"tcp://{self.address}:{self.port_topics}", block=True - ) self.socket_services.dial( address=f"tcp://{self.address}:{self.port_services}", block=True ) + + topic_client_already_connected = False + try: + topic_client_already_connected = self.has_topic_client() + except Exception as e: + projectairsim_log().warning( + f"Unable to query topic client state; enabling topics anyway: {e}" + ) + + if topic_client_already_connected: + projectairsim_log().warning( + "A pub-sub topic client is already connected. " + "Starting this client in service-only mode." + ) + else: + # Socket for comm via ProjectAirSim topics + self.socket_topics = pynng.Pair0( + send_timeout=1000, # Timeout after 1 second + ) + if "win" in platform: + # todo check linux API for socket.set_int_option() + self.socket_topics.dial( + f"tcp://{self.address}:{self.port_topics}".encode(), block=True + ) + if "linux" in platform: + self.socket_topics.dial( + address=f"tcp://{self.address}:{self.port_topics}", block=True + ) + self.topics_enabled = True + projectairsim_log().info("Connection opened.") self.state = True - self.recv_topic_thread = threading.Thread(target=self.__recv_topic) - self.recv_topic_thread.start() - projectairsim_log().info("Started the pub-sub topic receiving thread.") + if self.topics_enabled: + self.recv_topic_thread = threading.Thread(target=self.__recv_topic) + self.recv_topic_thread.start() + projectairsim_log().info("Started the pub-sub topic receiving thread.") def get_topic_info(self): """This is used by World to get the list of topic info on reload scene.""" + if not self.topics_enabled: + projectairsim_log().warning( + "Skipping topic info discovery because this client is running " + "in service-only mode." + ) + self.topics = {} + self.topic_info_updated = False + return + projectairsim_log().info( "Getting the list of available topic info from the sim server..." ) @@ -125,6 +155,13 @@ def subscribe(self, topic, callback, reliability=1.0): topic (str): the name of the topic callback (callable): function to call when data is received """ + if not self.topics_enabled: + projectairsim_log().warning( + f"Skipping subscription to '{topic}' because this client is " + "running in service-only mode." + ) + return + if topic in self.subs: self.subs[topic]["callbacks"].append(callback) self.subs[topic]["reliability"] = reliability @@ -144,6 +181,13 @@ def update_subscription_options(self, topic, reliability): def publish(self, topic, message): """Publishes a message to a given server topic""" + if not self.topics_enabled: + projectairsim_log().warning( + f"Skipping publish to '{topic}' because this client is running " + "in service-only mode." + ) + return + frame = self.__make_message_frame(topic, message) # self.socket_topics.send(frame) self.try_send(frame) @@ -172,6 +216,19 @@ def get_build_commit_hash(self) -> str: } return self.request(hash_req) + def has_topic_client(self) -> bool: + """Returns whether the server already has a pub-sub topic client.""" + has_topic_client_req: Dict = { + "method": "/Sim/HasTopicClient", + "params": {}, + "version": 1.0, + } + return bool(self.request(has_topic_client_req)) + + def is_service_only_mode(self) -> bool: + """Returns whether this client is connected without pub-sub topics.""" + return not self.topics_enabled + def unsubscribe(self, topics): """Unsubscribes from one or more server topics @@ -179,6 +236,10 @@ def unsubscribe(self, topics): topics (str or List): the topics. can be given as a string for one topic or a list for multiple """ + if not self.topics_enabled: + self.subs.clear() + return + if not isinstance(topics, list): topics = [topics] @@ -211,6 +272,9 @@ def unsubscribe_all(self): def try_send(self, msg, max_retries=5): """Attempts to send a message up to max_retries times""" + if not self.topics_enabled or self.socket_topics is None: + return + num_retries = 0 while num_retries < max_retries: @@ -399,8 +463,13 @@ def disconnect(self): # Unsubscribe after stopping the receive loop that processes the # incoming subscription messages. self.unsubscribe_all() - self.socket_topics.close() - self.socket_services.close() + if self.socket_topics is not None: + self.socket_topics.close() + self.socket_topics = None + if self.socket_services is not None: + self.socket_services.close() + self.socket_services = None + self.topics_enabled = False projectairsim_log().info("Disconnected.") def __get_authorization_token_public_key(self) -> str: diff --git a/client/python/projectairsim/src/projectairsim/drone.py b/client/python/projectairsim/src/projectairsim/drone.py index 37402656..c5e732aa 100644 --- a/client/python/projectairsim/src/projectairsim/drone.py +++ b/client/python/projectairsim/src/projectairsim/drone.py @@ -64,28 +64,66 @@ def set_topics(self, world: World): self.set_sensor_topics(world) self.set_robot_info_topics() - def set_sensor_topics(self, world: World): + def set_sensor_topics(self, world: World) -> None: """Sets up sensor topics for the drone. Called automatically. - - Args: - world (World): the associated ProjectAirSim World object + If config is missing, uses passive + discovery from already-cached topic info only (no new subscriptions). + Always logs and prints known topics for this drone. """ self.sensors = {} - scene_config_data = world.get_configuration() - data = None - for actor in scene_config_data["actors"]: - if actor["name"] == self.name: - data = actor["robot-config"] + # 1) Try from Scene configuration + scene_config_data = world.get_configuration() + actor_cfg = None + if scene_config_data and isinstance(scene_config_data, dict): + for actor in scene_config_data.get("actors", []): + if actor.get("name") == self.name: + actor_cfg = actor.get("robot-config") + break + + source = "config" + if ( + actor_cfg + and isinstance(actor_cfg, dict) + and isinstance(actor_cfg.get("sensors"), list) + and actor_cfg["sensors"] + ): + self._set_sensor_topics_from_config(actor_cfg) + else: + # 2) Fallback: passive discovery from preloaded topic cache only. + topics_cache = getattr(self.client, "topics", {}) + if isinstance(topics_cache, dict) and topics_cache: + projectairsim_log().warning( + f"[Drone.set_sensor_topics] No config sensors for '{self.name}'. " + "Using passive discovery from cached topics." + ) + self._set_sensor_topics_from_discovery() + source = "discovery-cached" + else: + projectairsim_log().warning( + f"[Drone.set_sensor_topics] No config sensors for '{self.name}' and no cached " + "topics available. Running in service-only mode (no topic discovery)." + ) + source = "service-only" - if data is None: - raise Exception("Actor " + self.name + " not found in the config") + # 3) Optional diagnostics for topics that belong to this drone + if getattr(self.client, "verbose_topic_diagnostics", False): + self._print_topics_for_this_drone(with_meta=True) - if "sensors" not in data: - return + total_endpoints = sum(len(v) for v in self.sensors.values() if isinstance(v, dict)) + if total_endpoints == 0: + projectairsim_log().warning( + f"[Drone.set_sensor_topics] No sensor endpoints mapped (source={source})." + ) + projectairsim_log().info( + f"[Drone.set_sensor_topics] Topic setup finished " + f"(source={source}, sensors={len(self.sensors)}, endpoints={total_endpoints})." + ) + def _set_sensor_topics_from_config(self, actor_cfg: Dict) -> None: + """Build self.sensors from the scene configuration.""" capture_setting_dict = { - 0: "scene_camera", # TODO rename scene_camera topic to rgb_camera + 0: "scene_camera", # TODO: rename to 'rgb_camera' in future 1: "depth_planar_camera", 2: "depth_camera", 3: "segmentation_camera", @@ -94,51 +132,155 @@ def set_sensor_topics(self, world: World): 6: "surface_normals_camera", } - for sensor in data["sensors"]: - name = sensor["id"] - sensor_type = sensor["type"] + sensors_list = actor_cfg.get("sensors", []) + if not isinstance(sensors_list, list): + projectairsim_log().warning("[Config] 'sensors' is not a list; skipping sensor mapping.") + return + + for sensor in sensors_list: + if not isinstance(sensor, dict): + projectairsim_log().warning("[Config] Ignoring non-dict sensor entry.") + continue + + name = sensor.get("id") + sensor_type = sensor.get("type") + if not name or not sensor_type: + projectairsim_log().warning("[Config] Sensor missing 'id' or 'type'; skipping.") + continue + sensor_root_topic = f"{self.sensors_topic}/{name}" self.sensors[name] = {} + if sensor_type == "camera": - sub_cameras = sensor["capture-settings"] - # Based on 'image-type' within the camera, set up the topic paths + sub_cameras = sensor.get("capture-settings", []) + if not isinstance(sub_cameras, list): + projectairsim_log().warning( + f"[Config] 'capture-settings' for camera '{name}' is not a list; skipping." + ) + sub_cameras = [] + for sub_camera in sub_cameras: - if sub_camera["capture-enabled"]: - image_type = capture_setting_dict[sub_camera["image-type"]] + if not isinstance(sub_camera, dict): + projectairsim_log().warning(f"[Config] Ignoring non-dict sub_camera in '{name}'.") + continue + if sub_camera.get("capture-enabled"): + image_type_id = sub_camera.get("image-type") + image_type = capture_setting_dict.get(image_type_id) + if image_type is None: + projectairsim_log().warning( + f"[Config] Unsupported image-type '{image_type_id}' in camera '{name}'; skipping." + ) + continue self.sensors[name][image_type] = f"{sensor_root_topic}/{image_type}" - self.sensors[name][ - f"{image_type}_info" - ] = f"{sensor_root_topic}/{image_type}_info" + self.sensors[name][f"{image_type}_info"] = f"{sensor_root_topic}/{image_type}_info" + elif sensor_type == "radar": - self.sensors[name][ - "radar_detections" - ] = f"{sensor_root_topic}/radar_detections" + self.sensors[name]["radar_detections"] = f"{sensor_root_topic}/radar_detections" self.sensors[name]["radar_tracks"] = f"{sensor_root_topic}/radar_tracks" + elif sensor_type == "imu": - self.sensors[name][ - "imu_kinematics" - ] = f"{sensor_root_topic}/imu_kinematics" + self.sensors[name]["imu_kinematics"] = f"{sensor_root_topic}/imu_kinematics" + elif sensor_type == "gps": self.sensors[name]["gps"] = f"{sensor_root_topic}/gps" + elif sensor_type == "airspeed": self.sensors[name]["airspeed"] = f"{sensor_root_topic}/airspeed" + elif sensor_type == "barometer": self.sensors[name]["barometer"] = f"{sensor_root_topic}/barometer" + elif sensor_type == "magnetometer": self.sensors[name]["magnetometer"] = f"{sensor_root_topic}/magnetometer" + elif sensor_type == "lidar": self.sensors[name]["lidar"] = f"{sensor_root_topic}/lidar" + elif sensor_type == "distance-sensor": - self.sensors[name][ - "distance_sensor" - ] = f"{sensor_root_topic}/distance_sensor" + self.sensors[name]["distance_sensor"] = f"{sensor_root_topic}/distance_sensor" + elif sensor_type == "battery": self.sensors[name]["battery"] = f"{sensor_root_topic}/battery" + else: - raise Exception( - f"Unknown sensor type '{sensor_type}' found in config " - f"for sensor '{name}'" - ) + raise Exception(f"Unknown sensor type '{sensor_type}' in config for sensor '{name}'") + + + def _set_sensor_topics_from_discovery(self) -> None: + """Populate self.sensors from currently cached topic metadata. + + This method is intentionally passive: it never asks the server to + refresh `r"/\$topics"` and therefore does not trigger new topic + subscriptions. + """ + topics: Dict[str, object] = getattr(self.client, "topics", {}) + if not isinstance(topics, dict) or not topics: + projectairsim_log().warning("[Discovery] No cached topics available.") + return + + self.sensors = {} + prefix = f"{self.sensors_topic}/" # .../robots//sensors/ + + for path, topic_meta in topics.items(): + if not isinstance(path, str) or not path.startswith(prefix): + continue + + topic_type = str(getattr(topic_meta, "topic_type", "")).lower() + if topic_type and topic_type != "published": + continue + + tail = path[len(prefix):] # "/.../leaf" + parts = [p for p in tail.split("/") if p] + if len(parts) < 2: + continue + sensor_id, leaf = parts[0], parts[-1] + + # Keep real data leaves dynamically and ignore RPC methods + # such as GetImages/SetPose (typically contain uppercase chars). + if any(ch.isupper() for ch in leaf): + continue + + self.sensors.setdefault(sensor_id, {})[leaf] = path + + total = sum(len(v) for v in self.sensors.values()) + projectairsim_log().info( + f"[Discovery] sensors={len(self.sensors)}, endpoints={total} under '{self.sensors_topic}'." + ) + + + def _print_topics_for_this_drone(self, with_meta: bool = True) -> None: + """Log all topics that belong to this drone (diagnostics). + Uses cached self.client.topics (path -> ProjectAirSimTopic). + """ + topics_dict = getattr(self.client, "topics", None) + if not isinstance(topics_dict, dict) or not topics_dict: + projectairsim_log().debug("[print_topics] No cached topics available.") + return + + prefix = f"{self.world_parent_topic}/robots/{self.name}" + paths = sorted( + p for p in topics_dict.keys() + if isinstance(p, str) and p.startswith(prefix) + ) + + projectairsim_log().debug(f"=== Topics for drone: {self.name} ===") + if not paths: + projectairsim_log().debug("(none)") + return + + if with_meta: + for p in paths: + t = topics_dict[p] # ProjectAirSimTopic + # Be tolerant if attributes are missing + t_type = getattr(t, "topic_type", "?") + msg = getattr(t, "message_type", "?") + hz = getattr(t, "frequency", "?") + projectairsim_log().debug(f"{p} [type={t_type}, msg={msg}, Hz={hz}]") + else: + for p in paths: + projectairsim_log().debug(p) + + projectairsim_log().debug(f"--- {len(paths)} topic(s) ---") def set_robot_info_topics(self): """Sets up robot info topics for the Drone. Called automatically""" @@ -150,6 +292,13 @@ def set_robot_info_topics(self): def log_topics(self): """Logs a human-readable list of all topics associated with the drone""" projectairsim_log().info("-------------------------------------------------") + if self.client.is_service_only_mode(): + projectairsim_log().info( + "Client is service-only; topic subscriptions are disabled." + ) + projectairsim_log().info("-------------------------------------------------") + return + projectairsim_log().info( f"The following topics can be subscribed to for robot '{self.name}':", ) diff --git a/client/python/projectairsim/src/projectairsim/world.py b/client/python/projectairsim/src/projectairsim/world.py index affe11cb..f811605c 100644 --- a/client/python/projectairsim/src/projectairsim/world.py +++ b/client/python/projectairsim/src/projectairsim/world.py @@ -5,7 +5,7 @@ Python API class for ProjectAirSim World. """ import commentjson -from typing import Dict, List +from typing import Dict, List, Optional import time from datetime import datetime import numpy as np @@ -41,7 +41,7 @@ def __init__( scene_config_name: str = "", delay_after_load_sec: int = 0, sim_config_path: str = "sim_config/", - sim_instance_idx: int = -1, + sim_instance_idx: int = -1 ): """ProjectAirSim World Interface. @@ -55,7 +55,7 @@ def __init__( self.client = client self.sim_config_path = sim_config_path self.sim_instance_idx = sim_instance_idx - self.parent_topic = "/Sim/SceneBasicDrone" # default-scene's ID + self.parent_topic = "/Sim/Scene" self.sim_config = None self.home_geo_point = None @@ -66,21 +66,33 @@ def __init__( sim_instance_idx, ) config_dict = config_loaded + config_dict["id"] = "Scene" self.scene_config_path = config_paths[0] self.robot_config_paths = config_paths[1] self.envactor_config_paths = config_paths[2] self.load_scene(config_dict, delay_after_load_sec=delay_after_load_sec) + else: + self.client.get_topic_info() + self.home_geo_point = self.get_home_geo_point() + projectairsim_log().info("Attaching to existing simulation session") + random.seed() self.import_ned_trajectory( "null_trajectory", [0, 1], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0] ) - def get_configuration(self) -> Dict: + def get_configuration(self) -> Optional[Dict]: """Get the current configuration that has been loaded to the sim server Returns: - Dict: the configuration + Optional[Dict]: the loaded configuration, or None when attached to an existing session """ + # self.sim_config is not set when World is created without parameters + if self.sim_config is None: + projectairsim_log().info( + f"Connected to existing simulation session" + ) + return None return self.sim_config def get_sim_clock_type(self) -> str: @@ -221,6 +233,21 @@ def get_wind_velocity(self) -> tuple: assert len(wind_vel) == 3 return (wind_vel[0], wind_vel[1], wind_vel[2]) + def get_home_geo_point(self) -> Dict: + """Get the home geo point for the loaded scene. + + Returns: + Dict: the home geo point with latitude, longitude, and altitude keys + """ + get_home_geo_point_req: Dict = { + "method": f"{self.parent_topic}/GetHomeGeoPoint", + "params": {}, + "version": 1.0, + } + home_geo_point = self.client.request(get_home_geo_point_req) + self.home_geo_point = home_geo_point + return home_geo_point + def resume(self) -> str: """Resume simulation @@ -1019,9 +1046,7 @@ def load_scene( # Force sim to pause on start if there are objects to spawn clock_is_steppable = scene_config_dict["clock"]["type"] == "steppable" - pause_on_start_by_user = False - if scene_config_dict["clock"].get("pause-on-start") is not None: - pause_on_start_by_user = scene_config_dict["clock"]["pause-on-start"] + pause_on_start_by_user = scene_config_dict["clock"].get("pause-on-start", False) if scene_config_dict.get("spawn-objects") is not None: if clock_is_steppable: @@ -1046,7 +1071,7 @@ def load_scene( self.client.get_topic_info() # get new scene's list of registered topic info time.sleep(delay_after_load_sec) self.parent_topic = f"/Sim/{scene_id}" - self.home_geo_point: Dict = scene_config_dict.get("home-geo-point", {}) + self.home_geo_point = self.get_home_geo_point() if scene_config_dict.get("spawn-objects") is not None: objects_loaded = self.load_scene_objects_from_config( diff --git a/client/python/projectairsim/tests/test_api_services.py b/client/python/projectairsim/tests/test_api_services.py index f583c921..97b0b679 100644 --- a/client/python/projectairsim/tests/test_api_services.py +++ b/client/python/projectairsim/tests/test_api_services.py @@ -1105,6 +1105,21 @@ def test_wind_velocity(client): raise Exception(str(err)) +def test_get_home_geo_point(client): + try: + world = World(client, "scene_test_drone.jsonc", 0) + + home_geo_point = world.get_home_geo_point() + + assert home_geo_point["latitude"] == pytest.approx(47.641468) + assert home_geo_point["longitude"] == pytest.approx(-122.140165) + assert home_geo_point["altitude"] == pytest.approx(122.0) + assert world.home_geo_point == home_geo_point + + except NNGException as err: + raise Exception(str(err)) + + def test_set_object_material(client): try: world = World(client, "scene_test_drone.jsonc", 0) diff --git a/core_sim/src/actor/robot.cpp b/core_sim/src/actor/robot.cpp index 33eeb12a..c1f424d4 100644 --- a/core_sim/src/actor/robot.cpp +++ b/core_sim/src/actor/robot.cpp @@ -677,49 +677,45 @@ void Robot::Impl::RegisterServiceMethod(const ServiceMethod& method, void Robot::Impl::RegisterServiceMethods() { // Register internal Service Methods offered by the Robot Class - auto get_gt_kinematics = - ServiceMethod(topic_path_ + "/GetGroundTruthKinematics", {""}); + auto get_gt_kinematics = ServiceMethod("GetGroundTruthKinematics", {""}); auto get_gt_kinematics_handler = get_gt_kinematics.CreateMethodHandler( &Robot::Impl::GetGroundTruthKinematics, *this); - service_manager_.RegisterMethod(get_gt_kinematics, get_gt_kinematics_handler); + RegisterServiceMethod(get_gt_kinematics, get_gt_kinematics_handler); auto set_gt_kinematics = - ServiceMethod(topic_path_ + "/SetGroundTruthKinematics", {"kinematics"}); + ServiceMethod("SetGroundTruthKinematics", {"kinematics"}); auto set_gt_kinematics_handler = set_gt_kinematics.CreateMethodHandler( &Robot::Impl::SetGroundTruthKinematics, *this); - service_manager_.RegisterMethod(set_gt_kinematics, set_gt_kinematics_handler); + RegisterServiceMethod(set_gt_kinematics, set_gt_kinematics_handler); - auto get_pose = ServiceMethod(topic_path_ + "/GetGroundTruthPose", {""}); + auto get_pose = ServiceMethod("GetGroundTruthPose", {""}); auto get_pose_handler = get_pose.CreateMethodHandler(&Robot::Impl::GetGroundTruthPose, *this); - service_manager_.RegisterMethod(get_pose, get_pose_handler); + RegisterServiceMethod(get_pose, get_pose_handler); - auto get_geo_location = - ServiceMethod(topic_path_ + "/GetGroundTruthGeoLocation", {""}); + auto get_geo_location = ServiceMethod("GetGroundTruthGeoLocation", {""}); auto get_geo_location_handler = get_geo_location.CreateMethodHandler( &Robot::Impl::GetGroundTruthGeoLocation, *this); - service_manager_.RegisterMethod(get_geo_location, get_geo_location_handler); + RegisterServiceMethod(get_geo_location, get_geo_location_handler); - auto set_pose = - ServiceMethod(topic_path_ + "/SetPose", {"pose", "reset_kinematics"}); + auto set_pose = ServiceMethod("SetPose", {"pose", "reset_kinematics"}); auto set_pose_handler = set_pose.CreateMethodHandler(&Robot::Impl::SetPose, *this); - service_manager_.RegisterMethod(set_pose, set_pose_handler); + RegisterServiceMethod(set_pose, set_pose_handler); - auto set_ext_force = - ServiceMethod(topic_path_ + "/SetExternalForce", {"ext_force"}); + auto set_ext_force = ServiceMethod("SetExternalForce", {"ext_force"}); auto set_ext_force_handler = set_ext_force.CreateMethodHandler(&Robot::Impl::SetExternalForce, *this); - service_manager_.RegisterMethod(set_ext_force, set_ext_force_handler); + RegisterServiceMethod(set_ext_force, set_ext_force_handler); auto get_camera_ray = - // ServiceMethod(topic_path_ + "/GetCameraRay", {"camera_id", + // ServiceMethod("GetCameraRay", {"camera_id", // "image_type", "position"}); - ServiceMethod(topic_path_ + "/GetCameraRay", + ServiceMethod("GetCameraRay", {"camera_id", "image_type", "x", "y"}); auto get_camera_ray_handler = get_camera_ray.CreateMethodHandler(&Robot::Impl::GetCameraRay, *this); - service_manager_.RegisterMethod(get_camera_ray, get_camera_ray_handler); + RegisterServiceMethod(get_camera_ray, get_camera_ray_handler); } bool Robot::Impl::SetExternalForce(const std::vector& ext_force) { diff --git a/core_sim/src/scene.cpp b/core_sim/src/scene.cpp index 11d5ba1d..f3077fdf 100644 --- a/core_sim/src/scene.cpp +++ b/core_sim/src/scene.cpp @@ -1,4 +1,4 @@ -// Copyright (C) Microsoft Corporation. +// Copyright (C) Microsoft Corporation. // Copyright (C) 2025 IAMAI CONSULTING CORP // MIT License. All rights reserved. @@ -169,6 +169,8 @@ class Scene::Impl : public ComponentWithTopicsAndServiceMethods { TimeNano ContinueForSingleStep(bool wait_until_complete = false); std::vector SimGetActors(); + GeoPoint GetHomeGeoPointForService(); + bool SetWindVelocity(float v_x, float v_y, float v_z); Vector3 GetWindVelocity(); void UpdateWindVelocity(); @@ -801,6 +803,10 @@ bool Scene::Impl::SetWindVelocity(float v_x, float v_y, float v_z) { return true; } +GeoPoint Scene::Impl::GetHomeGeoPointForService() { + return home_geo_point_.geo_point; +} + Vector3 Scene::Impl::GetWindVelocity() { return Environment::wind_velocity; } std::string Scene::Impl::CallBackAfter(int t_secs) { @@ -875,6 +881,11 @@ void Scene::Impl::RegisterServiceMethods() { sim_get_actors.CreateMethodHandler(&Scene::Impl::SimGetActors, *this); RegisterServiceMethod(sim_get_actors, sim_get_actors_handler); + auto get_home_geo_point = ServiceMethod("GetHomeGeoPoint", {""}); + auto get_home_geo_point_handler = get_home_geo_point.CreateMethodHandler( + &Scene::Impl::GetHomeGeoPointForService, *this); + RegisterServiceMethod(get_home_geo_point, get_home_geo_point_handler); + auto set_wind_vel = ServiceMethod("SetWindVelocity", {"v_x", "v_y", "v_z"}); auto set_wind_vel_handler = set_wind_vel.CreateMethodHandler(&Scene::Impl::SetWindVelocity, *this); diff --git a/core_sim/src/simulator.cpp b/core_sim/src/simulator.cpp index 2d47ab4e..1a1b3fb0 100644 --- a/core_sim/src/simulator.cpp +++ b/core_sim/src/simulator.cpp @@ -243,6 +243,11 @@ void Simulator::Impl::RegisterServiceMethods() { &TopicManager::Unsubscribe, topic_manager_); RegisterServiceMethod(unsubscribe_topic, unsubscribe_topic_handler); + auto has_topic_client = ServiceMethod("HasTopicClient", {}); + auto has_topic_client_handler = has_topic_client.CreateMethodHandler( + &TopicManager::HasConnectedClient, topic_manager_); + RegisterServiceMethod(has_topic_client, has_topic_client_handler); + client_authorization_.RegisterServiceMethods( topic_path_, [this](const ServiceMethod& method, MethodHandler method_handler) { diff --git a/core_sim/src/topic_manager.cpp b/core_sim/src/topic_manager.cpp index 0e868237..e4e89a18 100644 --- a/core_sim/src/topic_manager.cpp +++ b/core_sim/src/topic_manager.cpp @@ -52,6 +52,8 @@ class TopicManager::Impl { bool Unsubscribe(const std::vector& topic_paths); + bool HasConnectedClient() const; + void PublishTopic(const Topic& topic, const Message& message); void UnregisterTopic(const Topic& topic); @@ -133,6 +135,7 @@ class TopicManager::Impl { std::thread recv_thread_; Dispatcher send_dispatcher_; nng_socket topic_socket_; + std::atomic connected_client_count_; std::atomic state_; std::map> topic_table_; std::function& topic_paths) { return pimpl_->Unsubscribe(topic_paths); } +bool TopicManager::HasConnectedClient() { + return pimpl_->HasConnectedClient(); +} + void TopicManager::PublishTopic(const Topic& topic, const Message& message) { pimpl_->PublishTopic(topic, message); } @@ -217,6 +224,7 @@ TopicManager::Impl::Impl(const Logger& logger, recv_thread_(), send_dispatcher_("send_dispatcher", logger), topic_socket_(NNG_SOCKET_INITIALIZER), + connected_client_count_(0), state_(false), topic_table_(), topic_published_callback_(nullptr), @@ -230,11 +238,16 @@ void TopicManager::Impl::Load(const json& config_json) { } void TopicManager::Impl::HandleNNGPipeEvent(nng_pipe pipe, nng_pipe_ev ev) { - if (ev == NNG_PIPE_EV_REM_POST) { + if (ev == NNG_PIPE_EV_ADD_POST) { + connected_client_count_.fetch_add(1); + log_.LogVerbose(name_, "Pub-sub client connected."); + } else if (ev == NNG_PIPE_EV_REM_POST) { + auto previous_count = connected_client_count_.fetch_sub(1); + if (previous_count <= 0) connected_client_count_ = 0; + // Client disconnected--reset client authorization log_.LogVerbose( - name_, "Pub-sub client disconnected--resetting client authorization", - ev); + name_, "Pub-sub client disconnected--resetting client authorization"); client_authorization_.SetToken(nullptr, 0); // Clear topics queues and unsubscribe all topics @@ -274,6 +287,16 @@ void TopicManager::Impl::Start() { // of messages, just leave it unset to use the default value. // Register callback for client disconnects + rv = nng_pipe_notify(topic_socket_, NNG_PIPE_EV_ADD_POST, + &TopicManager::Impl::HandleNNGPipeEventProxy, this); + if (rv != 0) { + auto errno_str = nng_strerror(rv); + log_.LogError(name_, "nng_pipe_notify failed with '%s'.", errno_str); + throw Error( + "Error installing event listener on topic socket for serving " + "requests."); + } + rv = nng_pipe_notify(topic_socket_, NNG_PIPE_EV_REM_POST, &TopicManager::Impl::HandleNNGPipeEventProxy, this); if (rv != 0) { @@ -558,6 +581,10 @@ bool TopicManager::Impl::Unsubscribe( return success; } +bool TopicManager::Impl::HasConnectedClient() const { + return connected_client_count_.load() > 0; +} + void TopicManager::Impl::Unsubscribe(const Topic& topic) { auto topic_path = topic.GetPath(); Unsubscribe(topic_path); diff --git a/core_sim/src/topic_manager.hpp b/core_sim/src/topic_manager.hpp index 21f2e885..d8bd54f6 100644 --- a/core_sim/src/topic_manager.hpp +++ b/core_sim/src/topic_manager.hpp @@ -39,6 +39,8 @@ class TopicManager { bool Unsubscribe(const std::vector& topic_paths); + bool HasConnectedClient(); + void PublishTopic(const Topic& topic, const Message& message); void UnregisterTopic(const Topic& topic);