diff --git a/backend/PythonClient/airsim/__init__.py b/backend/PythonClient/airsim/__init__.py index 17f3acac1..5f6f05464 100644 --- a/backend/PythonClient/airsim/__init__.py +++ b/backend/PythonClient/airsim/__init__.py @@ -2,4 +2,4 @@ from .utils import * from .types import * -__version__ = "1.8.1" +__version__ = "3.3.0" diff --git a/backend/PythonClient/airsim/client.py b/backend/PythonClient/airsim/client.py index 7127e1e5c..c67ebb46a 100644 --- a/backend/PythonClient/airsim/client.py +++ b/backend/PythonClient/airsim/client.py @@ -1,22 +1,27 @@ -from __future__ import print_function - from .utils import * from .types import * - -import msgpackrpc #install as admin: pip install msgpack-rpc-python -import numpy as np #pip install numpy -import msgpack -import time -import math +import msgpackrpc # install as admin: pip install rpc-msgpack import logging +import sys -class VehicleClient: - def __init__(self, ip = "", port = 41451, timeout_value = 3600): - if (ip == ""): - ip = "host.docker.internal" - self.client = msgpackrpc.Client(msgpackrpc.Address(ip, port), timeout = timeout_value, pack_encoding = 'utf-8', unpack_encoding = 'utf-8') -#----------------------------------- Common vehicle APIs --------------------------------------------- +class VehicleClient: + _trip_stats_warned = False + + def __init__(self, ip="", port=41451, timeout_value=3600): + if ip == "": + ip = "127.0.0.1" + # Tracks which getTripStats RPC signature this server accepts. + # Values: unknown, with_vehicle, no_vehicle, unavailable. + self._trip_stats_rpc_mode = "unknown" + self.client = msgpackrpc.Client( + msgpackrpc.Address(ip, port), + timeout=timeout_value, + pack_encoding='utf-8', + unpack_encoding='utf-8', + ) + + #----------------------------------- Common vehicle APIs --------------------------------------------- def reset(self): """ Reset the vehicle to its original starting state @@ -30,24 +35,49 @@ def ping(self): If connection is established then this call will return true otherwise it will be blocked until timeout Returns: - bool: + bool: True if connection is established, otherwise False """ return self.client.call('ping') - def getClientVersion(self): - return 1 # sync with C++ client + @staticmethod + def getClientVersion(): + """ + Get the version of the client + + Returns: + int: Client version number + """ + return 4 # sync with C++ client def getServerVersion(self): + """ + Get the version of the server + + Returns: + int: Server version number + """ return self.client.call('getServerVersion') - def getMinRequiredServerVersion(self): - return 1 # sync with C++ client + @staticmethod + def getMinRequiredServerVersion(): + """ + Get the minimum required server version for compatibility + + Returns: + int: Minimum required server version number + """ + return 4 # sync with C++ client def getMinRequiredClientVersion(self): + """ + Get the minimum required client version for compatibility + + Returns: + int: Minimum required client version number + """ return self.client.call('getMinRequiredClientVersion') -#basic flight control - def enableApiControl(self, is_enabled, vehicle_name = ''): + def enableApiControl(self, is_enabled, vehicle_name=''): """ Enables or disables API control for vehicle corresponding to vehicle_name @@ -57,11 +87,12 @@ def enableApiControl(self, is_enabled, vehicle_name = ''): """ self.client.call('enableApiControl', is_enabled, vehicle_name) - def isApiControlEnabled(self, vehicle_name = ''): + def isApiControlEnabled(self, vehicle_name=''): """ Returns true if API control is established. - If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, `isApiControlEnabled` should return true. + If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, + `isApiControlEnabled` should return true. Args: vehicle_name (str, optional): Name of the vehicle @@ -71,7 +102,7 @@ def isApiControlEnabled(self, vehicle_name = ''): """ return self.client.call('isApiControlEnabled', vehicle_name) - def armDisarm(self, arm, vehicle_name = ''): + def armDisarm(self, arm, vehicle_name=''): """ Arms or disarms vehicle @@ -102,6 +133,10 @@ def simIsPause(self): """ return self.client.call("simIsPaused") + # Compatibility alias for callers that use the natural past participle form. + def simIsPaused(self): + return self.simIsPause() + def simContinueForTime(self, seconds): """ Continue the simulation for the specified number of seconds @@ -113,14 +148,15 @@ def simContinueForTime(self, seconds): def simContinueForFrames(self, frames): """ - Continue (or resume if paused) the simulation for the specified number of frames, after which the simulation will be paused. + Continue (or resume if paused) the simulation for the specified number of frames, after which the simulation + will be paused. Args: frames (int): Frames to run the simulation for """ self.client.call('simContinueForFrames', frames) - def getHomeGeoPoint(self, vehicle_name = ''): + def getHomeGeoPoint(self, vehicle_name=''): """ Get the Home location of the vehicle @@ -139,21 +175,21 @@ def confirmConnection(self): if self.ping(): print("Connected!") else: - print("Ping returned false!") + print("Ping returned false!") server_ver = self.getServerVersion() client_ver = self.getClientVersion() server_min_ver = self.getMinRequiredServerVersion() client_min_ver = self.getMinRequiredClientVersion() ver_info = "Client Ver:" + str(client_ver) + " (Min Req: " + str(client_min_ver) + \ - "), Server Ver:" + str(server_ver) + " (Min Req: " + str(server_min_ver) + ")" + "), Server Ver:" + str(server_ver) + " (Min Req: " + str(server_min_ver) + ")" if server_ver < server_min_ver: print(ver_info, file=sys.stderr) - print("AirSim server is of older version and not supported by this client. Please upgrade!") + print("Cosys-AirSim server is of older version and not supported by this client. Please upgrade!") elif client_ver < client_min_ver: print(ver_info, file=sys.stderr) - print("AirSim client is of older version and not supported by this server. Please upgrade!") + print("Cosys-AirSim client is of older version and not supported by this server. Please upgrade!") else: print(ver_info) print('') @@ -169,63 +205,64 @@ def simSetLightIntensity(self, light_name, intensity): Returns: bool: True if successful, otherwise False """ - return self.client.call("simSetLightIntensity", light_name, intensity) - def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0): + logging.warning("simSetLightIntensity is deprecated, use the new Artificial Light API instead") + return self.simSetWorldLightIntensity(light_name, intensity) + + def simSwapTextures(self, tags, tex_id=0, component_id=0, material_id=0): """ Runtime Swap Texture API - See https://microsoft.github.io/AirSim/retexturing/ for details - Args: tags (str): string of "," or ", " delimited tags to identify on which actors to perform the swap tex_id (int, optional): indexes the array of textures assigned to each actor undergoing a swap - If out-of-bounds for some object's texture set, it will be taken modulo the number of textures that were available - component_id (int, optional): - material_id (int, optional): + If out-of-bounds for some object's texture set, it will be taken modulo the number + of textures that were available + component_id (int, optional): Index of the component to apply the texture to + material_id (int, optional): Index of the material to apply the texture to Returns: - list[str]: List of objects which matched the provided tags and had the texture swap perfomed + list[str]: List of objects which matched the provided tags and had the texture swap performed """ return self.client.call("simSwapTextures", tags, tex_id, component_id, material_id) - def simSetObjectMaterial(self, object_name, material_name, component_id = 0): + def simSetObjectMaterial(self, object_name, material_name, component_id=0): """ Runtime Swap Texture API - See https://microsoft.github.io/AirSim/retexturing/ for details + Args: - object_name (str): name of object to set material for - material_name (str): name of material to set for object - component_id (int, optional) : index of material elements + object_name (str): Name of object to set material for + material_name (str): Name of material to set for object + component_id (int, optional): Index of material elements Returns: bool: True if material was set """ return self.client.call("simSetObjectMaterial", object_name, material_name, component_id) - def simSetObjectMaterialFromTexture(self, object_name, texture_path, component_id = 0): + def simSetObjectMaterialFromTexture(self, object_name, texture_path, component_id=0): """ Runtime Swap Texture API - See https://microsoft.github.io/AirSim/retexturing/ for details + Args: - object_name (str): name of object to set material for - texture_path (str): path to texture to set for object - component_id (int, optional) : index of material elements + object_name (str): Name of object to set material for + texture_path (str): Path to texture to set for object + component_id (int, optional): Index of material elements Returns: bool: True if material was set """ - return self.client.call("simSetObjectMaterialFromTexture", object_name, texture_path, component_id) + return self.client.call("simSetObjectMaterialFromTexture", object_name, texture_path, + component_id) - - # time-of-day control -#time - of - day control - def simSetTimeOfDay(self, is_enabled, start_datetime = "", is_start_datetime_dst = False, celestial_clock_speed = 1, update_interval_secs = 60, move_sun = True): + def simSetTimeOfDay(self, is_enabled, start_datetime="", is_start_datetime_dst=False, celestial_clock_speed=1, + update_interval_secs=60, move_sun=True): """ Control the position of Sun in the environment - Sun's position is computed using the coordinates specified in `OriginGeopoint` in settings for the date-time specified in the argument, + Sun's position is computed using the coordinates specified in `OriginGeopoint` in settings for the date-time + specified in the argument, else if the string is empty, current date & time is used Args: @@ -233,14 +270,16 @@ def simSetTimeOfDay(self, is_enabled, start_datetime = "", is_start_datetime_dst start_datetime (str, optional): Date & Time in %Y-%m-%d %H:%M:%S format, e.g. `2018-02-12 15:20:00` is_start_datetime_dst (bool, optional): True to adjust for Daylight Savings Time celestial_clock_speed (float, optional): Run celestial clock faster or slower than simulation clock - E.g. Value 100 means for every 1 second of simulation clock, Sun's position is advanced by 100 seconds + E.g. Value 100 means for every 1 second of simulation clock, Sun's + position is advanced by 100 seconds so Sun will move in sky much faster update_interval_secs (float, optional): Interval to update the Sun's position - move_sun (bool, optional): Whether or not to move the Sun + move_sun (bool, optional): To move the Sun or not """ - self.client.call('simSetTimeOfDay', is_enabled, start_datetime, is_start_datetime_dst, celestial_clock_speed, update_interval_secs, move_sun) + self.client.call('simSetTimeOfDay', is_enabled, start_datetime, is_start_datetime_dst, + celestial_clock_speed, + update_interval_secs, move_sun) -#weather def simEnableWeather(self, enable): """ Enable Weather effects. Needs to be called before using `simSetWeatherParameter` API @@ -260,151 +299,286 @@ def simSetWeatherParameter(self, param, val): """ self.client.call('simSetWeatherParameter', param, val) -#camera control -#simGetImage returns compressed png in array of bytes -#image_type uses one of the ImageType members - def simGetImage(self, camera_name, image_type, vehicle_name = '', external = False): + def simGetImage(self, camera_name, image_type, vehicle_name='', annotation_name=""): """ Get a single image - Returns bytes of png format image which can be dumped into abinary file to create .png image - `string_to_uint8_array()` can be used to convert into Numpy unit8 array - See https://microsoft.github.io/AirSim/image_apis/ for details + Returns bytes of png format image which can be dumped into a binary file to create .png image + `string_to_uint8_array()` can be used to convert into Numpy uint8 array Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Name of the vehicle with the camera - external (bool, optional): Whether the camera is an External Camera + annotation_name (str, optional): Name of the annotation to be applied if using image type Annotation. Returns: - Binary string literal of compressed png image + bytes: Binary string literal of compressed png image """ -#todo : in future remove below, it's only for compatibility to pre v1.2 + # todo: in future remove below, it's only for compatibility to pre v1.2 camera_name = str(camera_name) -#because this method returns std::vector < uint8>, msgpack decides to encode it as a string unfortunately. - result = self.client.call('simGetImage', camera_name, image_type, vehicle_name, external) - if (result == "" or result == "\0"): + # because this method returns std::vector < uint8>, msgpack decides to encode it as a string, unfortunately. + result = self.client.call('simGetImage', camera_name, image_type, vehicle_name, annotation_name) + if result == "" or result == "\0": return None return result -#camera control -#simGetImage returns compressed png in array of bytes -#image_type uses one of the ImageType members - def simGetImages(self, requests, vehicle_name = '', external = False): + def simGetImages(self, requests, vehicle_name=''): """ Get multiple images - See https://microsoft.github.io/AirSim/image_apis/ for details and examples - Args: requests (list[ImageRequest]): Images required vehicle_name (str, optional): Name of vehicle associated with the camera - external (bool, optional): Whether the camera is an External Camera Returns: - list[ImageResponse]: + list[ImageResponse]: List of image responses """ - responses_raw = self.client.call('simGetImages', requests, vehicle_name, external) + responses_raw = self.client.call('simGetImages', requests, vehicle_name) return [ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw] + def simGetPresetLensSettings(self, camera_name, vehicle_name=''): + """ + Get the preset lens settings for a given camera + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera -#CinemAirSim - def simGetPresetLensSettings(self, camera_name, vehicle_name = '', external = False): - result = self.client.call('simGetPresetLensSettings', camera_name, vehicle_name, external) - if (result == "" or result == "\0"): + Returns: + list[str]: Preset lens settings + """ + result = self.client.call('simGetPresetLensSettings', camera_name, vehicle_name) + if result == "" or result == "\0": return None return result - def simGetLensSettings(self, camera_name, vehicle_name = '', external = False): - result = self.client.call('simGetLensSettings', camera_name, vehicle_name, external) - if (result == "" or result == "\0"): + def simGetLensSettings(self, camera_name, vehicle_name=''): + """ + Get the current lens settings for a given camera + + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + str: Current lens settings + """ + result = self.client.call('simGetLensSettings', camera_name, vehicle_name) + if result == "" or result == "\0": return None return result - def simSetPresetLensSettings(self, preset_lens_settings, camera_name, vehicle_name = '', external = False): - self.client.call("simSetPresetLensSettings", preset_lens_settings, camera_name, vehicle_name, external) + def simSetPresetLensSettings(self, preset_lens_settings, camera_name, vehicle_name=''): + """ + Set the preset lens settings for a given camera - def simGetPresetFilmbackSettings(self, camera_name, vehicle_name = '', external = False): - result = self.client.call('simGetPresetFilmbackSettings', camera_name, vehicle_name, external) - if (result == "" or result == "\0"): + Args: + preset_lens_settings (str): Preset lens settings + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simSetPresetLensSettings", preset_lens_settings, camera_name, vehicle_name) + + def simGetPresetFilmbackSettings(self, camera_name, vehicle_name=''): + """ + Get the preset filmback settings for a given camera + + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + list[str]: Preset filmback settings + """ + result = self.client.call('simGetPresetFilmbackSettings', camera_name, vehicle_name) + if result == "" or result == "\0": return None return result - def simSetPresetFilmbackSettings(self, preset_filmback_settings, camera_name, vehicle_name = '', external = False): - self.client.call("simSetPresetFilmbackSettings", preset_filmback_settings, camera_name, vehicle_name, external) + def simSetPresetFilmbackSettings(self, preset_filmback_settings, camera_name, vehicle_name=''): + """ + Set the preset filmback settings for a given camera - def simGetFilmbackSettings(self, camera_name, vehicle_name = '', external = False): - result = self.client.call('simGetFilmbackSettings', camera_name, vehicle_name, external) - if (result == "" or result == "\0"): + Args: + preset_filmback_settings (list[str]): Preset filmback settings + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simSetPresetFilmbackSettings", preset_filmback_settings, camera_name, vehicle_name) + + def simGetFilmbackSettings(self, camera_name, vehicle_name=''): + """ + Get the current filmback settings for a given camera + + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + str: Current filmback settings + """ + result = self.client.call('simGetFilmbackSettings', camera_name, vehicle_name) + if result == "" or result == "\0": return None return result - def simSetFilmbackSettings(self, sensor_width, sensor_height, camera_name, vehicle_name = '', external = False): - return self.client.call("simSetFilmbackSettings", sensor_width, sensor_height, camera_name, vehicle_name, external) + def simSetFilmbackSettings(self, sensor_width, sensor_height, camera_name, vehicle_name=''): + """ + Set the filmback settings for a given camera - def simGetFocalLength(self, camera_name, vehicle_name = '', external = False): - return self.client.call("simGetFocalLength", camera_name, vehicle_name, external) + Args: + sensor_width (float): Width of the sensor + sensor_height (float): Height of the sensor + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera - def simSetFocalLength(self, focal_length, camera_name, vehicle_name = '', external = False): - self.client.call("simSetFocalLength", focal_length, camera_name, vehicle_name, external) + Returns: + bool: True if the settings were successfully applied, otherwise False + """ + return self.client.call("simSetFilmbackSettings", sensor_width, sensor_height, camera_name, vehicle_name) - def simEnableManualFocus(self, enable, camera_name, vehicle_name = '', external = False): - self.client.call("simEnableManualFocus", enable, camera_name, vehicle_name, external) + def simGetFocalLength(self, camera_name, vehicle_name=''): + """ + Get the focal length for a given camera - def simGetFocusDistance(self, camera_name, vehicle_name = '', external = False): - return self.client.call("simGetFocusDistance", camera_name, vehicle_name, external) + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera - def simSetFocusDistance(self, focus_distance, camera_name, vehicle_name = '', external = False): - self.client.call("simSetFocusDistance", focus_distance, camera_name, vehicle_name, external) + Returns: + float: Focal length of the camera + """ + return self.client.call("simGetFocalLength", camera_name, vehicle_name) - def simGetFocusAperture(self, camera_name, vehicle_name = '', external = False): - return self.client.call("simGetFocusAperture", camera_name, vehicle_name, external) + def simSetFocalLength(self, focal_length, camera_name, vehicle_name=''): + """ + Set the focal length for a given camera - def simSetFocusAperture(self, focus_aperture, camera_name, vehicle_name = '', external = False): - self.client.call("simSetFocusAperture", focus_aperture, camera_name, vehicle_name, external) + Args: + focal_length (float): Focal length to set + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simSetFocalLength", focal_length, camera_name, vehicle_name) - def simEnableFocusPlane(self, enable, camera_name, vehicle_name = '', external = False): - self.client.call("simEnableFocusPlane", enable, camera_name, vehicle_name, external) + def simEnableManualFocus(self, enable, camera_name, vehicle_name=''): + """ + Enable or disable manual focus for a given camera - def simGetCurrentFieldOfView(self, camera_name, vehicle_name = '', external = False): - return self.client.call("simGetCurrentFieldOfView", camera_name, vehicle_name, external) + Args: + enable (bool): True to enable manual focus, False to disable + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simEnableManualFocus", enable, camera_name, vehicle_name) -#End CinemAirSim - def simTestLineOfSightToPoint(self, point, vehicle_name = ''): + def simGetFocusDistance(self, camera_name, vehicle_name=''): """ - Returns whether the target point is visible from the perspective of the inputted vehicle + Get the focus distance for a given camera Args: - point (GeoPoint): target point - vehicle_name (str, optional): Name of vehicle + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + float: Focus distance of the camera + """ + return self.client.call("simGetFocusDistance", camera_name, vehicle_name) + + def simSetFocusDistance(self, focus_distance, camera_name, vehicle_name=''): + """ + Set the focus distance for a given camera + + Args: + focus_distance (float): Focus distance to set + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simSetFocusDistance", focus_distance, camera_name, vehicle_name) + + def simGetFocusAperture(self, camera_name, vehicle_name=''): + """ + Get the focus aperture for a given camera + + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + float: Focus aperture of the camera + """ + return self.client.call("simGetFocusAperture", camera_name, vehicle_name) + + def simSetFocusAperture(self, focus_aperture, camera_name, vehicle_name=''): + """ + Set the focus aperture for a given camera + + Args: + focus_aperture (float): Focus aperture to set + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simSetFocusAperture", focus_aperture, camera_name, vehicle_name) + + def simEnableFocusPlane(self, enable, camera_name, vehicle_name=''): + """ + Enable or disable the focus plane for a given camera + + Args: + enable (bool): True to enable focus plane, False to disable + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + """ + self.client.call("simEnableFocusPlane", enable, camera_name, vehicle_name) + + def simGetCurrentFieldOfView(self, camera_name, vehicle_name=''): + """ + Get the current field of view for a given camera + + Args: + camera_name (str): Name of the camera + vehicle_name (str, optional): Name of the vehicle with the camera + + Returns: + float: Current field of view of the camera + """ + return self.client.call("simGetCurrentFieldOfView", camera_name, vehicle_name) + + def simTestLineOfSightToPoint(self, point, vehicle_name=''): + """ + Returns whether the target point is visible from the perspective of the inputted vehicle. + + Args: + point (GeoPoint): Target point. + vehicle_name (str, optional): Name of the vehicle. Returns: - [bool]: Success + bool: True if the target point is visible, otherwise False. """ return self.client.call('simTestLineOfSightToPoint', point, vehicle_name) def simTestLineOfSightBetweenPoints(self, point1, point2): """ - Returns whether the target point is visible from the perspective of the source point + Returns whether the target point is visible from the perspective of the source point. Args: - point1 (GeoPoint): source point - point2 (GeoPoint): target point + point1 (GeoPoint): Source point. + point2 (GeoPoint): Target point. Returns: - [bool]: Success + bool: True if the target point is visible from the source point, otherwise False. """ return self.client.call('simTestLineOfSightBetweenPoints', point1, point2) def simGetWorldExtents(self): """ - Returns a list of GeoPoints representing the minimum and maximum extents of the world + Returns a list of GeoPoints representing the minimum and maximum extents of the world. Returns: - list[GeoPoint] + list[GeoPoint]: List containing the minimum and maximum extents of the world. """ responses_raw = self.client.call('simGetWorldExtents') return [GeoPoint.from_msgpack(response_raw) for response_raw in responses_raw] @@ -412,292 +586,594 @@ def simGetWorldExtents(self): def simRunConsoleCommand(self, command): """ Allows the client to execute a command in Unreal's native console, via an API. - Affords access to the countless built-in commands such as "stat unit", "stat fps", "open [map]", adjust any config settings, etc. etc. - Allows the user to create bespoke APIs very easily, by adding a custom event to the level blueprint, and then calling the console command "ce MyEventName [args]". No recompilation of AirSim needed! + Affords access to the countless built-in commands such as "stat unit", "stat fps", "open [map]", adjust any + config settings, etc. etc. + Allows the user to create bespoke APIs very easily, by adding a custom event to the level blueprint, and then + calling the console command "ce MyEventName [args]". No recompilation of Cosys-AirSim needed! Args: - command ([string]): Desired Unreal Engine Console command to run + command (str): Desired Unreal Engine Console command to run. Returns: - [bool]: Success + bool: True if the command was successfully executed, otherwise False. """ return self.client.call('simRunConsoleCommand', command) -#gets the static meshes in the unreal scene def simGetMeshPositionVertexBuffers(self): """ - Returns the static meshes that make up the scene - - See https://microsoft.github.io/AirSim/meshes/ for details and how to use this + Returns the static meshes that make up the scene. Returns: - list[MeshPositionVertexBuffersResponse]: + list[MeshPositionVertexBuffersResponse]: List of responses containing mesh position vertex buffers. """ responses_raw = self.client.call('simGetMeshPositionVertexBuffers') return [MeshPositionVertexBuffersResponse.from_msgpack(response_raw) for response_raw in responses_raw] - def simGetCollisionInfo(self, vehicle_name = ''): + def simGetCollisionInfo(self, vehicle_name=''): """ + Gets the collision information for the specified vehicle. + Args: - vehicle_name (str, optional): Name of the Vehicle to get the info of + vehicle_name (str, optional): Name of the vehicle to get the collision information of. Returns: - CollisionInfo: + CollisionInfo: Collision information of the specified vehicle. """ return CollisionInfo.from_msgpack(self.client.call('simGetCollisionInfo', vehicle_name)) - def simSetVehiclePose(self, pose, ignore_collision, vehicle_name = ''): + def simSetVehiclePose(self, pose, ignore_collision, vehicle_name=''): """ - Set the pose of the vehicle + Set the pose of the vehicle. - If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values + If you don't want to change position (or orientation), then just set the components of position + (or orientation) to floating point NaN values. Args: - pose (Pose): Desired Pose pf the vehicle - ignore_collision (bool): Whether to ignore any collision or not - vehicle_name (str, optional): Name of the vehicle to move + pose (Pose): Desired pose of the vehicle. + ignore_collision (bool): Whether to ignore any collision or not. + vehicle_name (str, optional): Name of the vehicle to move. """ self.client.call('simSetVehiclePose', pose, ignore_collision, vehicle_name) - def simGetVehiclePose(self, vehicle_name = ''): + def simGetVehiclePose(self, vehicle_name=''): """ - The position inside the returned Pose is in the frame of the vehicle's starting point - + Gets the pose of the specified vehicle. The position inside the returned Pose is in the frame of + the vehicle's starting point. + Args: - vehicle_name (str, optional): Name of the vehicle to get the Pose of + vehicle_name (str, optional): Name of the vehicle to get the pose of. Returns: - Pose: + Pose: Pose of the specified vehicle. """ pose = self.client.call('simGetVehiclePose', vehicle_name) return Pose.from_msgpack(pose) - def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''): + def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name=''): """ - Modify the color and thickness of the line when Tracing is enabled + Modify the color and thickness of the line when tracing is enabled. - Tracing can be enabled by pressing T in the Editor or setting `EnableTrace` to `True` in the Vehicle Settings + Tracing can be enabled by pressing 'T' in the Editor or setting `EnableTrace` to `True` in the Vehicle Settings. Args: - color_rgba (list): desired RGBA values from 0.0 to 1.0 - thickness (float, optional): Thickness of the line - vehicle_name (string, optional): Name of the vehicle to set Trace line values for + color_rgba (list): Desired RGBA values from 0.0 to 1.0. + thickness (float, optional): Thickness of the line. + vehicle_name (str, optional): Name of the vehicle to set trace line values for. """ self.client.call('simSetTraceLine', color_rgba, thickness, vehicle_name) - def simGetObjectPose(self, object_name): + def simGetObjectPose(self, object_name, ned=True): """ - The position inside the returned Pose is in the world frame + Gets the pose of the specified object. The position inside the returned Pose is in the world frame. Args: - object_name (str): Object to get the Pose of + object_name (str): Name of the object to get the pose of. + ned (bool, optional): Whether the pose is in NED coordinates. Returns: - Pose: + Pose: Pose of the specified object. """ - pose = self.client.call('simGetObjectPose', object_name) + pose = self.client.call('simGetObjectPose', object_name, ned) return Pose.from_msgpack(pose) - def simSetObjectPose(self, object_name, pose, teleport = True): + def simSetObjectPose(self, object_name, pose, teleport=True): """ - Set the pose of the object(actor) in the environment + Set the pose of the object (actor) in the environment. - The specified actor must have Mobility set to movable, otherwise there will be undefined behaviour. - See https://www.unrealengine.com/en-US/blog/moving-physical-objects for details on how to set Mobility and the effect of Teleport parameter + The specified actor must have Mobility set to movable, otherwise there will be undefined behavior. + See https://www.unrealengine.com/en-US/blog/moving-physical-objects for details on how to set Mobility and + the effect of the Teleport parameter. Args: - object_name (str): Name of the object(actor) to move - pose (Pose): Desired Pose of the object - teleport (bool, optional): Whether to move the object immediately without affecting their velocity + object_name (str): Name of the object (actor) to move. + pose (Pose): Desired pose of the object. + teleport (bool, optional): Whether to move the object immediately without affecting its velocity. Returns: - bool: If the move was successful + bool: True if the move was successful, otherwise False. """ return self.client.call('simSetObjectPose', object_name, pose, teleport) def simGetObjectScale(self, object_name): """ - Gets scale of an object in the world + Gets the scale of the specified object in the world. Args: - object_name (str): Object to get the scale of + object_name (str): Name of the object to get the scale of. Returns: - airsim.Vector3r: Scale + Vector3r: Scale of the specified object. """ scale = self.client.call('simGetObjectScale', object_name) return Vector3r.from_msgpack(scale) def simSetObjectScale(self, object_name, scale_vector): """ - Sets scale of an object in the world + Sets the scale of the specified object in the world. Args: - object_name (str): Object to set the scale of - scale_vector (airsim.Vector3r): Desired scale of object + object_name (str): Name of the object to set the scale of. + scale_vector (Vector3r): Desired scale of the object. Returns: - bool: True if scale change was successful + bool: True if the scale change was successful, otherwise False. """ return self.client.call('simSetObjectScale', object_name, scale_vector) - def simListSceneObjects(self, name_regex = '.*'): + def simListSceneObjects(self, name_regex='.*'): """ - Lists the objects present in the environment + Lists the objects present in the environment. - Default behaviour is to list all objects, regex can be used to return smaller list of matching objects or actors + The default behavior is to list all objects. A regex can be used to return a smaller list + of matching objects or actors. Args: - name_regex (str, optional): String to match actor names against, e.g. "Cylinder.*" + name_regex (str, optional): String to match actor names against, e.g., "Cylinder.*". Returns: - list[str]: List containing all the names + list[str]: List containing the names of the objects. """ return self.client.call('simListSceneObjects', name_regex) + def simListSceneObjectsTags(self, name_regex='.*'): + """ + Lists the objects present and the given tag matching the regular expression in the environment. + + The default behavior is to list all objects with first tag. A regex can be used to return a smaller list + of matching objects or actors. + + Args: + name_regex (str, optional): String to match actor tags against, e.g., "Gate.*". + + Returns: + list[list[str, str]]: List containing the names of the objects. + """ + return self.client.call('simListSceneObjectsTags', name_regex) + def simLoadLevel(self, level_name): """ - Loads a level specified by its name + Loads a level specified by its name. Args: - level_name (str): Name of the level to load + level_name (str): Name of the level to load. Returns: - bool: True if the level was successfully loaded + bool: True if the level was successfully loaded, otherwise False. """ return self.client.call('simLoadLevel', level_name) def simListAssets(self): """ - Lists all the assets present in the Asset Registry + Lists all the assets present in the Asset Registry. Returns: - list[str]: Names of all the assets + list[str]: Names of all the assets. """ return self.client.call('simListAssets') def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=False, is_blueprint=False): - """Spawned selected object in the world + """ + Spawns the selected object in the world. Args: - object_name (str): Desired name of new object - asset_name (str): Name of asset(mesh) in the project database - pose (airsim.Pose): Desired pose of object - scale (airsim.Vector3r): Desired scale of object - physics_enabled (bool, optional): Whether to enable physics for the object - is_blueprint (bool, optional): Whether to spawn a blueprint or an actor + object_name (str): Desired name of the new object. + asset_name (str): Name of the asset (mesh) in the project database. + pose (Pose): Desired pose of the object. + scale (Vector3r): Desired scale of the object. + physics_enabled (bool, optional): Whether to enable physics for the object. + is_blueprint (bool, optional): Whether to spawn a blueprint or an actor. Returns: - str: Name of spawned object, in case it had to be modified + str: Name of the spawned object, in case it had to be modified. """ return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled, is_blueprint) def simDestroyObject(self, object_name): - """Removes selected object from the world + """ + Removes the selected object from the world. Args: - object_name (str): Name of object to be removed + object_name (str): Name of the object to be removed. Returns: - bool: True if object is queued up for removal + bool: True if the object is queued up for removal, otherwise False. """ return self.client.call('simDestroyObject', object_name) - def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): + def simListInstanceSegmentationObjects(self): """ - Set segmentation ID for specific objects + Lists all the instance segmentation objects in the environment. - See https://microsoft.github.io/AirSim/image_apis/#segmentation for details + Returns: + list[str]: Names of all the instance segmentation objects. + """ + return self.client.call('simListInstanceSegmentationObjects') + + def simListInstanceSegmentationPoses(self, ned=True, only_visible=False): + """ + Lists the poses of all instance segmentation objects in the environment. Args: - mesh_name (str): Name of the mesh to set the ID of (supports regex) - object_id (int): Object ID to be set, range 0-255 + ned (bool, optional): Whether the poses are in NED coordinates. + only_visible (bool, optional): Whether to include only visible objects. + + Returns: + list[Pose]: List of poses of instance segmentation objects. + """ + poses_raw = self.client.call('simListInstanceSegmentationPoses', ned, only_visible) + return [Pose.from_msgpack(pose_raw) for pose_raw in poses_raw] - RBG values for IDs can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt - is_name_regex (bool, optional): Whether the mesh name is a regex + def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex=False): + """ + Sets the ID for the specified segmentation object. + + Args: + mesh_name (str): Name of the mesh. + object_id (int): Desired ID for the object. + is_name_regex (bool, optional): Whether the mesh name is a regex. Returns: - bool: If the mesh was found + bool: True if the ID was successfully set, otherwise False. """ return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) def simGetSegmentationObjectID(self, mesh_name): """ - Returns Object ID for the given mesh name - - Mapping of Object IDs to RGB values can be seen at https://microsoft.github.io/AirSim/seg_rgbs.txt + Gets the ID for the specified segmentation object. Args: - mesh_name (str): Name of the mesh to get the ID of + mesh_name (str): Name of the mesh. + + Returns: + int: ID of the specified object. """ return self.client.call('simGetSegmentationObjectID', mesh_name) - def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehicle_name = '', external = False): + def simListAnnotationObjects(self, annotation_name): + """ + Lists all annotation objects with the specified name of the layer. + + Args: + annotation_name (str): Name of the annotation layer. + + Returns: + list[str]: List of annotation objects with the specified name. + """ + return self.client.call('simListAnnotationObjects', annotation_name) + + def simListAnnotationPoses(self, annotation_name, ned=True, only_visible=False): + """ + Lists the poses of all annotation objects with the specified name of the layer. + + Args: + annotation_name (str): Name of the annotation layer. + ned (bool, optional): Whether the poses are in NED coordinates. + only_visible (bool, optional): Whether to include only visible objects. + + Returns: + list[Pose]: List of poses of annotation objects with the specified name. + """ + poses_raw = self.client.call('simListAnnotationPoses', annotation_name, ned, only_visible) + return [Pose.from_msgpack(pose_raw) for pose_raw in poses_raw] + + def simSetAnnotationObjectID(self, annotation_name, mesh_name, object_id, is_name_regex=False): + """ + Sets the ID for the specified annotation object on this annotation layer. + This works only for RGB layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + object_id (int): Desired ID for the object. + is_name_regex (bool, optional): Whether the mesh name is a regex. + + Returns: + bool: True if the ID was successfully set, otherwise False. + """ + return self.client.call('simSetAnnotationObjectID', annotation_name, mesh_name, object_id, is_name_regex) + + def simGetAnnotationObjectID(self, annotation_name, mesh_name): + """ + Gets the ID for the specified annotation object on this annotation layer. + This works only for RGB layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + + Returns: + int: ID of the specified object. + """ + return self.client.call('simGetAnnotationObjectID', annotation_name, mesh_name) + + def simSetAnnotationObjectColor(self, annotation_name, mesh_name, r, g, b, is_name_regex=False): + """ + Sets the color for the specified annotation object on this annotation layer. + This works only for RGB layers. Use simIsValidColor(r, g, b) to test if your color can work! + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + r (float): Red component of the color [0 255] + g (float): Green component of the color [0 255] + b (float): Blue component of the color [0 255] + is_name_regex (bool, optional): Whether the mesh name is a regex. + + Returns: + bool: True if the color was successfully set, otherwise False. + """ + return self.client.call('simSetAnnotationObjectColor', annotation_name, mesh_name, r, g, b, is_name_regex) + + def simGetAnnotationObjectColor(self, annotation_name, mesh_name): + """ + Gets the color for the specified annotation object on this annotation layer. + This works only for RGB layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + + Returns: + tuple: (r, g, b) color values of the specified object. + """ + return self.client.call('simGetAnnotationObjectColor', annotation_name, mesh_name) + + def simSetAnnotationObjectValue(self, annotation_name, mesh_name, greyscale_value, is_name_regex=False): + """ + Sets the greyscale value for the specified annotation object on this annotation layer. + This works only for greyscale layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + greyscale_value (float): Desired greyscale value for the object. + is_name_regex (bool, optional): Whether the mesh name is a regex. + + Returns: + bool: True if the greyscale value was successfully set, otherwise False. + """ + return self.client.call('simSetAnnotationObjectValue', annotation_name, mesh_name, greyscale_value, + is_name_regex) + + def simGetAnnotationObjectValue(self, annotation_name, mesh_name): + """ + Gets the greyscale value for the specified annotation object on this annotation layer. + This works only for greyscale layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + + Returns: + float: Greyscale value of the specified object. + """ + return self.client.call('simGetAnnotationObjectValue', annotation_name, mesh_name) + + def simSetAnnotationObjectTextureByPath(self, annotation_name, mesh_name, texture_path, is_name_regex=False): + """ + Sets the texture for the specified annotation object by path on this annotation layer. + See documentation on annotation for texture path syntax. + This works only for texture layers that are set to direct mode. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + texture_path (str): Path to the desired texture. + is_name_regex (bool, optional): Whether the mesh name is a regex. + + Returns: + bool: True if the texture was successfully set, otherwise False. + """ + return self.client.call('simSetAnnotationObjectTextureByPath', annotation_name, mesh_name, texture_path, + is_name_regex) + + def simEnableAnnotationObjectTextureByPath(self, annotation_name, mesh_name, is_name_regex=False): + """ + Enables the texture for the specified annotation object by relative path on this annotation layer. + This works only for texture layers set to relative path mode. + This requires the texture to be present in the expected location. See annotation documentation for more info. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + is_name_regex (bool, optional): Whether the mesh name is a regex. + + Returns: + bool: True if the texture was successfully enabled, otherwise False. + """ + return self.client.call('simEnableAnnotationObjectTextureByPath', annotation_name, mesh_name, is_name_regex) + + def simGetAnnotationObjectTexturePath(self, annotation_name, mesh_name): + """ + Gets the texture path for the specified annotation object on this annotation layer. + This works only for texture layers. + + Args: + annotation_name (str): Name of the annotation layer. + mesh_name (str): Name of the mesh. + + Returns: + str: Path to the texture of the specified object. + """ + return self.client.call('simGetAnnotationObjectTexturePath', annotation_name, mesh_name) + + @staticmethod + def simGetSegmentationColorMap(): + """ + Gets the segmentation color map. + + Returns: + dict: Dictionary mapping object IDs to colors. + """ + return load_colormap() + + @staticmethod + def simIsValidColor(r, g, b): + """ + Checks if the specified color is valid. + + Args: + r (int): Red component of the color [0 255] + g (int): Green component of the color [0 255] + b (int): Blue component of the color [0 255] + + Returns: + bool: True if the color is valid, otherwise False. + """ + return np.array([r, g, b]) in load_colormap() + + def simSetWorldLightVisibility(self, light_name, is_visible=True): + """ + Enable or disable an artificial light set as a static world light. + + Args: + light_name (str): Name of the world light. + is_visible (bool, optional): Set to true to enable the light, false to disable it. + + Returns: + bool: True if the light was toggled. + """ + return self.client.call('simSetWorldLightVisibility', light_name, is_visible) + + def simSetWorldLightIntensity(self, light_name, intensity): + """ + Set the intensity value of an artificial light set as a static world light. + + Args: + light_name (str): Name of the world light. + intensity (float): The intensity value of the light. Depends on the Intensity unit. + + Returns: + bool: True if the light was changed. + """ + return self.client.call('simSetWorldLightIntensity', light_name, intensity) + + def simSetVehicleLightVisibility(self, vehicle_name, light_name, is_visible=True): + """ + Enable or disable an artificial light set as a vehicle light. + + Args: + vehicle_name (str): Vehicle which the light is associated with. + light_name (str): Name of the world light. + is_visible (bool, optional): Set to true to enable the light, false to disable it. + + Returns: + bool: True if the light was toggled. + """ + return self.client.call('simSetVehicleLightVisibility', vehicle_name, light_name, is_visible) + + def simSetVehicleLightIntensity(self, vehicle_name, light_name, intensity): + """ + Set the intensity value of an artificial light set as a vehicle light. + + Args: + vehicle_name (str): Vehicle which the light is associated with. + light_name (str): Name of the world light. + intensity (float): The intensity value of the light. Depends on the Intensity unit. + + Returns: + bool: True if the light was changed. + """ + return self.client.call('simSetVehicleLightIntensity', vehicle_name, light_name, intensity) + + + def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehicle_name='', annotation_name=""): """ Add mesh name to detect in wild card format For example: simAddDetectionFilterMeshName("Car_*") will detect all instance named "Car_*" Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used image_type (ImageType): Type of image required mesh_name (str): mesh name in wild card format vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera + annotation_name (str, optional): Name of the annotation to be applied if using image type Annotation. """ - self.client.call('simAddDetectionFilterMeshName', camera_name, image_type, mesh_name, vehicle_name, external) + self.client.call('simAddDetectionFilterMeshName', camera_name, image_type, mesh_name, + vehicle_name, annotation_name) - def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicle_name = '', external = False): + def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicle_name='', annotation_name=""): """ Set detection radius for all cameras Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used image_type (ImageType): Type of image required radius_cm (int): Radius in [cm] vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera + annotation_name (str, optional): Name of the annotation to be applied if using image type Annotation. """ - self.client.call('simSetDetectionFilterRadius', camera_name, image_type, radius_cm, vehicle_name, external) + self.client.call('simSetDetectionFilterRadius', camera_name, image_type, radius_cm, vehicle_name, + annotation_name) - def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name = '', external = False): + def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name='', annotation_name=""): """ Clear all mesh names from detection filter Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera + annotation_name (str, optional): Name of the annotation to be applied if using image type Annotation. """ - self.client.call('simClearDetectionMeshNames', camera_name, image_type, vehicle_name, external) + self.client.call('simClearDetectionMeshNames', camera_name, image_type, vehicle_name, + annotation_name) - def simGetDetections(self, camera_name, image_type, vehicle_name = '', external = False): + def simGetDetections(self, camera_name, image_type, vehicle_name='', annotation_name=""): """ Get current detections Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used image_type (ImageType): Type of image required vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera + annotation_name (str, optional): Name of the annotation to be applied if using image type Annotation. Returns: DetectionInfo array """ - responses_raw = self.client.call('simGetDetections', camera_name, image_type, vehicle_name, external) + responses_raw = self.client.call('simGetDetections', camera_name, image_type, vehicle_name, + annotation_name) return [DetectionInfo.from_msgpack(response_raw) for response_raw in responses_raw] - def simPrintLogMessage(self, message, message_param = "", severity = 0): + def simPrintLogMessage(self, message, message_param="", severity=0): """ Prints the specified message in the simulator's window. - If message_param is supplied, then it's printed next to the message and in that case if this API is called with same message value - but different message_param again then previous line is overwritten with new line (instead of API creating new line on display). + If message_param is supplied, then it's printed next to the message and in that case if this API is + called with same message value + but different message_param again then previous line is overwritten with new line (instead of API creating + new line on display). - For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API is called with different values of i. + For example, `simPrintLogMessage("Iteration: ", to_string(i))` keeps updating same line on display when API + is called with different values of i. The valid values of severity parameter is 0 to 3 inclusive that corresponds to different colors. Args: @@ -707,65 +1183,65 @@ def simPrintLogMessage(self, message, message_param = "", severity = 0): """ self.client.call('simPrintLogMessage', message, message_param, severity) - def simGetCameraInfo(self, camera_name, vehicle_name = '', external=False): + def simGetCameraInfo(self, camera_name, vehicle_name=''): """ Get details about the camera Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera Returns: CameraInfo: """ -#TODO : below str() conversion is only needed for legacy reason and should be removed in future - return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name, external)) + # TODO : below str() conversion is only needed for legacy reason and should be removed in future + return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name)) - def simGetDistortionParams(self, camera_name, vehicle_name = '', external = False): + def simGetDistortionParams(self, camera_name, vehicle_name=''): """ Get camera distortion parameters Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera Returns: List (float): List of distortion parameter values corresponding to K1, K2, K3, P1, P2 respectively. """ - return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name, external) + return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name) - def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = '', external = False): + def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name=''): """ Set camera distortion parameters Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used distortion_params (dict): Dictionary of distortion param names and corresponding values {"K1": 0.0, "K2": 0.0, "K3": 0.0, "P1": 0.0, "P2": 0.0} vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera """ for param_name, value in distortion_params.items(): - self.simSetDistortionParam(camera_name, param_name, value, vehicle_name, external) + self.simSetDistortionParam(camera_name, param_name, value, vehicle_name) - def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = '', external = False): + def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name=''): """ Set single camera distortion parameter Args: - camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used + camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. + can also be used param_name (str): Name of distortion parameter value (float): Value of distortion parameter vehicle_name (str, optional): Vehicle which the camera is associated with - external (bool, optional): Whether the camera is an External Camera """ - self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name, external) + self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name) - def simSetCameraPose(self, camera_name, pose, vehicle_name = '', external = False): + def simSetCameraPose(self, camera_name, pose, vehicle_name=''): """ - Control the pose of a selected camera @@ -773,12 +1249,11 @@ def simSetCameraPose(self, camera_name, pose, vehicle_name = '', external = Fals camera_name (str): Name of the camera to be controlled pose (Pose): Pose representing the desired position and orientation of the camera vehicle_name (str, optional): Name of vehicle which the camera corresponds to - external (bool, optional): Whether the camera is an External Camera """ -#TODO : below str() conversion is only needed for legacy reason and should be removed in future - self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name, external) + #TODO : below str() conversion is only needed for legacy reason and should be removed in future + self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name) - def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = '', external = False): + def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name=''): """ - Control the field of view of a selected camera @@ -786,12 +1261,11 @@ def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = '', external camera_name (str): Name of the camera to be controlled fov_degrees (float): Value of field of view in degrees vehicle_name (str, optional): Name of vehicle which the camera corresponds to - external (bool, optional): Whether the camera is an External Camera """ -#TODO : below str() conversion is only needed for legacy reason and should be removed in future - self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name, external) + #TODO : below str() conversion is only needed for legacy reason and should be removed in future + self.client.call('simSetCameraFov', str(camera_name), fov_degrees, vehicle_name) - def simGetGroundTruthKinematics(self, vehicle_name = ''): + def simGetGroundTruthKinematics(self, vehicle_name=''): """ Get Ground truth kinematics of the vehicle @@ -805,13 +1279,15 @@ def simGetGroundTruthKinematics(self, vehicle_name = ''): """ kinematics_state = self.client.call('simGetGroundTruthKinematics', vehicle_name) return KinematicsState.from_msgpack(kinematics_state) + simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState} - def simSetKinematics(self, state, ignore_collision, vehicle_name = ''): + def simSetKinematics(self, state, ignore_collision, vehicle_name=''): """ Set the kinematics state of the vehicle - If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values + If you don't want to change position (or orientation) then just set components of position (or orientation) + to floating point nan values Args: state (KinematicsState): Desired Pose pf the vehicle @@ -820,7 +1296,39 @@ def simSetKinematics(self, state, ignore_collision, vehicle_name = ''): """ self.client.call('simSetKinematics', state, ignore_collision, vehicle_name) - def simGetGroundTruthEnvironment(self, vehicle_name = ''): + def simGetPhysicsRawKinematics(self, vehicle_name=''): + """ + Get Physics engine raw kinematics of the vehicle + + The position inside the returned KinematicsState is physics engine coordinate system, not Airsim + If you save it then you can load it back with simSetPhysicsRawKinematics + Accelaration is not used + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + KinematicsState: Physics engine raw kinematics + """ + kinematics_state = self.client.call('simGetPhysicsRawKinematics', vehicle_name) + return KinematicsState.from_msgpack(kinematics_state) + + simGetPhysicsRawKinematics.__annotations__ = {'return': KinematicsState} + + def simSetPhysicsRawKinematics(self, state, vehicle_name=''): + """ + Set Physics engine raw kinematics of the vehicle + + Need to be simGetPhysicsRawKinematics value or same coordinate system + Accelaration is not used + + Args: + state (KinematicsState): Desired Pose pf the vehicle + vehicle_name (str, optional): Name of the vehicle to move + """ + self.client.call('simSetPhysicsRawKinematics', state, vehicle_name) + + def simGetGroundTruthEnvironment(self, vehicle_name=''): """ Get ground truth environment state @@ -834,11 +1342,10 @@ def simGetGroundTruthEnvironment(self, vehicle_name = ''): """ env_state = self.client.call('simGetGroundTruthEnvironment', vehicle_name) return EnvironmentState.from_msgpack(env_state) - simGetGroundTruthEnvironment.__annotations__ = {'return': EnvironmentState} + simGetGroundTruthEnvironment.__annotations__ = {'return': EnvironmentState} -#sensor APIs - def getImuData(self, imu_name = '', vehicle_name = ''): + def getImuData(self, imu_name='', vehicle_name=''): """ Args: imu_name (str, optional): Name of IMU to get data from, specified in settings.json @@ -849,7 +1356,7 @@ def getImuData(self, imu_name = '', vehicle_name = ''): """ return ImuData.from_msgpack(self.client.call('getImuData', imu_name, vehicle_name)) - def getBarometerData(self, barometer_name = '', vehicle_name = ''): + def getBarometerData(self, barometer_name='', vehicle_name=''): """ Args: barometer_name (str, optional): Name of Barometer to get data from, specified in settings.json @@ -858,9 +1365,10 @@ def getBarometerData(self, barometer_name = '', vehicle_name = ''): Returns: BarometerData: """ - return BarometerData.from_msgpack(self.client.call('getBarometerData', barometer_name, vehicle_name)) + return BarometerData.from_msgpack(self.client.call('getBarometerData', barometer_name, + vehicle_name)) - def getMagnetometerData(self, magnetometer_name = '', vehicle_name = ''): + def getMagnetometerData(self, magnetometer_name='', vehicle_name=''): """ Args: magnetometer_name (str, optional): Name of Magnetometer to get data from, specified in settings.json @@ -869,9 +1377,10 @@ def getMagnetometerData(self, magnetometer_name = '', vehicle_name = ''): Returns: MagnetometerData: """ - return MagnetometerData.from_msgpack(self.client.call('getMagnetometerData', magnetometer_name, vehicle_name)) + return MagnetometerData.from_msgpack(self.client.call('getMagnetometerData', magnetometer_name, + vehicle_name)) - def getGpsData(self, gps_name = '', vehicle_name = ''): + def getGpsData(self, gps_name='', vehicle_name=''): """ Args: gps_name (str, optional): Name of GPS to get data from, specified in settings.json @@ -882,7 +1391,75 @@ def getGpsData(self, gps_name = '', vehicle_name = ''): """ return GpsData.from_msgpack(self.client.call('getGpsData', gps_name, vehicle_name)) - def getDistanceSensorData(self, distance_sensor_name = '', vehicle_name = ''): + def getTripStats(self, vehicle_name=''): + """ + Returns trip statistics for the vehicle. + + Kept for backward compatibility with clients that used the Microsoft AirSim + Python API helper. If the RPC endpoint is unavailable, this returns a default + `TripStats` instance instead of raising. + """ + if self._trip_stats_rpc_mode == "with_vehicle": + try: + return TripStats.from_msgpack( + self.client.call("getTripStats", vehicle_name) + ) + except Exception: + self._trip_stats_rpc_mode = "unknown" + + if self._trip_stats_rpc_mode == "no_vehicle": + try: + return TripStats.from_msgpack(self.client.call("getTripStats")) + except Exception: + self._trip_stats_rpc_mode = "unknown" + + if self._trip_stats_rpc_mode == "unavailable": + return TripStats() + + # Discovery path: try with vehicle arg first (if provided), then fallback. + if vehicle_name != "": + try: + trip_stats = TripStats.from_msgpack( + self.client.call("getTripStats", vehicle_name) + ) + self._trip_stats_rpc_mode = "with_vehicle" + return trip_stats + except Exception: + try: + trip_stats = TripStats.from_msgpack(self.client.call("getTripStats")) + self._trip_stats_rpc_mode = "no_vehicle" + return trip_stats + except Exception as exc: + self._trip_stats_rpc_mode = "unavailable" + self.__warn_trip_stats_unavailable_once(exc) + return TripStats() + + # No vehicle name provided, attempt no-arg signature first. + try: + trip_stats = TripStats.from_msgpack(self.client.call("getTripStats")) + self._trip_stats_rpc_mode = "no_vehicle" + return trip_stats + except Exception: + try: + trip_stats = TripStats.from_msgpack( + self.client.call("getTripStats", "") + ) + self._trip_stats_rpc_mode = "with_vehicle" + return trip_stats + except Exception as exc: + self._trip_stats_rpc_mode = "unavailable" + self.__warn_trip_stats_unavailable_once(exc) + return TripStats() + + def __warn_trip_stats_unavailable_once(self, exc): + if not VehicleClient._trip_stats_warned: + logging.warning( + "getTripStats RPC is unavailable (%s); returning default TripStats.", + exc, + ) + VehicleClient._trip_stats_warned = True + + def getDistanceSensorData(self, distance_sensor_name='', vehicle_name=''): """ Args: distance_sensor_name (str, optional): Name of Distance Sensor to get data from, specified in settings.json @@ -891,9 +1468,10 @@ def getDistanceSensorData(self, distance_sensor_name = '', vehicle_name = ''): Returns: DistanceSensorData: """ - return DistanceSensorData.from_msgpack(self.client.call('getDistanceSensorData', distance_sensor_name, vehicle_name)) + return DistanceSensorData.from_msgpack( + self.client.call('getDistanceSensorData', distance_sensor_name, vehicle_name)) - def getLidarData(self, lidar_name = '', vehicle_name = ''): + def getLidarData(self, lidar_name='', vehicle_name=''): """ Args: lidar_name (str, optional): Name of Lidar to get data from, specified in settings.json @@ -904,29 +1482,92 @@ def getLidarData(self, lidar_name = '', vehicle_name = ''): """ return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name)) - def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): + def getGPULidarData(self, lidar_name='', vehicle_name=''): + """ + Retrieves data from the specified GPU LiDAR sensor. + + Args: + lidar_name (str, optional): Name of the GPU LiDAR to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. + + Returns: + GPULidarData: Data from the specified GPU LiDAR sensor. + """ + return GPULidarData.from_msgpack(self.client.call('getGPULidarData', lidar_name, vehicle_name)) + + def getEchoData(self, echo_name='', vehicle_name=''): + """ + Retrieves data from the specified Echo sensor. + + Args: + echo_name (str, optional): Name of the Echo sensor to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. + + Returns: + EchoData: Data from the specified Echo sensor. + """ + return EchoData.from_msgpack(self.client.call('getEchoData', echo_name, vehicle_name)) + + def getUWBData(self, uwb_name='', vehicle_name=''): """ - NOTE: Deprecated API, use `getLidarData()` API instead - Returns Segmentation ID of each point's collided object in the last Lidar update + Retrieves data from the specified UWB (Ultra-Wideband) sensor. Args: - lidar_name (str, optional): Name of Lidar sensor - vehicle_name (str, optional): Name of the vehicle wth the sensor + uwb_name (str, optional): Name of the UWB sensor to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. Returns: - list[int]: Segmentation IDs of the objects + UwbData: Data from the specified UWB sensor. + """ + return UwbData.from_msgpack(self.client.call('getUWBData', uwb_name, vehicle_name)) + + def getUWBSensorData(self, uwb_name='', vehicle_name=''): """ - logging.warning("simGetLidarSegmentation API is deprecated, use getLidarData() API instead") - return self.getLidarData(lidar_name, vehicle_name).segmentation + Retrieves sensor-specific data from the specified UWB sensor. + + Args: + uwb_name (str, optional): Name of the UWB sensor to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. + + Returns: + UwbSensorData: Sensor-specific data from the specified UWB sensor. + """ + return UwbSensorData.from_msgpack(self.client.call('getUWBSensorData', uwb_name, vehicle_name)) + + def getWifiData(self, wifi_name='', vehicle_name=''): + """ + Retrieves data from the specified Wi-Fi sensor. + + Args: + wifi_name (str, optional): Name of the Wi-Fi sensor to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. + + Returns: + WifiData: Data from the specified Wi-Fi sensor. + """ + return WifiData.from_msgpack(self.client.call('getWifiData', wifi_name, vehicle_name)) + + def getWifiSensorData(self, wifi_name='', vehicle_name=''): + """ + Retrieves sensor-specific data from the specified Wi-Fi sensor. + + Args: + wifi_name (str, optional): Name of the Wi-Fi sensor to get data from, specified in settings.json. + vehicle_name (str, optional): Name of the vehicle to which the sensor corresponds. + + Returns: + WifiSensorData: Sensor-specific data from the specified Wi-Fi sensor. + """ + return WifiSensorData.from_msgpack(self.client.call('getWifiSensorData', wifi_name, + vehicle_name)) -#Plotting APIs def simFlushPersistentMarkers(self): """ Clear any persistent markers - those plotted with setting `is_persistent=True` in the APIs below """ self.client.call('simFlushPersistentMarkers') - def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, duration = -1.0, is_persistent = False): + def simPlotPoints(self, points, color_rgba=None, size=10.0, duration=-1.0, is_persistent=False): """ Plot a list of 3D points in World NED frame @@ -937,11 +1578,15 @@ def simPlotPoints(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], size = 10.0, du duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ + if color_rgba is None: + color_rgba = [1.0, 0.0, 0.0, 1.0] self.client.call('simPlotPoints', points, color_rgba, size, duration, is_persistent) - def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + def simPlotLineStrip(self, points, color_rgba=None, thickness=5.0, duration=-1.0, + is_persistent=False): """ - Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], ... , points[n-2] to points[n-1] + Plots a line strip in World NED frame, defined from points[0] to points[1], points[1] to points[2], + ... , points[n-2] to points[n-1] Args: points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects @@ -950,51 +1595,66 @@ def simPlotLineStrip(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ + if color_rgba is None: + color_rgba = [1.0, 0.0, 0.0, 1.0] self.client.call('simPlotLineStrip', points, color_rgba, thickness, duration, is_persistent) - def simPlotLineList(self, points, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, duration = -1.0, is_persistent = False): + def simPlotLineList(self, points, color_rgba=None, thickness=5.0, duration=-1.0, + is_persistent=False): """ - Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , points[n-2] to points[n-1] + Plots a line strip in World NED frame, defined from points[0] to points[1], points[2] to points[3], ... , + points[n-2] to points[n-1] Args: - points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. Must be even + points (list[Vector3r]): List of 3D locations of line start and end points, specified as Vector3r objects. + Must be even color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 thickness (float, optional): Thickness of line duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ + if color_rgba is None: + color_rgba = [1.0, 0.0, 0.0, 1.0] self.client.call('simPlotLineList', points, color_rgba, thickness, duration, is_persistent) - def simPlotArrows(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 1.0], thickness = 5.0, arrow_size = 2.0, duration = -1.0, is_persistent = False): + def simPlotArrows(self, points_start, points_end, color_rgba=None, thickness=5.0, arrow_size=2.0, + duration=-1.0, is_persistent=False): """ - Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to points_end[1], ... , points_start[n-1] to points_end[n-1] - + Plots a list of arrows in World NED frame, defined from points_start[0] to points_end[0], points_start[1] to + points_end[1], ... , points_start[n-1] to points_end[n-1] Args: - points_start (list[Vector3r]): List of 3D start positions of arrow start positions, specified as Vector3r objects - points_end (list[Vector3r]): List of 3D end positions of arrow start positions, specified as Vector3r objects + points_start (list[Vector3r]): List of 3D start positions of arrow start positions, + specified as Vector3r objects + points_end (list[Vector3r]): List of 3D end positions of arrow start positions, + specified as Vector3r objects color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 thickness (float, optional): Thickness of line arrow_size (float, optional): Size of arrow head duration (float, optional): Duration (seconds) to plot for is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ - self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) - + if color_rgba is None: + color_rgba = [1.0, 0.0, 0.0, 1.0] + self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, + arrow_size, duration, is_persistent) - def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0): + def simPlotStrings(self, strings, positions, scale=5, color_rgba=None, duration=-1.0): """ Plots a list of strings at desired positions in World NED frame. Args: strings (list[String], optional): List of strings to plot - positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list + positions (list[Vector3r]): List of positions where the strings should be plotted. + Should be in one-to-one correspondence with the strings' list scale (float, optional): Font scale of transform name color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 duration (float, optional): Duration (seconds) to plot for """ + if color_rgba is None: + color_rgba = [1.0, 0.0, 0.0, 1.0] self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration) - def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False): + def simPlotTransforms(self, poses, scale=5.0, thickness=5.0, duration=-1.0, is_persistent=False): """ Plots a list of transforms in World NED frame. @@ -1007,7 +1667,8 @@ def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0 """ self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent) - def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness = 5.0, text_scale = 10.0, text_color_rgba = [1.0, 0.0, 0.0, 1.0], duration = -1.0): + def simPlotTransformsWithNames(self, poses, names, tf_scale=5.0, tf_thickness=5.0, text_scale=10.0, + text_color_rgba=None, duration=-1.0): """ Plots a list of transforms with their names in World NED frame. @@ -1020,9 +1681,12 @@ def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name duration (float, optional): Duration (seconds) to plot for """ - self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration) + if text_color_rgba is None: + text_color_rgba = [1.0, 0.0, 0.0, 1.0] + self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, + text_color_rgba, duration) - def cancelLastTask(self, vehicle_name = ''): + def cancelLastTask(self, vehicle_name=''): """ Cancel previous Async task @@ -1031,7 +1695,6 @@ def cancelLastTask(self, vehicle_name = ''): """ self.client.call('cancelLastTask', vehicle_name) -#Recording APIs def startRecording(self): """ Start Recording @@ -1079,8 +1742,7 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): """ return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) -#Add new vehicle via RPC - def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path = ""): + def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path=""): """ Create vehicle at runtime @@ -1088,7 +1750,8 @@ def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path = ""): vehicle_name (str): Name of the vehicle being created vehicle_type (str): Type of vehicle, e.g. "simpleflight" pose (Pose): Initial pose of the vehicle - pawn_path (str, optional): Vehicle blueprint path, default empty wbich uses the default blueprint for the vehicle type + pawn_path (str, optional): Vehicle blueprint path, default empty which uses the default blueprint for the + vehicle type Returns: bool: Whether vehicle was created @@ -1106,19 +1769,30 @@ def listVehicles(self): def getSettingsString(self): """ - Fetch the settings text being used by AirSim + Fetch the settings text being used by Cosys-AirSim Returns: str: Settings text in JSON format """ return self.client.call('getSettingsString') -#----------------------------------- Multirotor APIs --------------------------------------------- + def simSetExtForce(self, ext_force): + """ + Set arbitrary external forces, in World frame, NED direction. Can be used + for implementing simple payloads. + + Args: + ext_force (Vector3r): Force, in World frame, NED direction, in N + """ + self.client.call('simSetExtForce', ext_force) + + +# ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): - def __init__(self, ip = "", port = 41451, timeout_value = 3600): + def __init__(self, ip="", port=41451, timeout_value=3600): super(MultirotorClient, self).__init__(ip, port, timeout_value) - def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): + def takeoffAsync(self, timeout_sec=20, vehicle_name=''): """ Takeoff vehicle to 3m above ground. Vehicle should not be moving when this API is used @@ -1131,7 +1805,7 @@ def takeoffAsync(self, timeout_sec = 20, vehicle_name = ''): """ return self.client.call_async('takeoff', timeout_sec, vehicle_name) - def landAsync(self, timeout_sec = 60, vehicle_name = ''): + def landAsync(self, timeout_sec=60, vehicle_name=''): """ Land the vehicle @@ -1144,7 +1818,7 @@ def landAsync(self, timeout_sec = 60, vehicle_name = ''): """ return self.client.call_async('land', timeout_sec, vehicle_name) - def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): + def goHomeAsync(self, timeout_sec=3e+38, vehicle_name=''): """ Return vehicle to Home i.e. Launch location @@ -1157,120 +1831,302 @@ def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''): """ return self.client.call_async('goHome', timeout_sec, vehicle_name) -#APIs for control - def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), vehicle_name=''): """ + Initiates a velocity command relative to the vehicle's body frame asynchronously. + Args: - vx (float): desired velocity in the X axis of the vehicle's local NED frame. - vy (float): desired velocity in the Y axis of the vehicle's local NED frame. - vz (float): desired velocity in the Z axis of the vehicle's local NED frame. - duration (float): Desired amount of time (seconds), to send this command for - drivetrain (DrivetrainType, optional): - yaw_mode (YawMode, optional): - vehicle_name (str, optional): Name of the multirotor to send this command to + vx (float): Desired velocity in the X axis of the vehicle's local NED frame. + vy (float): Desired velocity in the Y axis of the vehicle's local NED frame. + vz (float): Desired velocity in the Z axis of the vehicle's local NED frame. + duration (float): Desired amount of time (seconds) to send this command for. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + vehicle_name (str, optional): Name of the multirotor to send this command to. Returns: - msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. """ - return self.client.call_async('moveByVelocityBodyFrame', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + return self.client.call_async('moveByVelocityBodyFrame', vx, vy, vz, duration, drivetrain, + yaw_mode, vehicle_name) - def moveByVelocityZBodyFrameAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + def moveByVelocityZBodyFrameAsync(self, vx, vy, z, duration, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), vehicle_name=''): """ + Initiates a velocity command relative to the vehicle's body frame with a specified Z + axis position asynchronously. + Args: - vx (float): desired velocity in the X axis of the vehicle's local NED frame - vy (float): desired velocity in the Y axis of the vehicle's local NED frame - z (float): desired Z value (in local NED frame of the vehicle) - duration (float): Desired amount of time (seconds), to send this command for - drivetrain (DrivetrainType, optional): - yaw_mode (YawMode, optional): - vehicle_name (str, optional): Name of the multirotor to send this command to + vx (float): Desired velocity in the X axis of the vehicle's local NED frame. + vy (float): Desired velocity in the Y axis of the vehicle's local NED frame. + z (float): Desired Z value (in the local NED frame of the vehicle). + duration (float): Desired amount of time (seconds) to send this command for. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + vehicle_name (str, optional): Name of the multirotor to send this command to. Returns: - msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. """ + return self.client.call_async('moveByVelocityZBodyFrame', vx, vy, z, duration, drivetrain, + yaw_mode, vehicle_name) - return self.client.call_async('moveByVelocityZBodyFrame', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name=''): + """ + Initiates a movement command based on angles and Z axis position asynchronously. + + **Note:** This API is deprecated. Use `moveByRollPitchYawZAsync()` instead. - def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name = ''): + Args: + pitch (float): Desired pitch angle (in degrees). + roll (float): Desired roll angle (in degrees). + z (float): Desired Z value (in the local NED frame of the vehicle). + yaw (float): Desired yaw angle (in degrees). + duration (float): Desired amount of time (seconds) to send this command for. + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ logging.warning("moveByAngleZAsync API is deprecated, use moveByRollPitchYawZAsync() API instead") - return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, + vehicle_name) + + def moveByAngleThrottleAsync(self, pitch, roll, throttle, yaw_rate, duration, vehicle_name=''): + """ + Initiates a movement command based on angles, throttle, and yaw rate asynchronously. + + **Note:** This API is deprecated. Use `moveByRollPitchYawrateThrottleAsync()` instead. - def moveByAngleThrottleAsync(self, pitch, roll, throttle, yaw_rate, duration, vehicle_name = ''): - logging.warning("moveByAngleThrottleAsync API is deprecated, use moveByRollPitchYawrateThrottleAsync() API instead") - return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + Args: + pitch (float): Desired pitch angle (in degrees). + roll (float): Desired roll angle (in degrees). + throttle (float): Desired throttle value (normalized). + yaw_rate (float): Desired yaw rate (in degrees per second). + duration (float): Desired amount of time (seconds) to send this command for. + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ + logging.warning( + "moveByAngleThrottleAsync API is deprecated, use moveByRollPitchYawrateThrottleAsync() API instead") + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, + throttle, duration, vehicle_name) - def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + def moveByVelocityAsync(self, vx, vy, vz, duration, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), vehicle_name=''): """ + Initiates a velocity command in world (NED) frame asynchronously. + Args: - vx (float): desired velocity in world (NED) X axis - vy (float): desired velocity in world (NED) Y axis - vz (float): desired velocity in world (NED) Z axis - duration (float): Desired amount of time (seconds), to send this command for - drivetrain (DrivetrainType, optional): - yaw_mode (YawMode, optional): - vehicle_name (str, optional): Name of the multirotor to send this command to + vx (float): Desired velocity in the world (NED) X axis. + vy (float): Desired velocity in the world (NED) Y axis. + vz (float): Desired velocity in the world (NED) Z axis. + duration (float): Desired amount of time (seconds) to send this command for. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + vehicle_name (str, optional): Name of the multirotor to send this command to. Returns: - msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. """ - return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name) + return self.client.call_async('moveByVelocity', vx, vy, vz, duration, drivetrain, yaw_mode, + vehicle_name) - def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): - return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name) + def moveByVelocityZAsync(self, vx, vy, z, duration, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), vehicle_name=''): + """ + Initiates a velocity command with a specified Z axis position asynchronously. - def moveOnPathAsync(self, path, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), - lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): - return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + Args: + vx (float): Desired velocity in the world (NED) X axis. + vy (float): Desired velocity in the world (NED) Y axis. + z (float): Desired Z value (in the world (NED) frame). + duration (float): Desired amount of time (seconds) to send this command for. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + vehicle_name (str, optional): Name of the multirotor to send this command to. - def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), - lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): - return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ + return self.client.call_async('moveByVelocityZ', vx, vy, z, duration, drivetrain, yaw_mode, + vehicle_name) - def getTripStats(self, vehicle_name=''): - trip_stats_raw = self.client.call('getTripStats', vehicle_name) - return TripStats.from_msgpack(trip_stats_raw) - def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), - lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): - return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveOnPathAsync(self, path, velocity, timeout_sec=3e+38, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), lookahead=-1, adaptive_lookahead=1, vehicle_name=''): + """ + Initiates a movement along a specified path asynchronously. - def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): - return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + Args: + path (list[airsim.Vector3r]): List of waypoints defining the path. + velocity (float): Desired velocity along the path (meters per second). + timeout_sec (float, optional): Timeout duration in seconds. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + lookahead (float, optional): Lookahead distance for path following. + adaptive_lookahead (float, optional): Adaptive lookahead factor for path following. + vehicle_name (str, optional): Name of the multirotor to send this command to. - def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''): + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. """ - - Read current RC state and use it to control the vehicles. + return self.client.call_async('moveOnPath', path, velocity, timeout_sec, drivetrain, yaw_mode, + lookahead, adaptive_lookahead, vehicle_name) - Parameters sets up the constraints on velocity and minimum altitude while flying. If RC state is detected to violate these constraints - then that RC state would be ignored. + def moveToPositionAsync(self, x, y, z, velocity, timeout_sec=3e+38, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), lookahead=-1, adaptive_lookahead=1, vehicle_name=''): + """ + Initiates a movement to a specified position asynchronously. Args: - vx_max (float): max velocity allowed in x direction - vy_max (float): max velocity allowed in y direction - vz_max (float): max velocity allowed in z direction - z_min (float): min z allowed for vehicle position - duration (float): after this duration vehicle would switch back to non-manual mode - drivetrain (DrivetrainType): when ForwardOnly, vehicle rotates itself so that its front is always facing the direction of travel. If MaxDegreeOfFreedom then it doesn't do that (crab-like movement) - yaw_mode (YawMode): Specifies if vehicle should face at given angle (is_rate=False) or should be rotating around its axis at given rate (is_rate=True) - vehicle_name (str, optional): Name of the multirotor to send this command to + x (float): Desired X coordinate of the target position (in world (NED) frame). + y (float): Desired Y coordinate of the target position (in world (NED) frame). + z (float): Desired Z coordinate of the target position (in world (NED) frame). + velocity (float): Desired velocity towards the target position (meters per second). + timeout_sec (float, optional): Timeout duration in seconds. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + lookahead (float, optional): Lookahead distance for path following. + adaptive_lookahead (float, optional): Adaptive lookahead factor for path following. + vehicle_name (str, optional): Name of the multirotor to send this command to. + Returns: - msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ + return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, + yaw_mode, lookahead, + adaptive_lookahead, vehicle_name) + + def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec=3e+38, + drivetrain=DrivetrainType.MaxDegreeOfFreedom, yaw_mode=YawMode(), + lookahead=-1, adaptive_lookahead=1, vehicle_name=''): + """ + Initiates a movement to a specified GPS position asynchronously. + + Args: + latitude (float): Desired latitude of the target position. + longitude (float): Desired longitude of the target position. + altitude (float): Desired altitude of the target position (in meters). + velocity (float): Desired velocity towards the target position (meters per second). + timeout_sec (float, optional): Timeout duration in seconds. + drivetrain (DrivetrainType, optional): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + lookahead (float, optional): Lookahead distance for path following. + adaptive_lookahead (float, optional): Adaptive lookahead factor for path following. + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. """ - return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, vehicle_name) + return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, + timeout_sec, drivetrain, + yaw_mode, lookahead, adaptive_lookahead, vehicle_name) - def rotateToYawAsync(self, yaw, timeout_sec = 3e+38, margin = 5, vehicle_name = ''): + def moveToZAsync(self, z, velocity, timeout_sec=3e+38, yaw_mode=YawMode(), lookahead=-1, adaptive_lookahead=1, + vehicle_name=''): + """ + Initiates a movement to a specified Z axis position asynchronously. + + Args: + z (float): Desired Z coordinate of the target position (in meters). + velocity (float): Desired velocity towards the target position (meters per second). + timeout_sec (float, optional): Timeout duration in seconds. + yaw_mode (YawMode, optional): Specifies the yaw mode for the command. + lookahead (float, optional): Lookahead distance for path following. + adaptive_lookahead (float, optional): Adaptive lookahead factor for path following. + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ + return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, + adaptive_lookahead, + vehicle_name) + + def moveByManualAsync(self, vx_max, vy_max, z_min, duration, drivetrain=DrivetrainType.MaxDegreeOfFreedom, + yaw_mode=YawMode(), vehicle_name=''): + """ + Initiates a manual movement command asynchronously using RC state. + + This method sets constraints on velocity and minimum altitude while flying. If the RC state is detected to + violate these constraints, + that RC state would be ignored. + + Args: + vx_max (float): Maximum allowed velocity in the X direction (in meters per second). + vy_max (float): Maximum allowed velocity in the Y direction (in meters per second). + z_min (float): Minimum allowed Z coordinate for vehicle position (in meters). + duration (float): Duration after which the vehicle switches back to non-manual mode (in seconds). + drivetrain (DrivetrainType): Specifies the type of drivetrain used by the vehicle. + yaw_mode (YawMode): Specifies if the vehicle should face at a given angle (is_rate=False) + or should be rotating around its axis at a given rate (is_rate=True). + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ + return self.client.call_async('moveByManual', vx_max, vy_max, z_min, duration, drivetrain, yaw_mode, + vehicle_name) + + def rotateToYawAsync(self, yaw, timeout_sec=3e+38, margin=5, vehicle_name=''): + """ + Initiates a rotation command to a specific yaw angle asynchronously. + + Args: + yaw (float): Desired yaw angle (in degrees). + timeout_sec (float, optional): Timeout duration in seconds. + margin (float, optional): Margin allowed for reaching the desired yaw angle (in degrees). + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ return self.client.call_async('rotateToYaw', yaw, timeout_sec, margin, vehicle_name) - def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name = ''): + def rotateByYawRateAsync(self, yaw_rate, duration, vehicle_name=''): + """ + Initiates a rotation command at a specific yaw rate asynchronously. + + Args: + yaw_rate (float): Desired yaw rate (in degrees per second). + duration (float): Duration for which the vehicle should rotate (in seconds). + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ return self.client.call_async('rotateByYawRate', yaw_rate, duration, vehicle_name) - def hoverAsync(self, vehicle_name = ''): + def hoverAsync(self, vehicle_name=''): + """ + Initiates a hover command asynchronously. + + Args: + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ return self.client.call_async('hover', vehicle_name) - def moveByRC(self, rcdata = RCData(), vehicle_name = ''): + def moveByRC(self, rcdata=RCData(), vehicle_name=''): + """ + Initiates a movement command using RC data. + + Args: + rcdata (RCData, optional): RC data object specifying control input. + vehicle_name (str, optional): Name of the multirotor to send this command to. + + Returns: + msgpackrpc.future.Future: A future object. Call .join() to wait for the method to finish. + """ return self.client.call('moveByRC', rcdata, vehicle_name) -#low - level control API - def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name = ''): + def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, + vehicle_name=''): """ - Directly control the motors using PWM values @@ -1284,9 +2140,11 @@ def moveByMotorPWMsAsync(self, front_right_pwm, rear_left_pwm, front_left_pwm, r Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByMotorPWMs', front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name) + return self.client.call_async('moveByMotorPWMs', front_right_pwm, rear_left_pwm, + front_left_pwm, rear_right_pwm, + duration, vehicle_name) - def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = ''): + def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name=''): """ - z is given in local NED frame of the vehicle. - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. @@ -1296,17 +2154,20 @@ def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, + w.r.t. our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, + w.r.t. our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction + wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll (float): Desired roll angle, in radians. @@ -1319,29 +2180,34 @@ def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, vehicle_name) + return self.client.call_async('moveByRollPitchYawZ', roll, -pitch, -yaw, z, duration, + vehicle_name) - def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): + def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name=''): """ - Desired throttle is between 0.0 to 1.0 - - Roll angle, pitch angle, and yaw angle are given in **degrees** when using PX4 and in **radians** when using SimpleFlight, in the body frame. + - Roll angle, pitch angle, and yaw angle are given in **degrees** when using PX4 and in **radians** when + using SimpleFlight, in the body frame. - The body frame follows the Front Left Up (FLU) convention, and right-handedness. - Frame Convention: - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. + our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. + our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our + FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll (float): Desired roll angle. @@ -1354,9 +2220,10 @@ def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, duration, vehicle_name) + return self.client.call_async('moveByRollPitchYawThrottle', roll, -pitch, -yaw, throttle, + duration, vehicle_name) - def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name = ''): + def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, duration, vehicle_name=''): """ - Desired throttle is between 0.0 to 1.0 - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. @@ -1366,17 +2233,20 @@ def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, d - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, + w.r.t. our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, + w.r.t. our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt + our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll (float): Desired roll angle, in radians. @@ -1389,9 +2259,11 @@ def moveByRollPitchYawrateThrottleAsync(self, roll, pitch, yaw_rate, throttle, d Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, throttle, duration, vehicle_name) + return self.client.call_async('moveByRollPitchYawrateThrottle', roll, -pitch, -yaw_rate, + throttle, duration, + vehicle_name) - def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name = ''): + def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehicle_name=''): """ - z is given in local NED frame of the vehicle. - Roll angle, pitch angle, and yaw rate set points are given in **radians**, in the body frame. @@ -1401,17 +2273,20 @@ def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehic - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, + w.r.t. our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, + w.r.t. our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt + our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll (float): Desired roll angle, in radians. @@ -1424,9 +2299,10 @@ def moveByRollPitchYawrateZAsync(self, roll, pitch, yaw_rate, z, duration, vehic Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, vehicle_name) + return self.client.call_async('moveByRollPitchYawrateZ', roll, -pitch, -yaw_rate, z, duration, + vehicle_name) - def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name = ''): + def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, vehicle_name=''): """ - z is given in local NED frame of the vehicle. - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. @@ -1436,17 +2312,20 @@ def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, v - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. + our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. + our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU + body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll_rate (float): Desired roll rate, in radians / second @@ -1459,9 +2338,10 @@ def moveByAngleRatesZAsync(self, roll_rate, pitch_rate, yaw_rate, z, duration, v Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, z, duration, vehicle_name) + return self.client.call_async('moveByAngleRatesZ', roll_rate, -pitch_rate, -yaw_rate, + z, duration, vehicle_name) - def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name = ''): + def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name=''): """ - Desired throttle is between 0.0 to 1.0 - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. @@ -1471,17 +2351,20 @@ def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttl - X axis is along the **Front** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **roll** angle. - | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, w.r.t. our FLU body frame. + | Hence, rolling with a positive angle is equivalent to translating in the **right** direction, + w.r.t. our FLU body frame. - Y axis is along the **Left** direction of the quadrotor. | Clockwise rotation about this axis defines a positive **pitch** angle. - | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, w.r.t. our FLU body frame. + | Hence, pitching with a positive angle is equivalent to translating in the **front** direction, + w.r.t. our FLU body frame. - Z axis is along the **Up** direction. | Clockwise rotation about this axis defines a positive **yaw** angle. - | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. + | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU + body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: roll_rate (float): Desired roll rate, in radians / second @@ -1494,12 +2377,15 @@ def moveByAngleRatesThrottleAsync(self, roll_rate, pitch_rate, yaw_rate, throttl Returns: msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ - return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, throttle, duration, vehicle_name) + return self.client.call_async('moveByAngleRatesThrottle', roll_rate, -pitch_rate, -yaw_rate, + throttle, duration, + vehicle_name) - def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains(), vehicle_name = ''): + def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains(), vehicle_name=''): """ - - Modifying these gains will have an affect on *ALL* move*() APIs. - This is because any velocity setpoint is converted to an angle level setpoint which is tracked with an angle level controllers. + - Modifying these gains will have an effect on *ALL* move*() APIs. + This is because any velocity setpoint is converted to an angle level setpoint which is tracked + with an angle level controllers. That angle level setpoint is itself tracked with and angle rate controller. - This function should only be called if the default angle rate control PID gains need to be modified. @@ -1509,15 +2395,17 @@ def setAngleRateControllerGains(self, angle_rate_gains=AngleRateControllerGains( - Pass AngleRateControllerGains() to reset gains to default recommended values. vehicle_name (str, optional): Name of the multirotor to send this command to """ - self.client.call('setAngleRateControllerGains', *(angle_rate_gains.to_lists()+(vehicle_name,))) + self.client.call('setAngleRateControllerGains', *(angle_rate_gains.to_lists() + (vehicle_name,))) - def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGains(), vehicle_name = ''): + def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGains(), vehicle_name=''): """ - - Sets angle level controller gains (used by any API setting angle references - for ex: moveByRollPitchYawZAsync(), moveByRollPitchYawThrottleAsync(), etc) + - Sets angle level controller gains (used by any API setting angle references - + for ex: moveByRollPitchYawZAsync(), moveByRollPitchYawThrottleAsync(), etc.) - Modifying these gains will also affect the behaviour of moveByVelocityAsync() API. - This is because the AirSim flight controller will track velocity setpoints by converting them to angle set points. + This is because the Cosys-AirSim flight controller will track velocity setpoints + by converting them to angle set points. - This function should only be called if the default angle level control PID gains need to be modified. - - Passing AngleLevelControllerGains() sets gains to default airsim values. + - Passing AngleLevelControllerGains() sets gains to default Cosys-AirSim values. Args: angle_level_gains (AngleLevelControllerGains): @@ -1525,25 +2413,26 @@ def setAngleLevelControllerGains(self, angle_level_gains=AngleLevelControllerGai - Pass AngleLevelControllerGains() to reset gains to default recommended values. vehicle_name (str, optional): Name of the multirotor to send this command to """ - self.client.call('setAngleLevelControllerGains', *(angle_level_gains.to_lists()+(vehicle_name,))) + self.client.call('setAngleLevelControllerGains', *(angle_level_gains.to_lists() + + (vehicle_name,))) - def setVelocityControllerGains(self, velocity_gains=VelocityControllerGains(), vehicle_name = ''): + def setVelocityControllerGains(self, velocity_gains=VelocityControllerGains(), vehicle_name=''): """ - Sets velocity controller gains for moveByVelocityAsync(). - This function should only be called if the default velocity control PID gains need to be modified. - - Passing VelocityControllerGains() sets gains to default airsim values. + - Passing VelocityControllerGains() sets gains to default Cosys-AirSim values. Args: velocity_gains (VelocityControllerGains): - Correspond to the world X, Y, Z axes. - Pass VelocityControllerGains() to reset gains to default recommended values. - - Modifying velocity controller gains will have an affect on the behaviour of moveOnSplineAsync() and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. + - Modifying velocity controller gains will have an effect on the behaviour of moveOnSplineAsync() + and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. vehicle_name (str, optional): Name of the multirotor to send this command to """ - self.client.call('setVelocityControllerGains', *(velocity_gains.to_lists()+(vehicle_name,))) - + self.client.call('setVelocityControllerGains', *(velocity_gains.to_lists() + (vehicle_name,))) - def setPositionControllerGains(self, position_gains=PositionControllerGains(), vehicle_name = ''): + def setPositionControllerGains(self, position_gains=PositionControllerGains(), vehicle_name=''): """ Sets position controller gains for moveByPositionAsync. This function should only be called if the default position control PID gains need to be modified. @@ -1554,10 +2443,9 @@ def setPositionControllerGains(self, position_gains=PositionControllerGains(), v - Pass PositionControllerGains() to reset gains to default recommended values. vehicle_name (str, optional): Name of the multirotor to send this command to """ - self.client.call('setPositionControllerGains', *(position_gains.to_lists()+(vehicle_name,))) + self.client.call('setPositionControllerGains', *(position_gains.to_lists() + (vehicle_name,))) -#query vehicle state - def getMultirotorState(self, vehicle_name = ''): + def getMultirotorState(self, vehicle_name=''): """ The position inside the returned MultirotorState is in the frame of the vehicle's starting point @@ -1565,12 +2453,13 @@ def getMultirotorState(self, vehicle_name = ''): vehicle_name (str, optional): Vehicle to get the state of Returns: - MultirotorState: + MultirotorState: Struct containing multirotor state values """ return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name)) + getMultirotorState.__annotations__ = {'return': MultirotorState} -#query rotor states - def getRotorStates(self, vehicle_name = ''): + + def getRotorStates(self, vehicle_name=''): """ Used to obtain the current state of all a multirotor's rotors. The state includes the speeds, thrusts and torques for all rotors. @@ -1582,14 +2471,16 @@ def getRotorStates(self, vehicle_name = ''): RotorStates: Containing a timestamp and the speed, thrust and torque of all rotors. """ return RotorStates.from_msgpack(self.client.call('getRotorStates', vehicle_name)) + getRotorStates.__annotations__ = {'return': RotorStates} + #----------------------------------- Car APIs --------------------------------------------- class CarClient(VehicleClient, object): - def __init__(self, ip = "", port = 41451, timeout_value = 3600): + def __init__(self, ip="", port=41451, timeout_value=3600): super(CarClient, self).__init__(ip, port, timeout_value) - def setCarControls(self, controls, vehicle_name = ''): + def setCarControls(self, controls, vehicle_name=''): """ Control the car using throttle, steering, brake, etc. @@ -1599,7 +2490,7 @@ def setCarControls(self, controls, vehicle_name = ''): """ self.client.call('setCarControls', controls, vehicle_name) - def getCarState(self, vehicle_name = ''): + def getCarState(self, vehicle_name=''): """ The position inside the returned CarState is in the frame of the vehicle's starting point @@ -1607,7 +2498,7 @@ def getCarState(self, vehicle_name = ''): vehicle_name (str, optional): Name of vehicle Returns: - CarState: + CarState: Struct containing car state values """ state_raw = self.client.call('getCarState', vehicle_name) return CarState.from_msgpack(state_raw) @@ -1618,7 +2509,26 @@ def getCarControls(self, vehicle_name=''): vehicle_name (str, optional): Name of vehicle Returns: - CarControls: + CarControls: Struct containing control values """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) + + +#------------------------------ ComputerVision APIs --------------------------------------- +class ComputerVisionClient(VehicleClient, object): + def __init__(self, ip="", port=41451, timeout_value=3600): + super(ComputerVisionClient, self).__init__(ip, port, timeout_value) + + def getComputerVisionState(self, vehicle_name=''): + """ + The position inside the returned ComputerVisionState is in the frame of the vehicle's starting point + + Args: + vehicle_name (str, optional): Name of vehicle + + Returns: + ComputerVisionState: Struct containing computer vision state values + """ + state_raw = self.client.call('getComputerVisionState', vehicle_name) + return ComputerVisionState.from_msgpack(state_raw) diff --git a/backend/PythonClient/airsim/colormap.npy b/backend/PythonClient/airsim/colormap.npy new file mode 100644 index 000000000..eaa63f190 Binary files /dev/null and b/backend/PythonClient/airsim/colormap.npy differ diff --git a/backend/PythonClient/airsim/pfm.py b/backend/PythonClient/airsim/pfm.py index 6f9f963a8..29f177bba 100644 --- a/backend/PythonClient/airsim/pfm.py +++ b/backend/PythonClient/airsim/pfm.py @@ -1,9 +1,6 @@ import numpy as np -import matplotlib.pyplot as plt import re import sys -import pdb - def read_pfm(file): """ Read a pfm file """ @@ -38,11 +35,11 @@ def read_pfm(file): raise Exception('Malformed PFM header: width, height cannot be found') scale = float(file.readline().rstrip()) - if scale < 0: # little-endian + if scale < 0: # little-endian endian = '<' scale = -scale else: - endian = '>' # big-endian + endian = '>' # big-endian data = np.fromfile(file, endian + 'f') shape = (height, width, 3) if color else (height, width) @@ -50,7 +47,7 @@ def read_pfm(file): data = np.reshape(data, shape) # DEY: I don't know why this was there. file.close() - + return data, scale @@ -63,9 +60,9 @@ def write_pfm(file, image, scale=1): if image.dtype.name != 'float32': raise Exception('Image dtype must be float32.') - if len(image.shape) == 3 and image.shape[2] == 3: # color image + if len(image.shape) == 3 and image.shape[2] == 3: # color image color = True - elif len(image.shape) == 2 or len(image.shape) == 3 and image.shape[2] == 1: # greyscale + elif len(image.shape) == 2 or len(image.shape) == 3 and image.shape[2] == 1: # greyscale color = False else: raise Exception('Image must have H x W x 3, H x W x 1 or H x W dimensions.') diff --git a/backend/PythonClient/airsim/types.py b/backend/PythonClient/airsim/types.py index 51e50a327..d530d8448 100644 --- a/backend/PythonClient/airsim/types.py +++ b/backend/PythonClient/airsim/types.py @@ -1,6 +1,4 @@ -from __future__ import print_function -import msgpackrpc #install as admin: pip install msgpack-rpc-python -import numpy as np #pip install numpy +import numpy as np import math class MsgpackMixin: @@ -14,9 +12,19 @@ def to_msgpack(self, *args, **kwargs): @classmethod def from_msgpack(cls, encoded): obj = cls() - #obj.__dict__ = {k.decode('utf-8'): (from_msgpack(v.__class__, v) if hasattr(v, "__dict__") else v) for k, v in encoded.items()} - obj.__dict__ = { k : (v if not isinstance(v, dict) else getattr(getattr(obj, k).__class__, "from_msgpack")(v)) for k, v in encoded.items()} - #return cls(**msgpack.unpack(encoded)) + # ww = encoded.items() + # obj.__dict__ = {k.decode('utf-8'): (from_msgpack(v.__class__, v) if hasattr(v, "__dict__") else v) for k, v in encoded.items()} + if isinstance(encoded, dict): + for k, v in encoded.items(): + if (isinstance(v, dict) and hasattr(getattr(obj, k).__class__, 'from_msgpack')): + obj.__dict__[k] = getattr(getattr(obj, k).__class__, 'from_msgpack')(v) + else: + obj.__dict__[k] = v + else: + obj = encoded + + # obj.__dict__ = { k : (v if not isinstance(v, dict) else getattr(getattr(obj, k).__class__, "from_msgpack")(v)) for k, v in encoded.items()} + # return cls(**msgpack.unpack(encoded)) return obj class _ImageType(type): @@ -41,15 +49,22 @@ def OpticalFlow(cls): return 8 def OpticalFlowVis(cls): return 9 + def Lighting(cls): + return 10 + def Annotation(cls): + return 11 + def __getattr__(self, key): if key == 'DepthPlanner': - print('\033[31m'+"DepthPlanner has been (correctly) renamed to DepthPlanar. Please use ImageType.DepthPlanar instead."+'\033[0m') - raise AttributeError + print('\033[31m'+"DepthPlanner is deprecated. Please use ImageType.DepthPlanar instead."+'\033[0m') + return self.DepthPlanar + raise AttributeError(f"type object 'ImageType' has no attribute '{key}'") class ImageType(metaclass=_ImageType): Scene = 0 DepthPlanar = 1 + DepthPlanner = DepthPlanar DepthPerspective = 2 DepthVis = 3 DisparityNormalized = 4 @@ -58,6 +73,8 @@ class ImageType(metaclass=_ImageType): Infrared = 7 OpticalFlow = 8 OpticalFlowVis = 9 + Lighting = 10 + Annotation = 11 class DrivetrainType: MaxDegreeOfFreedom = 0 @@ -70,6 +87,7 @@ class LandedState: class WeatherParameter: Rain = 0 Roadwetness = 1 + RoadWetness = Roadwetness Snow = 2 RoadSnow = 3 MapleLeaf = 4 @@ -91,7 +109,7 @@ class Vector3r(MsgpackMixin): y_val = 0.0 z_val = 0.0 - def __init__(self, x_val = 0.0, y_val = 0.0, z_val = 0.0): + def __init__(self, x_val=0.0, y_val=0.0, z_val=0.0): self.x_val = x_val self.y_val = y_val self.z_val = z_val @@ -218,7 +236,7 @@ def rotate(self, other): else: raise ValueError('length of the other Quaternionr must be 1') else: - raise TypeError('unsupported operand type(s) for \'rotate\': %s and %s' % ( str(type(self)), str(type(other))) ) + raise TypeError('unsupported operand type(s) for \'rotate\': %s and %s' % ( str(type(self)), str(type(other))) ) def conjugate(self): return Quaternionr(-self.x_val, -self.y_val, -self.z_val, self.w_val) @@ -227,14 +245,17 @@ def star(self): return self.conjugate() def inverse(self): - return self.star() / self.dot(self) + return self.from_numpy_array(self.star().to_numpy_array() / self.dot(self)) def sgn(self): - return self/self.get_length() + return self / self.get_length() def get_length(self): return ( self.x_val**2 + self.y_val**2 + self.z_val**2 + self.w_val**2 )**0.5 + def from_numpy_array(self, array): + return Quaternionr(array[0], array[1], array[2], array[3]) + def to_numpy_array(self): return np.array([self.x_val, self.y_val, self.z_val, self.w_val], dtype=np.float32) @@ -261,6 +282,15 @@ def containsNan(self): def __iter__(self): return iter((self.position, self.orientation)) +class Twist(MsgpackMixin): + linear = Vector3r() + angular = Vector3r() + + def __init__(self, linear_val=Vector3r(), angular_val=Vector3r()): + self.linear = linear_val + self.angular = angular_val + + class CollisionInfo(MsgpackMixin): has_collided = False normal = Vector3r() @@ -313,14 +343,15 @@ class ImageRequest(MsgpackMixin): image_type = ImageType.Scene pixels_as_float = False compress = False + annotation_name = "" - def __init__(self, camera_name, image_type, pixels_as_float = False, compress = True): + def __init__(self, camera_name, image_type, pixels_as_float = False, compress = True, annotation_name = ""): # todo: in future remove str(), it's only for compatibility to pre v1.2 self.camera_name = str(camera_name) self.image_type = image_type self.pixels_as_float = pixels_as_float self.compress = compress - + self.annotation_name = annotation_name class ImageResponse(MsgpackMixin): image_data_uint8 = np.uint8(0) @@ -334,6 +365,7 @@ class ImageResponse(MsgpackMixin): width = 0 height = 0 image_type = ImageType.Scene + annotation_name = "" class CarControls(MsgpackMixin): throttle = 0.0 @@ -381,6 +413,10 @@ class EnvironmentState(MsgpackMixin): temperature = 0.0 air_density = 0.0 +class ComputerVisionState(MsgpackMixin): + kinematics_estimated = KinematicsState() + timestamp = np.uint64(0) + class CarState(MsgpackMixin): speed = 0.0 gear = 0 @@ -391,13 +427,6 @@ class CarState(MsgpackMixin): kinematics_estimated = KinematicsState() timestamp = np.uint64(0) -class TripStats(MsgpackMixin): - voltage = 0.0 - energy_consume = 0.0 - flight_time = 2.0 - distance_traveled = 0.0 - collision_count = 0 - class MultirotorState(MsgpackMixin): collision = CollisionInfo() kinematics_estimated = KinematicsState() @@ -409,6 +438,18 @@ class MultirotorState(MsgpackMixin): ready_message = "" can_arm = False + +class TripStats(MsgpackMixin): + # Keep compatibility with projects that still use VehicleClient.getTripStats(). + state_of_charge = 100.0 + voltage = 0.0 + current = 0.0 + energy_consumed = 0.0 + distance_traveled = 0.0 + flight_time = 0.0 + collision_count = 0 + + class RotorStates(MsgpackMixin): timestamp = np.uint64(0) rotors = [] @@ -425,7 +466,75 @@ class LidarData(MsgpackMixin): point_cloud = 0.0 time_stamp = np.uint64(0) pose = Pose() - segmentation = 0 + groundtruth = '' + + +class GPULidarData(MsgpackMixin): + point_cloud = 0.0 + time_stamp = np.uint64(0) + pose = Pose() + + +class EchoData(MsgpackMixin): + point_cloud = 0.0 + time_stamp = np.uint64(0) + pose = Pose() + groundtruth = '' + passive_beacons_point_cloud = 0.0 + passive_beacons_groundtruth = '' + + +class UwbSensorData(MsgpackMixin): + time_stamp = np.uint64(0) + pose = Pose() + beaconsActiveID = [] + beaconsActiveRssi = [] + beaconsActivePosX = [] + beaconsActivePosY = [] + beaconsActivePosZ = [] + + +class UwbData(MsgpackMixin): + time_stamp = [] + mur_achorId = [] + mur_anchorX = [] + mur_anchorY = [] + mur_anchorZ = [] + mur_anchor_valid_range = [] + mur_anchor_distance = [] + mur_anchor_rssi = [] + mura_tagId = [] + mura_tagX = [] + mura_tagY = [] + mura_tagZ = [] + mura_ranges = [] + + +class WifiSensorData(MsgpackMixin): + time_stamp = np.uint64(0) + pose = Pose() + beaconsActiveID = [] + beaconsActiveRssi = [] + beaconsActivePosX = [] + beaconsActivePosY = [] + beaconsActivePosZ = [] + + +class WifiData(MsgpackMixin): + time_stamp = [] + wr_achorId = [] + wr_anchorX = [] + wr_anchorY = [] + wr_anchorZ = [] + wr_anchor_valid_range = [] + wr_anchor_distance = [] + wr_anchor_rssi = [] + wra_tagId = [] + wra_tagX = [] + wra_tagY = [] + wra_tagZ = [] + wra_ranges = [] + class ImuData(MsgpackMixin): time_stamp = np.uint64(0) @@ -484,7 +593,7 @@ class DetectionInfo(MsgpackMixin): box2D = Box2D() box3D = Box3D() relative_pose = Pose() - + class PIDGains(): """ Struct to store values of PID gains. Used to transmit controller gain values while instantiating diff --git a/backend/PythonClient/airsim/utils.py b/backend/PythonClient/airsim/utils.py index 7f866d7d0..ee82fa555 100644 --- a/backend/PythonClient/airsim/utils.py +++ b/backend/PythonClient/airsim/utils.py @@ -1,13 +1,9 @@ -import numpy as np #pip install numpy -import math -import time import sys import os import inspect -import types import re import logging - +import csv from .types import * @@ -49,20 +45,146 @@ def write_file(filename, bstr): with open(filename, 'wb') as afile: afile.write(bstr) + +def get_colormap_channel_values(): + values = np.zeros(258, dtype=int) + step = 256 + iter = 0 + values[0] = 0 + while step >= 1: + val = step - 1 + while val <= 256: + iter = iter + 1 + values[iter] = int(val) + val = val + (step * 2) + step = int(step / 2) + init = True + return values + + +gammaCorrectionTable = [0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 79, 0, + 81, 0, 83, 0, 85, 0, 86, 0, 88, 0, + 90, 0, 93, 0, 95, 0, 96, 0, 98, 0, + 101, 0, 102, 0, 105, 0, 106, 0, 109, 0, + 110, 0, 113, 0, 114, 0, 117, 0, 119, 0, + 120, 0, 122, 0, 125, 0, 127, 0, 129, 0, + 131, 0, 133, 0, 135, 0, 137, 0, 139, 0, + 141, 0, 143, 0, 145, 0, 147, 147, 148, 150, + 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, + 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, + 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, + 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, + 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, + 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, + 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, + 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, + 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, + 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, + 251, 252, 253, 254, 255] + + +def get_colormap_colors(maxVal, enable1, enable2, enable3, colormap, channelValues, okValues): + if not enable1: + max1 = maxVal - 1 + else: + max1 = 0 + if not enable2: + max2 = maxVal - 1 + else: + max2 = 0 + if not enable3: + max3 = maxVal - 1 + else: + max3 = 0 + i = 0 + j = 0 + k = 0 + while i <= max1: + while j <= max2: + while k <= max3: + if enable1: + r = channelValues[maxVal] + else: + r = channelValues[i] + if enable2: + g = channelValues[maxVal] + else: + g = channelValues[j] + if enable3: + b = channelValues[maxVal] + else: + b = channelValues[k] + if r in okValues and g in okValues and b in okValues and r != 149 and g != 149 and b != 149: + colormap.append([gammaCorrectionTable[r], gammaCorrectionTable[g], gammaCorrectionTable[b]]) + k = k + 1 + j = j + 1 + k = 0 + i = i + 1 + j = 0 + + +def generate_colormap(): + channelValues = get_colormap_channel_values() + numPerChannel = 256 + colorMap = [] + okValues = [] + num_per_channel = 256 + uneven_start = 79 + full_start = 149 + for i in np.arange(uneven_start, full_start + 1, 2): + okValues.append(i) + for i in np.arange(full_start + 1, numPerChannel, 1): + okValues.append(i) + for maxChannelIndex in range(0, numPerChannel): + get_colormap_colors(maxChannelIndex, False, False, True, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, False, True, False, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, False, True, True, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, True, False, False, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, True, False, True, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, True, True, False, colorMap, channelValues, okValues) + get_colormap_colors(maxChannelIndex, True, True, True, colorMap, channelValues, okValues) + colorMap = np.asarray(colorMap) + return colorMap + +def load_read_csv(path: str): + with open(path, 'r') as csv_file: + reader = csv.reader(csv_file) + matrix = None + first_row = True + for row_index, row in enumerate(reader): + if first_row: + size = len(row) + matrix = np.zeros((size, size), dtype=np.int32) + first_row = False + matrix[row_index] = row + + return matrix + +def load_colormap(): + colorMap = np.load(os.path.dirname(os.path.abspath(__file__)) + "/colormap.npy") + return colorMap + + # helper method for converting getOrientation to roll/pitch/yaw # https:#en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - -def to_eularian_angles(q): +def quaternion_to_euler_angles(q): z = q.z_val y = q.y_val x = q.x_val w = q.w_val - ysqr = y * y # roll (x-axis rotation) - t0 = +2.0 * (w*x + y*z) - t1 = +1.0 - 2.0*(x*x + ysqr) - roll = math.atan2(t0, t1) + sinr_cosp = +2.0 * (w*x + y*z) + cosr_cosp = +1.0 - 2.0*(x*x + y*y) + roll = math.atan2(sinr_cosp, cosr_cosp) # pitch (y-axis rotation) t2 = +2.0 * (w*y - z*x) @@ -73,28 +195,158 @@ def to_eularian_angles(q): pitch = math.asin(t2) # yaw (z-axis rotation) - t3 = +2.0 * (w*z + x*y) - t4 = +1.0 - 2.0 * (ysqr + z*z) - yaw = math.atan2(t3, t4) + siny_cosp = +2.0 * (w*z + x*y) + cosy_cosp = +1.0 - 2.0 * (y*y + z*z) + yaw = math.atan2(siny_cosp, cosy_cosp) - return (pitch, roll, yaw) + return roll, pitch, yaw -def to_quaternion(pitch, roll, yaw): - t0 = math.cos(yaw * 0.5) - t1 = math.sin(yaw * 0.5) - t2 = math.cos(roll * 0.5) - t3 = math.sin(roll * 0.5) - t4 = math.cos(pitch * 0.5) - t5 = math.sin(pitch * 0.5) +def euler_to_quaternion(roll, pitch, yaw): + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) q = Quaternionr() - q.w_val = t0 * t2 * t4 + t1 * t3 * t5 #w - q.x_val = t0 * t3 * t4 - t1 * t2 * t5 #x - q.y_val = t0 * t2 * t5 + t1 * t3 * t4 #y - q.z_val = t1 * t2 * t4 - t0 * t3 * t5 #z + q.w_val = cy * cr * cp + sy * sr * sp + q.x_val = cy * sr * cp - sy * cr * sp + q.y_val = cy * cr * sp + sy * sr * cp + q.z_val = sy * cr * cp - cy * sr * sp return q + +def euler_to_rotation_matrix(roll, pitch, yaw): + cx = math.cos(roll * 0.5) + sx = math.sin(roll * 0.5) + cy = math.cos(pitch * 0.5) + sy = math.sin(pitch * 0.5) + cz = math.cos(yaw * 0.5) + sz = math.sin(yaw * 0.5) + + r11 = cy*cz + r12 = -cy*sz + r13 = sy + r21 = cx*sz + cz*sx*sy + r22 = cx*cz - sx*sy*sz + r23 = -cy*sx + r31 = sx*sz - cx*cz*sy + r32 = cz*sx + cx*sy*sz + r33 = cx*cy + + R = [[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]] + + return R + + +def apply_rotation_offset(position, pitch, yaw, roll): + R = euler_to_rotation_matrix(roll, pitch, yaw) + + rotated_position = Vector3r() + rotated_position.x_val = position.x_val * R[0][0] + position.y_val * R[0][1] + position.z_val * R[0][2] + rotated_position.y_val = position.x_val * R[1][0] + position.y_val * R[1][1] + position.z_val * R[1][2] + rotated_position.z_val = position.x_val * R[2][0] + position.y_val * R[2][1] + position.z_val * R[2][2] + + return rotated_position + +def get_camera_type(cameraType): + if cameraType == "Scene": + cameraTypeClass = ImageType.Scene + elif cameraType == "Segmentation": + cameraTypeClass = ImageType.Segmentation + elif cameraType == "DepthPerspective": + cameraTypeClass = ImageType.DepthPerspective + elif cameraType == "DepthPlanar": + cameraTypeClass = ImageType.DepthPlanar + elif cameraType == "DepthVis": + cameraTypeClass = ImageType.DepthVis + elif cameraType == "Infrared": + cameraTypeClass = ImageType.Infrared + elif cameraType == "SurfaceNormals": + cameraTypeClass = ImageType.SurfaceNormals + elif cameraType == "DisparityNormalized": + cameraTypeClass = ImageType.DisparityNormalized + elif cameraType == "OpticalFlow": + cameraTypeClass = ImageType.OpticalFlow + elif cameraType == "OpticalFlowVis": + cameraTypeClass = ImageType.OpticalFlowVis + elif cameraType == "Lighting": + cameraTypeClass = ImageType.Lighting + elif cameraType == "Annotation": + cameraTypeClass = ImageType.Annotation + else: + cameraTypeClass = ImageType.Scene + return cameraTypeClass + + +def is_pixels_as_float(cameraType): + if cameraType == "Scene": + return False + elif cameraType == "Segmentation": + return False + elif cameraType == "DepthPerspective": + return True + elif cameraType == "DepthPlanar": + return True + elif cameraType == "DepthVis": + return True + elif cameraType == "Infrared": + return False + elif cameraType == "SurfaceNormals": + return False + elif cameraType == "DisparityNormalized": + return True + elif cameraType == "OpticalFlow": + return False + elif cameraType == "OpticalFlowVis": + return False + elif cameraType == "Lighting": + return False + elif cameraType == "Annotation": + return False + else: + return False + + +def get_image_bytes(data, cameraType): + if cameraType == "Scene": + img_rgb_string = data.image_data_uint8 + elif cameraType == "Segmentation": + img_rgb_string = data.image_data_uint8 + elif cameraType == "DepthPerspective": + img_depth_float = data.image_data_float + img_depth_float32 = np.asarray(img_depth_float, dtype=np.float32) + img_rgb_string = img_depth_float32.tobytes() + elif cameraType == "DepthPlanar": + img_depth_float = data.image_data_float + img_depth_float32 = np.asarray(img_depth_float, dtype=np.float32) + img_rgb_string = img_depth_float32.tobytes() + elif cameraType == "DepthVis": + img_depth_float = data.image_data_float + img_depth_float32 = np.asarray(img_depth_float, dtype=np.float32) + img_rgb_string = img_depth_float32.tobytes() + elif cameraType == "Infrared": + img_rgb_string = data.image_data_uint8 + elif cameraType == "SurfaceNormals": + img_rgb_string = data.image_data_uint8 + elif cameraType == "DisparityNormalized": + img_depth_float = data.image_data_float + img_depth_float32 = np.asarray(img_depth_float, dtype=np.float32) + img_rgb_string = img_depth_float32.tobytes() + elif cameraType == "OpticalFlow": + img_rgb_string = data.image_data_uint8 + elif cameraType == "OpticalFlowVis": + img_rgb_string = data.image_data_uint8 + elif cameraType == "Lighting": + img_rgb_string = data.image_data_uint8 + elif cameraType == "Annotation": + img_rgb_string = data.image_data_uint8 + else: + img_rgb_string = data.image_data_uint8 + return img_rgb_string + def wait_key(message = ''): ''' Wait for a key press on the console and return it. ''' diff --git a/backend/PythonClient/check_airsim_api_usage.py b/backend/PythonClient/check_airsim_api_usage.py new file mode 100644 index 000000000..9e79b474b --- /dev/null +++ b/backend/PythonClient/check_airsim_api_usage.py @@ -0,0 +1,160 @@ +""" +Static compatibility checker for local AirSim API usage. + +It scans Python files outside `airsim/` and reports: +1) `airsim.` references that are not exported by local `airsim` package files. +2) Calls on AirSim client objects (`client.foo()`, `self.client.foo()`) where `foo` + is not a method on `VehicleClient` or `MultirotorClient`. +""" + +from __future__ import annotations + +import ast +from dataclasses import dataclass +from pathlib import Path +from typing import Dict, Iterable, List, Set, Tuple + + +ROOT = Path(__file__).resolve().parent +AIRSIM_DIR = ROOT / "airsim" +SKIP_PARTS = {"airsim", "__pycache__"} + + +@dataclass(frozen=True) +class Usage: + file: Path + line: int + name: str + + +def _parse(path: Path) -> ast.AST: + text = path.read_text(encoding="utf-8-sig") + return ast.parse(text, filename=str(path)) + + +def _iter_project_files(root: Path) -> Iterable[Path]: + for path in root.rglob("*.py"): + if any(part in SKIP_PARTS for part in path.parts): + continue + yield path + + +def _collect_airsim_exports() -> Tuple[Set[str], Set[str]]: + exports: Set[str] = set() + client_methods: Set[str] = set() + + for module_name in ("types.py", "utils.py", "client.py"): + path = AIRSIM_DIR / module_name + tree = _parse(path) + for node in tree.body: + if isinstance(node, (ast.ClassDef, ast.FunctionDef, ast.AsyncFunctionDef)): + exports.add(node.name) + elif isinstance(node, ast.Assign): + for target in node.targets: + if isinstance(target, ast.Name): + exports.add(target.id) + elif isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name): + exports.add(node.target.id) + + if isinstance(node, ast.ClassDef) and node.name in {"VehicleClient", "MultirotorClient"}: + for member in node.body: + if isinstance(member, ast.FunctionDef): + client_methods.add(member.name) + + return exports, client_methods + + +def _is_multirotor_ctor(call: ast.Call) -> bool: + func = call.func + return ( + isinstance(func, ast.Attribute) + and isinstance(func.value, ast.Name) + and func.value.id == "airsim" + and func.attr == "MultirotorClient" + ) + + +def _is_self_client_attr(node: ast.AST) -> bool: + return ( + isinstance(node, ast.Attribute) + and isinstance(node.value, ast.Name) + and node.value.id == "self" + and node.attr == "client" + ) + + +def _collect_usages(path: Path) -> Tuple[List[Usage], List[Usage]]: + tree = _parse(path) + module_symbol_usages: List[Usage] = [] + client_method_usages: List[Usage] = [] + known_client_vars: Set[str] = set() + + class Visitor(ast.NodeVisitor): + def visit_Assign(self, node: ast.Assign) -> None: + if isinstance(node.value, ast.Call) and _is_multirotor_ctor(node.value): + for target in node.targets: + if isinstance(target, ast.Name): + known_client_vars.add(target.id) + self.generic_visit(node) + + def visit_Attribute(self, node: ast.Attribute) -> None: + if isinstance(node.value, ast.Name) and node.value.id == "airsim": + module_symbol_usages.append(Usage(path, node.lineno, node.attr)) + self.generic_visit(node) + + def visit_Call(self, node: ast.Call) -> None: + if isinstance(node.func, ast.Attribute): + base = node.func.value + if isinstance(base, ast.Name) and base.id in known_client_vars: + client_method_usages.append(Usage(path, node.lineno, node.func.attr)) + elif _is_self_client_attr(base): + client_method_usages.append(Usage(path, node.lineno, node.func.attr)) + self.generic_visit(node) + + Visitor().visit(tree) + return module_symbol_usages, client_method_usages + + +def _format_grouped(usages: Iterable[Usage]) -> str: + grouped: Dict[str, List[Usage]] = {} + for usage in usages: + grouped.setdefault(usage.name, []).append(usage) + + lines: List[str] = [] + for name in sorted(grouped): + refs = ", ".join(f"{u.file.relative_to(ROOT)}:{u.line}" for u in sorted(grouped[name], key=lambda x: (str(x.file), x.line))) + lines.append(f" - {name}: {refs}") + return "\n".join(lines) + + +def main() -> int: + exports, client_methods = _collect_airsim_exports() + + all_symbol_usages: List[Usage] = [] + all_client_calls: List[Usage] = [] + + for file in _iter_project_files(ROOT): + symbols, client_calls = _collect_usages(file) + all_symbol_usages.extend(symbols) + all_client_calls.extend(client_calls) + + missing_symbols = [u for u in all_symbol_usages if u.name not in exports] + missing_client_calls = [u for u in all_client_calls if u.name not in client_methods] + + if not missing_symbols and not missing_client_calls: + print("AirSim compatibility check passed: no missing symbols or client methods found.") + return 0 + + if missing_symbols: + print("Missing airsim module symbols:") + print(_format_grouped(missing_symbols)) + + if missing_client_calls: + print("Missing AirSim client methods:") + print(_format_grouped(missing_client_calls)) + + return 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/backend/PythonClient/multirotor/airsim_application.py b/backend/PythonClient/multirotor/airsim_application.py index 65f3bdef7..29b00e19c 100644 --- a/backend/PythonClient/multirotor/airsim_application.py +++ b/backend/PythonClient/multirotor/airsim_application.py @@ -5,6 +5,7 @@ from abc import abstractmethod from PythonClient import airsim +from PythonClient.multirotor.client_factory import create_multirotor_client from PythonClient.multirotor.storage.storage_config import get_storage_service class AirSimApplication: @@ -16,7 +17,7 @@ def __init__(self): self.circular_mission_names = {"FlyInCircle"} self.polygon_mission_names = {"FlyToPoints", "FlyToPointsGeo"} self.point_mission_names = {"FlyStraight"} - self.client = airsim.MultirotorClient() + self.client = create_multirotor_client() # self.client.confirmConnection() self.setting_file = self.load_airsim_setting() self.drone_number = len(self.setting_file['Vehicles']) # only support name format of Drone1, Drone2... @@ -46,6 +47,56 @@ def load_cesium_setting(): cesium_json = json.load(f) return cesium_json + @staticmethod + def _normalize_vehicle_name(name): + if name is None: + return "" + return "".join(ch for ch in str(name).lower() if ch.isalnum()) + + def resolve_vehicle_name(self, requested_name): + requested = str(requested_name or "") + requested_normalized = self._normalize_vehicle_name(requested) + + configured_names = [] + vehicles = self.setting_file.get("Vehicles", {}) + if isinstance(vehicles, dict): + configured_names.extend(vehicles.keys()) + configured_names.extend(self.all_drone_names) + configured_names = list(dict.fromkeys(configured_names)) + + if requested in configured_names: + return requested + + if requested_normalized: + for candidate in configured_names: + if self._normalize_vehicle_name(candidate) == requested_normalized: + print(f"[AirSim vehicle] resolved '{requested}' -> '{candidate}'") + return candidate + + rpc_names = [] + try: + rpc_names = self.client.listVehicles() or [] + except Exception: + rpc_names = [] + + if requested in rpc_names: + return requested + + if requested_normalized: + for candidate in rpc_names: + if self._normalize_vehicle_name(candidate) == requested_normalized: + print(f"[AirSim vehicle] resolved '{requested}' -> '{candidate}'") + return candidate + + all_candidates = list(dict.fromkeys(configured_names + list(rpc_names))) + if len(all_candidates) == 1 and requested != all_candidates[0]: + print( + f"[AirSim vehicle] single candidate fallback '{requested}' -> '{all_candidates[0]}'" + ) + return all_candidates[0] + + return requested + def set_log_dir(self, dir_name): self.log_subdir = dir_name diff --git a/backend/PythonClient/multirotor/client_factory.py b/backend/PythonClient/multirotor/client_factory.py new file mode 100644 index 000000000..7d82a277f --- /dev/null +++ b/backend/PythonClient/multirotor/client_factory.py @@ -0,0 +1,32 @@ +import os + +from PythonClient import airsim + + +def _get_rpc_host(): + return os.getenv("DRV_UNREAL_HOST", "127.0.0.1") + + +def _get_rpc_port(): + raw_port = ( + os.getenv("DRV_UNREAL_RPC_PORT") + or os.getenv("DRV_UNREAL_API_PORT") + or "41451" + ) + try: + return int(raw_port) + except (TypeError, ValueError): + print( + f"[WARN] Invalid DRV_UNREAL_RPC_PORT/DRV_UNREAL_API_PORT value '{raw_port}', falling back to 41451" + ) + return 41451 + + +def get_rpc_target(): + return _get_rpc_host(), _get_rpc_port() + + +def create_multirotor_client(timeout_value=3600): + host, port = get_rpc_target() + print(f"[AirSim RPC target] host={host} port={port}") + return airsim.MultirotorClient(ip=host, port=port, timeout_value=timeout_value) diff --git a/backend/PythonClient/multirotor/control/jsonFileGenerator.py b/backend/PythonClient/multirotor/control/jsonFileGenerator.py index 391ec455b..25cf1fea3 100644 --- a/backend/PythonClient/multirotor/control/jsonFileGenerator.py +++ b/backend/PythonClient/multirotor/control/jsonFileGenerator.py @@ -12,8 +12,16 @@ def __init__(self, drone_number, min_separation_dist): self.min_separation_dist = min_separation_dist spawn_radius = math.ceil(drone_number ** 0.5) print("spawn_radius ", spawn_radius) - settings_template = {"SettingsVersion": 1.2, "SimMode": "Multirotor"} - one_drone_template = {"Drone1": {"VehicleType": "SimpleFlight", "X": 0, "Y": 0, "Z": 0}} + settings_template = { + "SettingsVersion": 2.0, + "SimMode": "Multirotor", + "EnableRpc": True, + "ApiServerPort": 41451, + "LocalHostIp": "0.0.0.0", + } + one_drone_template = { + "Drone1": {"VehicleType": "SimpleFlight", "X": 0, "Y": 0, "Z": 0} + } break_flag = False cur_num = 1 diff --git a/backend/PythonClient/multirotor/control/simulation_task_manager.py b/backend/PythonClient/multirotor/control/simulation_task_manager.py index 5b35882cd..ba81effa1 100644 --- a/backend/PythonClient/multirotor/control/simulation_task_manager.py +++ b/backend/PythonClient/multirotor/control/simulation_task_manager.py @@ -7,13 +7,16 @@ import traceback from datetime import datetime from queue import Queue -from time import sleep +from time import monotonic, sleep from dotenv import load_dotenv -from msgpackrpc.error import TransportError +from msgpackrpc.error import TimeoutError, TransportError from numpy import random -from PythonClient import airsim +from PythonClient.multirotor.client_factory import ( + create_multirotor_client, + get_rpc_target, +) from PythonClient.multirotor.socket.stream_manager import StreamManager from PythonClient.multirotor.util.geo.geo_util import GeoUtil @@ -89,6 +92,9 @@ def start(self): def __update_settings(self, raw_request_json): self.unreal_off() + # Always rebuild these from scratch for each task update. + self.__drone_mission_pair_list.clear() + self.__monitor_list.clear() new_setting_dot_json = copy.deepcopy(self.__DEFAULT_EMPTY_SETTINGS_DOT_JSON) self.__handle_streaming_settings(raw_request_json) self.__populate_drone_and_mission_settings(new_setting_dot_json, raw_request_json) @@ -112,47 +118,63 @@ def __run_fuzzy_test_batch(self, current_queue_top, fuzzy_test_dict): # hardcoded for user study for i in [0, 7, 14]: x, y, z = self.__set_fuzzy_wind_vector(i) - setting_copy['environment']['Wind']['X'] = x - setting_copy['environment']['Wind']['Y'] = y - setting_copy['environment']['Wind']['Z'] = z - self.__report_subdir_string = current_queue_top[1] + os.sep + f"Fuzzy_Wind_{i}" - self.__update_settings(setting_copy) + setting_copy["environment"]["Wind"]["X"] = x + setting_copy["environment"]["Wind"]["Y"] = y + setting_copy["environment"]["Wind"]["Z"] = z + self.__report_subdir_string = ( + current_queue_top[1] + os.sep + f"Fuzzy_Wind_{i}" + ) try: - self.__batch_exe_all(self.__drone_mission_pair_list, self.__monitor_list, fuzzy_test_dict) + self.__update_settings(setting_copy) + self.__batch_exe_all( + self.__drone_mission_pair_list, + self.__monitor_list, + fuzzy_test_dict, + ) except TransportError: print("DroneWorld not running") return - self.__drone_mission_pair_list.clear() - self.__monitor_list.clear() + finally: + self.__drone_mission_pair_list.clear() + self.__monitor_list.clear() elif fuzzy_test_dict["target"] == "Speed": for i in range(1, self.__FUZZY_TEST_MAX_SPEED, precision): fuzzy_test_dict["value"] = i - self.__report_subdir_string = current_queue_top[1] + os.sep + f"Fuzzy_Speed_{i}" - self.__update_settings(setting_copy) + self.__report_subdir_string = ( + current_queue_top[1] + os.sep + f"Fuzzy_Speed_{i}" + ) try: - self.__batch_exe_all(self.__drone_mission_pair_list, self.__monitor_list, fuzzy_test_dict) + self.__update_settings(setting_copy) + self.__batch_exe_all( + self.__drone_mission_pair_list, + self.__monitor_list, + fuzzy_test_dict, + ) except TransportError: print("DroneWorld not running") return - self.__drone_mission_pair_list.clear() - self.__monitor_list.clear() + finally: + self.__drone_mission_pair_list.clear() + self.__monitor_list.clear() else: print("Fuzzy test target not recognized") self.batch_number += 1 def __run_regular_batch(self, current_queue_top): - self.__update_settings(current_queue_top[0]) - print("next up: ", self.__drone_mission_pair_list, self.__monitor_list) - self.__report_subdir_string = current_queue_top[1] - try: - self.__batch_exe_all(self.__drone_mission_pair_list, self.__monitor_list, False) + self.__update_settings(current_queue_top[0]) + print("next up: ", self.__drone_mission_pair_list, self.__monitor_list) + self.__report_subdir_string = current_queue_top[1] + self.__batch_exe_all( + self.__drone_mission_pair_list, self.__monitor_list, False + ) + self.batch_number += 1 except TransportError: print("DroneWorld not running") return - self.batch_number += 1 - self.__drone_mission_pair_list.clear() - self.__monitor_list.clear() + finally: + self.__drone_mission_pair_list.clear() + self.__monitor_list.clear() # uuid: %Y-%m-%d-%H-%M-%S def add_task(self, raw_request_json, uuid): @@ -260,8 +282,33 @@ def __populate_monitor_list(self, raw_request_json): print("Warning: monitors payload is not a dict, skipping monitor setup") monitors = {} monitor_name_param_list = [] - for monitor, param in monitors.items(): - monitor_name_param_list.append((monitor, param['param'])) + allow_battery_monitor = ( + os.getenv("ENABLE_BATTERY_MONITOR", "true").strip().lower() == "true" + ) + for monitor, config in monitors.items(): + if not isinstance(config, dict): + print(f"Warning: monitor '{monitor}' config is not a dict, skipping") + continue + + is_enabled = config.get("enable", True) + if isinstance(is_enabled, str): + is_enabled = is_enabled.strip().lower() == "true" + if not bool(is_enabled): + continue + + if monitor == "battery_monitor" and not allow_battery_monitor: + print( + "[INFO] battery_monitor disabled via ENABLE_BATTERY_MONITOR=false." + ) + continue + + params = config.get("param", []) + if params is None: + params = [] + if not isinstance(params, list): + params = [params] + + monitor_name_param_list.append((monitor, params)) self.__monitor_list = monitor_name_param_list @staticmethod @@ -337,6 +384,9 @@ def __save_settings_dot_json(new_setting_dot_json): + ", ".join(debug_settings_candidates) ) + # Always emit Cosys-AirSim settings with the expected schema version. + new_setting_dot_json["SettingsVersion"] = 2.0 + os.makedirs(os.path.dirname(output_path), exist_ok=True) with open(output_path, "w") as f: json.dump(new_setting_dot_json, f, indent=4) @@ -375,7 +425,8 @@ def __batch_exe_all(self, drone_mission_pair_list, monitor_list, fuzzy_test_info :param fuzzy_test_info: Dict of fuzzy test info, None otherwise :return: None """ - airsim.MultirotorClient().reset() # reset scene before each task + rpc_client = self.__wait_for_airsim_rpc() + rpc_client.reset() # reset scene before each task mission_threads = [] monitor_threads = [] for drone_mission_pair in drone_mission_pair_list: @@ -417,6 +468,46 @@ def __batch_exe_all(self, drone_mission_pair_list, monitor_list, fuzzy_test_info mission_threads.clear() monitor_threads.clear() + @staticmethod + def __get_rpc_wait_seconds(): + raw_timeout = os.getenv("DRV_RPC_READY_TIMEOUT_SEC", "45") + raw_interval = os.getenv("DRV_RPC_READY_POLL_SEC", "1") + try: + timeout_sec = float(raw_timeout) + except (TypeError, ValueError): + timeout_sec = 45.0 + try: + interval_sec = float(raw_interval) + except (TypeError, ValueError): + interval_sec = 1.0 + return max(timeout_sec, 1.0), max(interval_sec, 0.1) + + def __wait_for_airsim_rpc(self): + timeout_sec, interval_sec = self.__get_rpc_wait_seconds() + host, port = get_rpc_target() + deadline = monotonic() + timeout_sec + attempt = 0 + last_error = None + + while monotonic() < deadline: + attempt += 1 + try: + client = create_multirotor_client(timeout_value=5) + if client.ping(): + print( + f"[AirSim RPC ready] host={host} port={port} attempt={attempt}" + ) + return client + except (TransportError, TimeoutError, OSError) as exc: + last_error = exc + except Exception as exc: # Defensive catch so the retry loop survives API changes. + last_error = exc + sleep(interval_sec) + + raise TransportError( + f"AirSim RPC not ready at {host}:{port} after {timeout_sec:.0f}s; last_error={last_error}" + ) + def __create_mission_thread(self, drone_mission_pair): """ Create one mission thread and return the thread and mission instance @@ -491,7 +582,16 @@ def __create_default_drone_full_length_reqeust(): @staticmethod def __create_default_empty_settings_dot_json(): - return dict(SettingsVersion=1.2, SimMode="Multirotor", ViewMode="SpringArmChase", Vehicles={}) + _, rpc_port = get_rpc_target() + return dict( + SettingsVersion=2.0, + SimMode="Multirotor", + ViewMode="SpringArmChase", + EnableRpc=True, + ApiServerPort=rpc_port, + LocalHostIp="0.0.0.0", + Vehicles={}, + ) @staticmethod def __to_pascal_case(string): @@ -577,12 +677,27 @@ def __environment_check(): """ Initialize json files if do not exist """ - file_path = os.path.join(os.path.expanduser('~'), "Documents", "AirSim") + os.sep + 'settings.json' - if not os.path.exists(file_path): - open(file_path, 'w').close() + airsim_dir = os.path.join(os.path.expanduser('~'), "Documents", "AirSim") + os.makedirs(airsim_dir, exist_ok=True) + + file_path = os.path.join(airsim_dir, "settings.json") + should_seed_settings = not os.path.exists(file_path) + if not should_seed_settings: + try: + with open(file_path, "r") as infile: + current_settings = json.load(infile) + should_seed_settings = not isinstance(current_settings, dict) + except (json.JSONDecodeError, OSError): + should_seed_settings = True + + if should_seed_settings: + _, rpc_port = get_rpc_target() default_settings = { - "SettingsVersion": 1.2, + "SettingsVersion": 2.0, "SimMode": "Multirotor", + "EnableRpc": True, + "ApiServerPort": rpc_port, + "LocalHostIp": "0.0.0.0", "Vehicles": { "Drone1": { "FlightController": "SimpleFlight", @@ -594,8 +709,9 @@ def __environment_check(): } with open(file_path, 'w') as outfile: json.dump(default_settings, outfile, indent=4) + print(f"[OK] Seeded valid settings.json at {file_path}") - file_path = os.path.join(os.path.expanduser('~'), "Documents", "AirSim") + os.sep + 'cesium.json' + file_path = os.path.join(airsim_dir, 'cesium.json') if not os.path.exists(file_path): open(file_path, 'w').close() default_settings = { @@ -606,6 +722,12 @@ def __environment_check(): with open(file_path, 'w') as outfile: json.dump(default_settings, outfile, indent=4) + file_path = os.path.join(airsim_dir, 'materials.csv') + if not os.path.exists(file_path): + # Cosys-AirSim looks for this file during stencil annotation initialization. + with open(file_path, 'w') as outfile: + outfile.write("") + def get_current_task_batch(self): return self.__current_task_batch diff --git a/backend/PythonClient/multirotor/mission/abstract/abstract_mission.py b/backend/PythonClient/multirotor/mission/abstract/abstract_mission.py index 751ba3fec..9bc72934d 100644 --- a/backend/PythonClient/multirotor/mission/abstract/abstract_mission.py +++ b/backend/PythonClient/multirotor/mission/abstract/abstract_mission.py @@ -16,13 +16,13 @@ class State(Enum): def __init__(self, target_drone="Default"): self.flight_time_in_seconds = None super().__init__() - self.target_drone = target_drone + self.target_drone = self.resolve_vehicle_name(target_drone) self.state = self.State.IDLE self.report_dir = os.path.join(os.path.expanduser('~'), "Documents", "AirSim") + os.sep + datetime.datetime.now().strftime("%Y_%m_%d_%H:%M:%S") self.objects = [self.client.simGetObjectPose(i) for i in self.all_drone_names] self.states = [self.client.getMultirotorState(i) for i in self.all_drone_names] - self.client.enableApiControl(True, vehicle_name=target_drone) + self.client.enableApiControl(True, vehicle_name=self.target_drone) def takeoff(self, drone_name): self.client.takeoffAsync(vehicle_name=drone_name).join() diff --git a/backend/PythonClient/multirotor/monitor/battery_monitor.py b/backend/PythonClient/multirotor/monitor/battery_monitor.py index 098f5145f..ba1cc1bc8 100644 --- a/backend/PythonClient/multirotor/monitor/battery_monitor.py +++ b/backend/PythonClient/multirotor/monitor/battery_monitor.py @@ -1,27 +1,61 @@ -from PythonClient.multirotor.monitor.abstract.single_drone_mission_monitor import SingleDroneMissionMonitor +from time import sleep + +from PythonClient.multirotor.monitor.abstract.single_drone_mission_monitor import ( + SingleDroneMissionMonitor, +) class BatteryMonitor(SingleDroneMissionMonitor): - def __init__(self, mission, min_battery_percentage=15): + def __init__( + self, mission, min_battery_percentage=15, target_failure_percentage=None + ): super().__init__(mission) - self.min_battery_percentage = min_battery_percentage + self.min_battery_percentage = self.__to_float(min_battery_percentage, 15.0) + self.target_failure_percentage = self.__to_float( + target_failure_percentage, None + ) self.passed = True + self._low_battery_reported = False + self._skipped = False self.mission = mission self.target = mission.target_drone + @staticmethod + def __to_float(value, default): + try: + return float(value) + except (TypeError, ValueError): + return default + def start(self): - self.append_info_to_log(f"{self.target_drone};speed {self.mission.speed} m/s with wind {self.wind_speed_text}") - while self.mission.state == self.mission.IDLE: - pass - while self.mission.state != self.mission.END: - charge = self.client.getTripStats().state_of_charge - if charge < self.min_battery_percentage: + self.append_info_to_log( + f"{self.target_drone};speed {self.mission.speed} m/s with wind {self.wind_speed_text}" + ) + while self.mission.state == self.mission.State.IDLE: + sleep(0.1) + while self.mission.state != self.mission.State.END: + trip_stats = self.client.getTripStats(vehicle_name=self.target_drone) + if getattr(self.client, "_trip_stats_rpc_mode", "") == "unavailable": + self._skipped = True + break + charge = trip_stats.state_of_charge + if charge < self.min_battery_percentage and not self._low_battery_reported: + self._low_battery_reported = True self.passed = False - self.append_fail_to_log(f"{self.target_drone};Battery is below {self.min_battery_percentage}%") + self.append_fail_to_log( + f"{self.target_drone};Battery is below {self.min_battery_percentage}%" + ) + sleep(0.1) self.stop() def stop(self): + if self._skipped: + self.append_info_to_log( + f"{self.target_drone};Battery monitor skipped because getTripStats is unsupported by this simulator" + ) + self.save_report() + return if self.passed: self.append_pass_to_log(f"{self.target_drone};Battery is always above {self.min_battery_percentage}%") else: diff --git a/backend/PythonClient/multirotor/monitor/unordered_waypoint_monitor.py b/backend/PythonClient/multirotor/monitor/unordered_waypoint_monitor.py index ff13fbf1d..7871cabab 100644 --- a/backend/PythonClient/multirotor/monitor/unordered_waypoint_monitor.py +++ b/backend/PythonClient/multirotor/monitor/unordered_waypoint_monitor.py @@ -18,8 +18,15 @@ def start(self): self.stop() def init_dict(self): + self.point_dict.clear() for p in self.mission.points: - self.point_dict[p] = False + self.point_dict[self.__point_key(p)] = False + + @staticmethod + def __point_key(point): + if isinstance(point, (list, tuple)): + return tuple(point) + return point def update_dict(self): dt = 0.1 @@ -28,9 +35,14 @@ def update_dict(self): vehicle_name=self.target_drone).kinematics_estimated.position cur = (position.x_val, position.y_val, position.z_val) for p in self.mission.points: - if (self.get_distance_btw_points(cur, p)) <= self.deviation_threshold and not self.point_dict[p]: - self.append_info_to_log(f"{self.target_drone};reached {p} within {self.deviation_threshold} meters") - self.point_dict[p] = True + key = self.__point_key(p) + if ( + self.get_distance_btw_points(cur, p) + ) <= self.deviation_threshold and not self.point_dict[key]: + self.append_info_to_log( + f"{self.target_drone};reached {p} within {self.deviation_threshold} meters" + ) + self.point_dict[key] = True sleep(dt) def stop(self): diff --git a/backend/PythonClient/multirotor/socket/mission_streamer.py b/backend/PythonClient/multirotor/socket/mission_streamer.py index 8c1e53d3b..9c9d3ef7b 100644 --- a/backend/PythonClient/multirotor/socket/mission_streamer.py +++ b/backend/PythonClient/multirotor/socket/mission_streamer.py @@ -1,6 +1,7 @@ import cv2 import numpy as np import PythonClient.airsim as airsim +from PythonClient.multirotor.client_factory import create_multirotor_client class MissionStreamer: @@ -15,7 +16,7 @@ def __init__(self, mission): self.DECODE_EXTENSION = '.jpg' self.mission = mission self.target_drone = mission.target_drone - self.client = airsim.MultirotorClient() # initialize a new client to prevent mission lag + self.client = create_multirotor_client() # initialize a new client to prevent mission lag def frame_generator(self): while self.mission.state != "end": diff --git a/backend/PythonClient/multirotor/socket/stream_manager.py b/backend/PythonClient/multirotor/socket/stream_manager.py index 382b02991..ec8f943b7 100644 --- a/backend/PythonClient/multirotor/socket/stream_manager.py +++ b/backend/PythonClient/multirotor/socket/stream_manager.py @@ -1,4 +1,4 @@ -import PythonClient.airsim as airsim +from PythonClient.multirotor.client_factory import create_multirotor_client from PythonClient.multirotor.socket.mission_streamer import MissionStreamer @@ -8,7 +8,7 @@ class StreamManager: """ def __init__(self): - self.client = airsim.MultirotorClient() + self.client = create_multirotor_client() self.streamers = [] def get_stream(self, drone_name, camera_name): diff --git a/backend/PythonClient/multirotor/util/geo/geo_util.py b/backend/PythonClient/multirotor/util/geo/geo_util.py index feb0d4615..24ea5cfcf 100644 --- a/backend/PythonClient/multirotor/util/geo/geo_util.py +++ b/backend/PythonClient/multirotor/util/geo/geo_util.py @@ -194,17 +194,15 @@ def get_elevation(lat, lng): api_key = os.getenv('GOOGLE_MAPS_API_KEY', 'google maps api key') url = f"https://maps.googleapis.com/maps/api/elevation/json?locations={lat}%2C{lng}&key={api_key}" response = requests.get(url).json() - - # Check if the request was successful - if response.get('status') != 'OK': - print(f"Error from API: {response.get('status')}") - print(f"API Response: {response}") + + # Check if the request was successful without logging response fields. + if response.get("status") != "OK": + print("Error from elevation API.") return None # Check if 'results' is in the response and not empty if 'results' in response and response['results']: return response['results'][0]['elevation'] else: - print(f"No elevation data found for location: {lat}, {lng}") - print(f"API Response: {response}") + print("No elevation data found from elevation API.") return None \ No newline at end of file diff --git a/backend/PythonClient/multirotor/wind_control/airsim_wind_controller.py b/backend/PythonClient/multirotor/wind_control/airsim_wind_controller.py index f4b0c4628..7253db4ee 100644 --- a/backend/PythonClient/multirotor/wind_control/airsim_wind_controller.py +++ b/backend/PythonClient/multirotor/wind_control/airsim_wind_controller.py @@ -2,6 +2,7 @@ import time from PythonClient import airsim +from PythonClient.multirotor.client_factory import create_multirotor_client from PythonClient.multirotor.wind_control.openfoam_csv_reader import FoamCSVReader from PythonClient.multirotor.wind_control.openfoam_data_reader import FoamReader @@ -17,7 +18,7 @@ def __init__(self, openfoam_data_root, reader, filename): :param reader: str, "csv" or "foam", "csv" reads the data from csv files, "foam" reads from the openfoam data directly, abandoned, too slow. """ - self.client = airsim.MultirotorClient() + self.client = create_multirotor_client() self.client.confirmConnection() foam_data_root = openfoam_data_root if foam_data_root is not None: diff --git a/backend/mock_simulator/mock_task_manager.py b/backend/mock_simulator/mock_task_manager.py index f4530d02c..d661f04fe 100644 --- a/backend/mock_simulator/mock_task_manager.py +++ b/backend/mock_simulator/mock_task_manager.py @@ -13,6 +13,14 @@ def __init__(self): self.state = True self.unreal_state = {"state": "idle (mock)"} self.__user_directory = os.path.join(os.path.expanduser('~'), "Documents", "AirSim") + self.__ensure_airsim_files() + + def __ensure_airsim_files(self): + os.makedirs(self.__user_directory, exist_ok=True) + materials_path = os.path.join(self.__user_directory, "materials.csv") + if not os.path.exists(materials_path): + with open(materials_path, "w") as f: + f.write("") def createFakeReport(self, uuid): print("uploading report") @@ -61,4 +69,4 @@ def load_cesium_setting(self): cesium_setting = json.load(f) except FileNotFoundError: print("Cesium file not found") - return cesium_setting \ No newline at end of file + return cesium_setting diff --git a/docker-compose.dev.yaml b/docker-compose.dev.yaml index 6fe8e4010..b8122d85a 100644 --- a/docker-compose.dev.yaml +++ b/docker-compose.dev.yaml @@ -20,7 +20,8 @@ services: environment: - BACKEND_HOST=${BACKEND_HOST:-backend} - FLASK_RUN_PORT=${BACKEND_PORT:-5000} - - DRV_UNREAL_HOST=drv-unreal + - DRV_UNREAL_HOST=${DRV_UNREAL_HOST:-host.docker.internal} + - DRV_UNREAL_RPC_PORT=${DRV_UNREAL_RPC_PORT:-41451} - DRV_UNREAL_API_PORT=${SIM_INTERNAL_PORT:-3000} - DRV_PIXELSTREAM_PORT=${PIXELSTREAM_PORT:-8888} - BACKEND_PORT=${BACKEND_PORT:-5000} diff --git a/docker-compose.yaml b/docker-compose.yaml index 7fb285419..4a9aa79b0 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -71,7 +71,7 @@ services: - "-nullrhi" - "-nosound" - "-unattended" - - "-Port=${SIM_INTERNAL_PORT}" + - "-Port=${SIM_INTERNAL_PORT:-3000}" environment: - DISPLAY=${DISPLAY:-:0} volumes: diff --git a/readme.md b/readme.md index 3a3b450d5..589947370 100644 --- a/readme.md +++ b/readme.md @@ -374,6 +374,10 @@ STORAGE_EMULATOR_HOST=${BACKEND_HOST:-localhost}:${FAKE_GCS_PORT:-4443} # Optional: when true, backend loads a local debug settings.json override before writing AirSim settings JSON_DEBUG_MODE=false +# Optional: controls whether backend runs battery_monitor when enabled in frontend payload +# Default true; set false if your simulator build doesn't expose getTripStats reliably +ENABLE_BATTERY_MONITOR=true + ``` ### JSON Debug Mode (settings override) @@ -396,6 +400,14 @@ Behavior: - If `JSON_DEBUG_MODE=true` but no debug `settings.json` exists, backend falls back to normal generated settings. - Default is `false`, so normal settings generation is unchanged unless you opt in. +### Battery Monitor Toggle (`ENABLE_BATTERY_MONITOR`) + +- `ENABLE_BATTERY_MONITOR` controls whether backend instantiates `battery_monitor` entries from the frontend monitor payload. +- Default is `true`. +- When set to `true`, battery monitors run when enabled in the frontend configuration. +- Set it to `false` if your simulator build does not support `getTripStats` RPC reliably. +- Use `false` to avoid repeated battery-monitor warnings/failures on incompatible simulator builds. + The contents of `./frontend/.env` should include the following variables: ```sh