diff --git a/client/python/example_user_scripts/hello_unreal_vehicle.py b/client/python/example_user_scripts/hello_unreal_vehicle.py new file mode 100644 index 00000000..84152efb --- /dev/null +++ b/client/python/example_user_scripts/hello_unreal_vehicle.py @@ -0,0 +1,162 @@ +""" +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Project AirSim Unreal vehicle motion and sensor example. + +Loads scene_unreal_vehicle.jsonc, sends throttle/brake/steering actuator +commands, and prints ground-truth kinematics to confirm that the Unreal/Chaos +vehicle is moving, synchronizing state back into Project AirSim, and publishing +its configured sensor streams. +""" + +import asyncio +from pathlib import Path + +from projectairsim import ProjectAirSimClient, World +from projectairsim.unreal_vehicle import UnrealVehicle +from projectairsim.utils import projectairsim_log + + +sample_counts = {} + + +def summarize_sensor_sample(sample): + if not isinstance(sample, dict): + return sample + + summary = {} + for key, value in sample.items(): + if key in ("data", "point_cloud"): + summary[key] = f"<{len(value)} values>" + else: + summary[key] = value + return summary + + +def make_sensor_logger(sensor_id: str, stream_name: str, every: int = 20): + key = f"{sensor_id}.{stream_name}" + sample_counts[key] = 0 + + def log_sample(_, sample): + sample_counts[key] += 1 + if sample_counts[key] % every == 0: + projectairsim_log().info(f"{key}: {summarize_sensor_sample(sample)}") + + return log_sample + + +def subscribe_sensors(client: ProjectAirSimClient, vehicle: UnrealVehicle): + for sensor_id, streams in vehicle.sensors.items(): + for stream_name, topic in streams.items(): + if stream_name.endswith("_info"): + continue + + every = 60 if stream_name.endswith("_camera") else 20 + client.subscribe(topic, make_sensor_logger(sensor_id, stream_name, every)) + projectairsim_log().info(f"Subscribed to {sensor_id}.{stream_name}") + + +def set_controls( + vehicle: UnrealVehicle, + throttle: float = 0.0, + brake: float = 0.0, + steering: float = 0.0, +): + vehicle.set_parameter("throttle", throttle) + vehicle.set_parameter("brake", brake) + vehicle.set_parameter("steering", steering) + + +def stop_vehicle(vehicle: UnrealVehicle): + set_controls(vehicle, throttle=0.0, brake=0.0, steering=0.0) + + +def get_vector(data: dict, *path: str) -> dict: + value = data + for key in path: + if not isinstance(value, dict): + return {} + value = value.get(key, {}) + return value if isinstance(value, dict) else {} + + +async def log_kinematics(vehicle: UnrealVehicle, label: str): + kin = vehicle.get_kinematics() + if not isinstance(kin, dict): + projectairsim_log().info(f"{label}: {kin}") + return + + pos = get_vector(kin, "pose", "position") or get_vector(kin, "position") + vel = get_vector(kin, "twist", "linear") or get_vector(kin, "linear_velocity") + + projectairsim_log().info( + f"{label}: " + f"pos=({pos.get('x', 0):.2f}, {pos.get('y', 0):.2f}, {pos.get('z', 0):.2f}) " + f"vel=({vel.get('x', 0):.2f}, {vel.get('y', 0):.2f}, {vel.get('z', 0):.2f})" + ) + + +async def run_motion_test(vehicle: UnrealVehicle): + await log_kinematics(vehicle, "Initial") + + projectairsim_log().info("Forward") + set_controls(vehicle, throttle=0.7) + await asyncio.sleep(3.0) + await log_kinematics(vehicle, "After forward") + + projectairsim_log().info("Right curve") + set_controls(vehicle, throttle=0.35, steering=0.75) + await asyncio.sleep(3.0) + await log_kinematics(vehicle, "After right curve") + + projectairsim_log().info("Left curve") + set_controls(vehicle, throttle=0.35, steering=-0.75) + await asyncio.sleep(3.0) + await log_kinematics(vehicle, "After left curve") + + projectairsim_log().info("Straighten") + set_controls(vehicle, throttle=0.5) + await asyncio.sleep(2.0) + await log_kinematics(vehicle, "After straighten") + + projectairsim_log().info("Brake") + set_controls(vehicle, brake=1.0) + await asyncio.sleep(2.0) + await log_kinematics(vehicle, "After brake") + + stop_vehicle(vehicle) + + +async def main(): + client = ProjectAirSimClient() + sim_config_path = str(Path(__file__).resolve().parent / "sim_config") + + try: + client.connect() + projectairsim_log().info("Connected to ProjectAirSim") + + world = World( + client, + "scene_unreal_vehicle.jsonc", + delay_after_load_sec=2, + sim_config_path=sim_config_path, + ) + if world.switch_streaming_view(): + projectairsim_log().info("Switched simulator viewport to vehicle camera") + + vehicle = UnrealVehicle(client, world, "UnrealVehicle") + subscribe_sensors(client, vehicle) + + await run_motion_test(vehicle) + projectairsim_log().info("Done") + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + client.disconnect() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/example_user_scripts/hello_unreal_vehicle_rc.py b/client/python/example_user_scripts/hello_unreal_vehicle_rc.py new file mode 100644 index 00000000..18c93b48 --- /dev/null +++ b/client/python/example_user_scripts/hello_unreal_vehicle_rc.py @@ -0,0 +1,229 @@ +""" +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Project AirSim Unreal vehicle example with arrow-key remote control. + +Loads scene_unreal_vehicle.jsonc and drives the Unreal/Chaos vehicle using +the arrow keys (or WASD). Focus the OpenCV chase-camera window while driving. +Press Q or Esc to stop. +""" + +import asyncio +import time + +import cv2 + +from projectairsim import ProjectAirSimClient, World +from projectairsim.unreal_vehicle import UnrealVehicle +from projectairsim.utils import projectairsim_log, unpack_image + + +def set_controls( + vehicle: UnrealVehicle, + throttle: float = 0.0, + brake: float = 0.0, + steering: float = 0.0, +): + vehicle.set_parameter("throttle", throttle) + vehicle.set_parameter("brake", brake) + vehicle.set_parameter("steering", steering) + + +def stop_vehicle(vehicle: UnrealVehicle): + set_controls(vehicle, throttle=0.0, brake=0.0, steering=0.0) + + +def get_vector(data: dict, *path: str) -> dict: + value = data + for key in path: + if not isinstance(value, dict): + return {} + value = value.get(key, {}) + return value if isinstance(value, dict) else {} + + +async def log_kinematics(vehicle: UnrealVehicle, label: str): + kin = vehicle.get_kinematics() + if not isinstance(kin, dict): + projectairsim_log().info(f"{label}: {kin}") + return + + pos = get_vector(kin, "pose", "position") or get_vector(kin, "position") + vel = get_vector(kin, "twist", "linear") or get_vector(kin, "linear_velocity") + + projectairsim_log().info( + f"{label}: " + f"pos=({pos.get('x', 0):.2f}, {pos.get('y', 0):.2f}, {pos.get('z', 0):.2f}) " + f"vel=({vel.get('x', 0):.2f}, {vel.get('y', 0):.2f}, {vel.get('z', 0):.2f})" + ) + + +class KeyHoldTracker: + """Tracks held keys using repeated key events from OpenCV waitKeyEx.""" + + _ARROW_UP = frozenset({65362, 2490368}) + _ARROW_DOWN = frozenset({65364, 2621440}) + _ARROW_LEFT = frozenset({65361, 2424832}) + _ARROW_RIGHT = frozenset({65363, 2555904}) + _QUIT_KEYS = frozenset({ord("q"), ord("Q"), 27}) + + def __init__(self, hold_timeout_sec: float = 0.2): + self._hold_timeout_sec = hold_timeout_sec + self._active_keys: dict[str, float] = {} + self._quit = False + + def poll(self, key_code: int): + now = time.monotonic() + + if key_code in self._QUIT_KEYS: + self._quit = True + if key_code in self._ARROW_UP or key_code in (ord("w"), ord("W")): + self._active_keys["up"] = now + if key_code in self._ARROW_DOWN or key_code in (ord("s"), ord("S")): + self._active_keys["down"] = now + if key_code in self._ARROW_LEFT or key_code in (ord("a"), ord("A")): + self._active_keys["left"] = now + if key_code in self._ARROW_RIGHT or key_code in (ord("d"), ord("D")): + self._active_keys["right"] = now + + expired = [ + key + for key, last_seen in self._active_keys.items() + if now - last_seen > self._hold_timeout_sec + ] + for key in expired: + del self._active_keys[key] + + def is_pressed(self, key: str) -> bool: + return key in self._active_keys + + def should_quit(self) -> bool: + return self._quit + + +class VehicleRCConfig: + """Tuning values for arrow-key vehicle control.""" + + def __init__( + self, + throttle: float = 0.7, + brake: float = 1.0, + steering: float = 0.75, + poll_interval_sec: float = 0.02, + status_interval_sec: float = 2.0, + key_hold_timeout_sec: float = 0.2, + camera_sensor_id: str = "Chase", + window_name: str = "Unreal Vehicle RC", + ): + self.throttle = throttle + self.brake = brake + self.steering = steering + self.poll_interval_sec = poll_interval_sec + self.status_interval_sec = status_interval_sec + self.key_hold_timeout_sec = key_hold_timeout_sec + self.camera_sensor_id = camera_sensor_id + self.window_name = window_name + + +class ArrowKeyVehicleController: + """Maps arrow keys to throttle, brake, and steering actuator commands.""" + + def __init__(self, vehicle: UnrealVehicle, config: VehicleRCConfig | None = None): + self.vehicle = vehicle + self.config = config or VehicleRCConfig() + self._running = False + + @staticmethod + def print_controls(window_name: str): + print(f"\n--- Arrow-Key Vehicle Control ({window_name} window) ---") + print("Up / W: throttle") + print("Down / S: brake") + print("Left / A: steer left") + print("Right / D: steer right") + print("Q / Esc: quit") + print(f"Keep the '{window_name}' window focused while driving.") + print("------------------------------------------------------\n") + + def _read_controls(self, key_tracker: KeyHoldTracker) -> tuple[float, float, float]: + throttle = 0.0 + brake = 0.0 + steering = 0.0 + + if key_tracker.is_pressed("up"): + throttle = self.config.throttle + elif key_tracker.is_pressed("down"): + brake = self.config.brake + + if key_tracker.is_pressed("left"): + steering = -self.config.steering + elif key_tracker.is_pressed("right"): + steering = self.config.steering + + return throttle, brake, steering + + async def run(self): + self.print_controls(self.config.window_name) + self._running = True + elapsed_since_status = 0.0 + + cur_image = [None] + + def on_image(_, image_msg): + cur_image[0] = image_msg + + chase_topic = self.vehicle.sensors[self.config.camera_sensor_id]["scene_camera"] + self.vehicle.client.subscribe(chase_topic, on_image) + + key_tracker = KeyHoldTracker(hold_timeout_sec=self.config.key_hold_timeout_sec) + + try: + while self._running and not key_tracker.should_quit(): + if cur_image[0] is not None: + cv2.imshow(self.config.window_name, unpack_image(cur_image[0])) + + key_code = cv2.waitKeyEx(1) + key_tracker.poll(key_code) + + throttle, brake, steering = self._read_controls(key_tracker) + set_controls(self.vehicle, throttle, brake, steering) + + elapsed_since_status += self.config.poll_interval_sec + if elapsed_since_status >= self.config.status_interval_sec: + await log_kinematics(self.vehicle, "Driving") + elapsed_since_status = 0.0 + + await asyncio.sleep(self.config.poll_interval_sec) + finally: + self._running = False + stop_vehicle(self.vehicle) + cv2.destroyAllWindows() + + +async def main(): + client = ProjectAirSimClient() + + try: + client.connect() + projectairsim_log().info("Connected to Project AirSim") + + world = World(client, "scene_unreal_vehicle.jsonc", delay_after_load_sec=2) + vehicle = UnrealVehicle(client, world, "UnrealVehicle") + controller = ArrowKeyVehicleController(vehicle) + + await log_kinematics(vehicle, "Initial") + await controller.run() + await log_kinematics(vehicle, "Final") + projectairsim_log().info("Done") + + except KeyboardInterrupt: + projectairsim_log().info("Interrupted") + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + finally: + cv2.destroyAllWindows() + client.disconnect() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/example_user_scripts/hello_unreal_vehicle_simpledrive.py b/client/python/example_user_scripts/hello_unreal_vehicle_simpledrive.py new file mode 100644 index 00000000..5e555383 --- /dev/null +++ b/client/python/example_user_scripts/hello_unreal_vehicle_simpledrive.py @@ -0,0 +1,93 @@ +""" +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. + +Project AirSim Unreal vehicle example driven by SimpleDrive. + +Loads scene_unreal_vehicle_simpledrive.jsonc (unreal-physics vehicle + +simple-drive-api controller), arms the vehicle, and commands a 3-point +MoveOnPath trajectory. +""" + +import asyncio + +from projectairsim import ProjectAirSimClient, Rover, World +from projectairsim.utils import projectairsim_log + + +def get_vector(data: dict, *path: str) -> dict: + value = data + for key in path: + if not isinstance(value, dict): + return {} + value = value.get(key, {}) + return value if isinstance(value, dict) else {} + + +async def log_kinematics(rover: Rover, label: str): + kin = rover.get_ground_truth_kinematics() + if not isinstance(kin, dict): + projectairsim_log().info(f"{label}: {kin}") + return + + pos = get_vector(kin, "pose", "position") or get_vector(kin, "position") + vel = get_vector(kin, "twist", "linear") or get_vector(kin, "linear_velocity") + + projectairsim_log().info( + f"{label}: " + f"pos=({pos.get('x', 0):.2f}, {pos.get('y', 0):.2f}, {pos.get('z', 0):.2f}) " + f"vel=({vel.get('x', 0):.2f}, {vel.get('y', 0):.2f}, {vel.get('z', 0):.2f})" + ) + + +async def main(): + client = ProjectAirSimClient() + + try: + client.connect() + projectairsim_log().info("Connected to Project AirSim") + + world = World( + client, "scene_unreal_vehicle_simpledrive.jsonc", delay_after_load_sec=2 + ) + # Rover client exposes SimpleDrive APIs; physics remains unreal-vehicle. + rover = Rover(client, world, "UnrealVehicle") + + projectairsim_log().info("Enabling API control") + assert rover.enable_api_control() + + projectairsim_log().info("Arming vehicle") + assert rover.arm() + + await log_kinematics(rover, "Initial") + + # 3-point NED path near spawn origin (-500, 0, -4) + path = [[-480.0, 0.0], [-480.0, 20.0], [-460.0, 20.0]] + projectairsim_log().info(f"MoveOnPath with {len(path)} waypoints: {path}") + move_task = await rover.move_on_path_async(path=path, velocity=4.0, timeout_sec=60.0) + await move_task + + await log_kinematics(rover, "After path") + + projectairsim_log().info("Braking") + brake_task = await rover.set_rover_controls( + engine=0.0, steering_angle=0.0, brake=1.0 + ) + await brake_task + await rover.wait_until_stopped_async(timeout_sec=10.0) + + await log_kinematics(rover, "Final") + + rover.disarm() + rover.disable_api_control() + projectairsim_log().info("Done") + + except Exception as err: + projectairsim_log().error(f"Exception occurred: {err}", exc_info=True) + + finally: + client.disconnect() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/client/python/example_user_scripts/sim_config/env_actor_waypoint_marker_cube.jsonc b/client/python/example_user_scripts/sim_config/env_actor_waypoint_marker_cube.jsonc new file mode 100644 index 00000000..67bcc7ab --- /dev/null +++ b/client/python/example_user_scripts/sim_config/env_actor_waypoint_marker_cube.jsonc @@ -0,0 +1,19 @@ +{ + "physics-type": "non-physics", + "links": [ + { + "name": "Marker", + "collision": { + "enabled": false + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Engine/BasicShapes/Cube", + "scale": "1.5 1.5 1.5" + } + } + } + ], + "joints": [] +} diff --git a/client/python/example_user_scripts/sim_config/robot_unreal_vehicle.jsonc b/client/python/example_user_scripts/sim_config/robot_unreal_vehicle.jsonc new file mode 100644 index 00000000..3aeaa427 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_unreal_vehicle.jsonc @@ -0,0 +1,170 @@ +{ + // Example configuration for an unreal vehicle physics robot. + // The dynamic model is fully handled by a UE5 AActor (Blueprint or C++) + // that implements the IProjectAirSimVehicle interface. ProjectAirSim reads + // kinematics from it and forwards actuator signals to it via that interface. + // + // No links, joints, or Project AirSim actuator components need to be defined — + // those are modeled within the Unreal vehicle AActor itself. + + "$schema": "../../projectairsim/src/projectairsim/schema/robot_config_schema.jsonc", + + "physics-type": "unreal-physics", + + // UE5 Blueprint or C++ class path for the unreal vehicle. + // The class MUST implement IProjectAirSimVehicle. + // If omitted, the system will search the world for any actor implementing the interface. + // Examples: + // "/ProjectAirSim/VehicleTemplate/Blueprints/SportsCar/SportsCar_Pawn.SportsCar_Pawn_C" (Plugin Blueprint) + // "/Game/MyFolder/MyVehicle.MyVehicle_C" (Project Blueprint) + // "/Script/MyModule.AMyVehicleActor" (C++ class) + "unreal-vehicle-class": "/ProjectAirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C", + // Controller — use unreal-vehicle-api which exposes SetParameter to the + // Python client. No actuator-order or airframe needed. + "controller": { + "id": "UnrealVehicleController", + "type": "unreal-vehicle-api", + "unreal-vehicle-api-settings": { + // Define the actuators exposed by the unreal vehicle. + // Each entry sets a name and an initial default value. + // The Blueprint's SetActuatorSignal must handle these names. + "actuators": [ + {"name": "throttle", "default-value": 0.0}, + {"name": "brake", "default-value": 0.0}, + {"name": "steering", "default-value": 0.0} + ] + } + }, + + // Sensors attach to the robot root and follow the unreal vehicle's pose. + // The parent-link field can be omitted since there are no links — sensors + // will automatically attach to the root component. + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 640, + "height": 480, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "origin": { + "xyz": "-10.0 1.5 -3.0", + "rpy-deg": "0 -15 0" + } + }, + { + "id": "Chase2", + "type": "camera", + "enabled": true, + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 640, + "height": 480, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "origin": { + "xyz": "-3.0 0 -3.0", + "rpy-deg": "0 -15 0" + } + }, + { + "id": "IMU1", + "type": "imu", + "enabled": true, + "accelerometer": { + "velocity-random-walk": 2.353e-3, + "tau": 800, + "bias-stability": 3.53e-4, + "turn-on-bias": "0 0 0" + }, + "gyroscope": { + "angle-random-walk": 8.72644e-5, + "tau": 500, + "bias-stability": 2.23014e-5, + "turn-on-bias": "0 0 0" + } + }, + { + "id": "lidar1", + "type": "lidar", + "enabled": true, + "number-of-channels": 16, + "range": 100, + "points-per-second": 100000, + "horizontal-rotation-frequency": 10, + "horizontal-fov-start-deg": 0.0, + "horizontal-fov-end-deg": 360.0, + "vertical-fov-upper-deg": 0.0, + "vertical-fov-lower-deg": -30.0, + "draw-debug-points": false, + "origin": { + "xyz": "0.8 0 -1.5", + "rpy-deg": "0 0 0" + } + }, + { + "id": "DistanceSensor", + "type": "distance-sensor", + "enabled": true, + "max-distance": 50.0, + "min-distance": 0.5, + "draw-debug-points": true, + "origin": { + "xyz": "1.5 0 -1.0", + "rpy-deg": "0 0 0" + } + }, + { + "id": "GPS", + "type": "gps", + "enabled": true, + "eph-time-constant": 0.9, + "epv-time-constant": 0.9, + "eph-initial": 100, + "epv-initial": 100, + "eph-final": 0.1, + "epv-final": 0.1, + "eph-min_3d": 3.0, + "eph-min_2d": 4.0 + }, + { + "id": "Barometer", + "type": "barometer", + "enabled": true, + "qnh": 1013.250, + "pressure-factor-sigma": 0.001825, + "pressure-factor-tau": 3600, + "uncorrelated-noise-sigma": 2.7, + "update-latency": 0, + "update-frequency": 50, + "startup-delay": 0 + }, + { + "id": "Magnetometer", + "type": "magnetometer", + "enabled": true, + "scale-factor": 1, + "noise-sigma": "0.005 0.005 0.005", + "noise-bias": "0.0 0.0 0.0" + } + ] +} diff --git a/client/python/example_user_scripts/sim_config/robot_unreal_vehicle_simpledrive.jsonc b/client/python/example_user_scripts/sim_config/robot_unreal_vehicle_simpledrive.jsonc new file mode 100644 index 00000000..d5356889 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/robot_unreal_vehicle_simpledrive.jsonc @@ -0,0 +1,48 @@ +{ + // Unreal vehicle (Chaos physics in UE) controlled by SimpleDrive. + // ProjectAirSim mirrors kinematics and applies SimpleDrive throttle / + // steering / brake outputs to the UE vehicle each tick. + + "$schema": "../../projectairsim/src/projectairsim/schema/robot_config_schema.jsonc", + + "physics-type": "unreal-physics", + + // UE5 Blueprint or C++ class path for the unreal vehicle. + "unreal-vehicle-class": "/ProjectAirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C", + + "controller": { + "id": "Simple_Drive_Controller", + "vehicle-setup": "ackermann", + "type": "simple-drive-api", + "simple-drive-api-settings": { + // No Project AirSim wheel actuators — UE owns the dynamics. + "actuator-order": [] + } + }, + + "sensors": [ + { + "id": "Chase", + "type": "camera", + "enabled": true, + "capture-interval": 0.03, + "capture-settings": [ + { + "image-type": 0, + "width": 640, + "height": 480, + "fov-degrees": 90, + "capture-enabled": true, + "streaming-enabled": true, + "pixels-as-float": false, + "compress": false, + "target-gamma": 2.5 + } + ], + "origin": { + "xyz": "-10.0 1.5 -3.0", + "rpy-deg": "0 -15 0" + } + } + ] +} diff --git a/client/python/example_user_scripts/sim_config/scene_unreal_vehicle.jsonc b/client/python/example_user_scripts/sim_config/scene_unreal_vehicle.jsonc new file mode 100644 index 00000000..9fa59876 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/scene_unreal_vehicle.jsonc @@ -0,0 +1,31 @@ +{ + "id": "SceneUnrealVehicleCar", + "actors": [ + { + "type": "robot", + "name": "UnrealVehicle", + "origin": { + "xyz": "-500.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "robot_unreal_vehicle.jsonc" + } + ], + "clock": { + "type": "engine-driven", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 122.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" + } diff --git a/client/python/example_user_scripts/sim_config/scene_unreal_vehicle_simpledrive.jsonc b/client/python/example_user_scripts/sim_config/scene_unreal_vehicle_simpledrive.jsonc new file mode 100644 index 00000000..f81068f4 --- /dev/null +++ b/client/python/example_user_scripts/sim_config/scene_unreal_vehicle_simpledrive.jsonc @@ -0,0 +1,60 @@ +{ + "id": "SceneUnrealVehicleSimpleDrive", + "actors": [ + { + "type": "robot", + "name": "UnrealVehicle", + "origin": { + "xyz": "-500.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "robot_unreal_vehicle_simpledrive.jsonc" + } + ], + "environment-actors": [ + { + "type": "env_actor", + "name": "MoveOnPathWaypoint1Marker", + "origin": { + "xyz": "-485.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + }, + { + "type": "env_actor", + "name": "MoveOnPathWaypoint2Marker", + "origin": { + "xyz": "-450.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + }, + { + "type": "env_actor", + "name": "MoveOnPathWaypoint3Marker", + "origin": { + "xyz": "-380.0 20.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + } + ], + "clock": { + "type": "engine-driven", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 122.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" + } diff --git a/client/python/projectairsim/src/projectairsim/__init__.py b/client/python/projectairsim/src/projectairsim/__init__.py index 0e1cc0d0..146d0230 100644 --- a/client/python/projectairsim/src/projectairsim/__init__.py +++ b/client/python/projectairsim/src/projectairsim/__init__.py @@ -10,5 +10,6 @@ from .rover import Rover from .env_actor import EnvActor from .static_sensor_actor import StaticSensorActor +from .unreal_vehicle import UnrealVehicle -__all__ = ["Drone", "ProjectAirSimClient", "World", "Rover","EnvActor"] +__all__ = ["Drone", "ProjectAirSimClient", "World", "Rover", "EnvActor", "UnrealVehicle"] diff --git a/client/python/projectairsim/src/projectairsim/client.py b/client/python/projectairsim/src/projectairsim/client.py index 89f537ff..0d380986 100644 --- a/client/python/projectairsim/src/projectairsim/client.py +++ b/client/python/projectairsim/src/projectairsim/client.py @@ -381,14 +381,14 @@ def postprocess_response(self, response): async def postprocess_response_async_with_callback( self, response, callback: callable - ) -> None: + ) -> Any: """Helper function that for asynchronously postprocessing a response""" if inspect.isawaitable(response): response = await response result = self.postprocess_response(response) - # Return or validate result here if callback is not None: callback(result) + return result def disconnect(self): """Disconnects from the server""" diff --git a/client/python/projectairsim/src/projectairsim/robot.py b/client/python/projectairsim/src/projectairsim/robot.py new file mode 100644 index 00000000..b54b3b94 --- /dev/null +++ b/client/python/projectairsim/src/projectairsim/robot.py @@ -0,0 +1,118 @@ +""" +Copyright (C) 2025 IAMAI CONSULTING CORP +MIT License. +Shared base class for ProjectAirSim robot clients. +""" + +from typing import Dict + + +class Robot(object): + def __init__(self, client, world, name: str): + """Base robot client with common topic/sensor/kinematics APIs. + + Args: + client: ProjectAirSim client object + world: ProjectAirSim world object + name (str): Name of the robot actor in the scene + """ + self.client = client + self.world = world + self.name = name + self.world_parent_topic = world.parent_topic + self.home_geo_point = getattr(world, "home_geo_point", None) + self.set_topics(world) + + def set_topics(self, world): + """Sets up common topics for the robot. Called automatically.""" + self.parent_topic = f"{self.world_parent_topic}/robots/{self.name}" + self.sensors_topic = f"{self.parent_topic}/sensors" + self.set_sensor_topics(world) + self.set_robot_info_topics() + + def set_sensor_topics(self, world): + """Build sensor topic map from scene config.""" + self.sensors = {} + scene_config_data = world.get_configuration() + data = None + + for actor in scene_config_data["actors"]: + if actor["name"] == self.name: + data = actor["robot-config"] + + if data is None: + raise Exception("Actor " + self.name + " not found in the config") + + if "sensors" not in data: + return + + capture_setting_dict = { + 0: "scene_camera", + 1: "depth_planar_camera", + 2: "depth_camera", + 3: "segmentation_camera", + 4: "depth_vis_camera", + 5: "disparity_normalized_camera", + 6: "surface_normals_camera", + } + + for sensor in data["sensors"]: + name = sensor["id"] + sensor_type = sensor["type"] + sensor_root_topic = f"{self.sensors_topic}/{name}" + self.sensors[name] = {} + + if sensor_type == "camera": + sub_cameras = sensor.get("capture-settings", []) + for sub_camera in sub_cameras: + if sub_camera.get("capture-enabled", False): + image_type = capture_setting_dict[sub_camera["image-type"]] + self.sensors[name][image_type] = f"{sensor_root_topic}/{image_type}" + self.sensors[name][ + f"{image_type}_info" + ] = f"{sensor_root_topic}/{image_type}_info" + elif sensor_type == "radar": + self.sensors[name][ + "radar_detections" + ] = f"{sensor_root_topic}/radar_detections" + self.sensors[name]["radar_tracks"] = f"{sensor_root_topic}/radar_tracks" + elif sensor_type == "imu": + self.sensors[name]["imu_kinematics"] = f"{sensor_root_topic}/imu_kinematics" + elif sensor_type == "gps": + self.sensors[name]["gps"] = f"{sensor_root_topic}/gps" + elif sensor_type == "airspeed": + self.sensors[name]["airspeed"] = f"{sensor_root_topic}/airspeed" + elif sensor_type == "barometer": + self.sensors[name]["barometer"] = f"{sensor_root_topic}/barometer" + elif sensor_type == "magnetometer": + self.sensors[name]["magnetometer"] = f"{sensor_root_topic}/magnetometer" + elif sensor_type == "lidar": + self.sensors[name]["lidar"] = f"{sensor_root_topic}/lidar" + elif sensor_type == "distance-sensor": + self.sensors[name]["distance_sensor"] = f"{sensor_root_topic}/distance_sensor" + elif sensor_type == "battery": + self.sensors[name]["battery"] = f"{sensor_root_topic}/battery" + else: + raise Exception( + f"Unknown sensor type '{sensor_type}' found in config " + f"for sensor '{name}'" + ) + + def set_robot_info_topics(self): + """Sets up common robot info topics. Called automatically.""" + self.robot_info = {} + self.robot_info["actual_pose"] = f"{self.parent_topic}/actual_pose" + self.robot_info["collision_info"] = f"{self.parent_topic}/collision_info" + + def get_ground_truth_kinematics(self) -> Dict: + """Get ground truth kinematics. + + Returns: + Dict: the Kinematics + """ + req = { + "method": f"{self.parent_topic}/GetGroundTruthKinematics", + "params": {}, + "version": 1.0, + } + return self.client.request(req) diff --git a/client/python/projectairsim/src/projectairsim/rover.py b/client/python/projectairsim/src/projectairsim/rover.py index 8537c65a..12d4f0b9 100644 --- a/client/python/projectairsim/src/projectairsim/rover.py +++ b/client/python/projectairsim/src/projectairsim/rover.py @@ -10,12 +10,13 @@ import time from projectairsim import ProjectAirSimClient, World +from projectairsim.robot import Robot from projectairsim.utils import projectairsim_log, geo_to_ned_coordinates from typing import List, Dict from projectairsim.types import Pose -class Rover(object): +class Rover(Robot): def __init__(self, client: ProjectAirSimClient, world: World, name: str): """ProjectAirSim Rover Actor Interface @@ -25,11 +26,7 @@ def __init__(self, client: ProjectAirSimClient, world: World, name: str): name (str): Name of the Rover actor in the scene """ projectairsim_log().info(f"Initalizing Rover '{name}'...") - self.client = client - self.world = World - self.name = name - self.world_parent_topic = world.parent_topic - self.set_topics(world) + super().__init__(client, world, name) self.vel_cmd = {"axes_0": 0.0, "axes_1": 0.0, "axes_2": 0.0, "axes_3": 0.0} self.home_geo_point = world.home_geo_point self.axis_mapping = { @@ -199,6 +196,52 @@ async def move_to_position_async( async_task_cr = await self.client.request_async(req, callback) return async_task_cr + async def move_on_path_async( + self, + path, + velocity: float, + timeout_sec: float = 3e38, + yaw_rate_max: float = -1, # Unlimited + lookahead: float = -1.0, + adaptive_lookahead: float = 1.0, + callback: callable = None, + ) -> asyncio.Task: + """Move along a sequence of NED waypoints. Returns immediately. + + Each waypoint uses x/y (north/east). A z coordinate may be present but + is ignored for ground vehicles. SimpleDrive visits waypoints in order + via MoveToPosition. + + Args: + path (List[List[float]]): path points in NED coordinates + velocity (float): desired velocity (m/s) + timeout_sec (float): timeout per segment (seconds) + yaw_rate_max (float): max yaw rate, ignored if < 0 (rad/s) + lookahead (float): lookahead for each segment + adaptive_lookahead (float): adaptive lookahead for each segment + callback (callable): callback on command completion or error + + Returns: + asyncio.Task: An awaitable task wrapping the async coroutine + """ + + params: Dict = { + "path": path, + "velocity": velocity, + "timeout_sec": timeout_sec, + "yaw_rate_max": yaw_rate_max, + "lookahead": lookahead, + "adaptive_lookahead": adaptive_lookahead, + } + req: Dict = { + "method": f"{self.parent_topic}/MoveOnPath", + "params": params, + "version": 1.0, + } + + async_task_cr = await self.client.request_async(req, callback) + return async_task_cr + async def move_by_heading_async( self, heading: float, diff --git a/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc b/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc index 2a887ee1..f6148a58 100644 --- a/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc +++ b/client/python/projectairsim/src/projectairsim/schema/robot_config_schema.jsonc @@ -7,7 +7,11 @@ "physics-type": { "description": "Defines the physics type for the robot", "type": "string", - "enum": [ "fast-physics", "non-physics", "matlab-physics" ] + "enum": [ "fast-physics", "non-physics", "matlab-physics", "unreal-physics" ] + }, + "unreal-vehicle-class": { + "description": "UE5 Blueprint or C++ class path for the unreal vehicle (must implement IProjectAirSimVehicle)", + "type": "string" }, "links": { "description": "Define the links for the robot", @@ -68,7 +72,7 @@ }, "type": { "type": "string", - "enum": [ "simple-flight-api", "simple-drive-api","px4-api", "ardupilot-api", "manual-controller-api", "matlab-controller-api"] + "enum": [ "simple-flight-api", "simple-drive-api","px4-api", "ardupilot-api", "manual-controller-api", "matlab-controller-api", "unreal-vehicle-api"] }, "simple-flight-api-settings": { "type": "object", @@ -104,7 +108,6 @@ "allOf": [ { "$ref": "#/definitions/simple-flight-required" }, //make simple-flight-settings required for simple-flight { "$ref": "#/definitions/simple-drive-required" }, //make simple-drive-settings required for simple-drive - { "$ref": "#/definitions/simple-drive-required" }, //make simple-drive-settings required for simple-drive { "$ref": "#/definitions/px4-required" }, //make px4-settings required for px4 { "$ref": "#/definitions/ardupilot-required" } //make ardupilot-settings required for ardupilot ] @@ -134,7 +137,7 @@ "type": "array", "items": { "type": "object", - "required": [ "id", "type", "enabled", "parent-link" ], + "required": [ "id", "type", "enabled" ], "properties": { "id": { "type": "string" }, "type": { @@ -396,7 +399,7 @@ } } }, - "required": [ "physics-type", "links" ], + "required": [ "physics-type" ], //Standard Definitions used in the Schema "definitions": { diff --git a/client/python/projectairsim/src/projectairsim/unreal_vehicle.py b/client/python/projectairsim/src/projectairsim/unreal_vehicle.py new file mode 100644 index 00000000..e453ef5e --- /dev/null +++ b/client/python/projectairsim/src/projectairsim/unreal_vehicle.py @@ -0,0 +1,76 @@ +# Copyright (C) 2025 IAMAI CONSULTING CORP +# +# MIT License. All rights reserved. +# Python client for ProjectAirSim Unreal vehicles. + +import json + +from projectairsim import ProjectAirSimClient, World +from projectairsim.robot import Robot +from projectairsim.utils import projectairsim_log +from typing import Dict + + +class UnrealVehicle(Robot): + def __init__(self, client: ProjectAirSimClient, world: World, name: str): + """ProjectAirSim Unreal Vehicle Interface. + + This client controls vehicles whose dynamics are modeled by an Unreal + Engine AActor that implements the IUnrealVehicleActor interface. + Actuator signals are forwarded to the AActor and kinematics are read + back from it. + + Args: + client (ProjectAirSimClient): ProjectAirSim client object + world (World): ProjectAirSim world object + name (str): Name of the robot actor in the scene + """ + projectairsim_log().info( + f"Initializing UnrealVehicle '{name}'..." + ) + super().__init__(client, world, name) + projectairsim_log().info( + f"UnrealVehicle '{self.name}' initialized for " + f"World scene '{self.world_parent_topic}'" + ) + + def set_parameter(self, name: str, value: float) -> bool: + """Set a named control parameter on the unreal vehicle. + + Args: + name (str): Name of the parameter + value (float): Parameter value to set + + Returns: + bool: True if the parameter was set successfully + """ + req = { + "method": f"{self.parent_topic}/SetParameter", + "params": {"name": name, "value": value}, + "version": 1.0, + } + result = self.client.request(req) + return result + + def set_actuator(self, name: str, signal: float) -> bool: + """Compatibility alias for set_parameter.""" + return self.set_parameter(name, signal) + + def get_kinematics(self) -> Dict: + """Get the current kinematics of the unreal vehicle. + + Returns: + Dict: Kinematics data containing position, orientation, + linear_velocity, angular_velocity, linear_acceleration, + angular_acceleration + """ + # Use the common Robot API. Some builds do not expose GetKinematics + # as a dedicated service for unreal vehicles. + result = super().get_ground_truth_kinematics() + if isinstance(result, str): + return json.loads(result) + return result + + def set_sensor_topics(self, world: World): + """Build sensor topic map from the Unreal vehicle robot config.""" + super().set_sensor_topics(world) diff --git a/client/python/projectairsim/tests/sim_config/env_actor_waypoint_marker_cube.jsonc b/client/python/projectairsim/tests/sim_config/env_actor_waypoint_marker_cube.jsonc new file mode 100644 index 00000000..67bcc7ab --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/env_actor_waypoint_marker_cube.jsonc @@ -0,0 +1,19 @@ +{ + "physics-type": "non-physics", + "links": [ + { + "name": "Marker", + "collision": { + "enabled": false + }, + "visual": { + "geometry": { + "type": "unreal_mesh", + "name": "/Engine/BasicShapes/Cube", + "scale": "1.5 1.5 1.5" + } + } + } + ], + "joints": [] +} diff --git a/client/python/projectairsim/tests/sim_config/robot_test_unreal_vehicle_simpledrive.jsonc b/client/python/projectairsim/tests/sim_config/robot_test_unreal_vehicle_simpledrive.jsonc new file mode 100644 index 00000000..10fc9146 --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/robot_test_unreal_vehicle_simpledrive.jsonc @@ -0,0 +1,12 @@ +{ + "physics-type": "unreal-physics", + "unreal-vehicle-class": "/ProjectAirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C", + "controller": { + "id": "Simple_Drive_Controller", + "vehicle-setup": "ackermann", + "type": "simple-drive-api", + "simple-drive-api-settings": { + "actuator-order": [] + } + } +} diff --git a/client/python/projectairsim/tests/sim_config/scene_test_unreal_vehicle_simpledrive.jsonc b/client/python/projectairsim/tests/sim_config/scene_test_unreal_vehicle_simpledrive.jsonc new file mode 100644 index 00000000..419698bc --- /dev/null +++ b/client/python/projectairsim/tests/sim_config/scene_test_unreal_vehicle_simpledrive.jsonc @@ -0,0 +1,60 @@ +{ + "id": "SceneTestUnrealVehicleSimpleDrive", + "actors": [ + { + "type": "robot", + "name": "UnrealVehicle", + "origin": { + "xyz": "-500.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "robot-config": "robot_test_unreal_vehicle_simpledrive.jsonc" + } + ], + "environment-actors": [ + { + "type": "env_actor", + "name": "MoveOnPathWaypoint1Marker", + "origin": { + "xyz": "-485.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + }, + { + "type": "env_actor", + "name": "MoveOnPathWaypoint2Marker", + "origin": { + "xyz": "-450.0 0.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + }, + { + "type": "env_actor", + "name": "MoveOnPathWaypoint3Marker", + "origin": { + "xyz": "-400.0 20.0 -4.0", + "rpy-deg": "0 0 0" + }, + "env-actor-config": "env_actor_waypoint_marker_cube.jsonc" + } + ], + "clock": { + "type": "steppable", + "step-ns": 3000000, + "real-time-update-rate": 3000000, + "pause-on-start": false + }, + "home-geo-point": { + "latitude": 47.641468, + "longitude": -122.140165, + "altitude": 122.0 + }, + "segmentation": { + "initialize-ids": true, + "ignore-existing": false, + "use-owner-name": true + }, + "scene-type": "UnrealNative" +} diff --git a/core_sim/include/core_sim/actor/robot.hpp b/core_sim/include/core_sim/actor/robot.hpp index 59b4aeb4..4b04e2f1 100644 --- a/core_sim/include/core_sim/actor/robot.hpp +++ b/core_sim/include/core_sim/actor/robot.hpp @@ -66,6 +66,7 @@ class Robot : public Actor { void SetPhysicsConnectionSettings(const std::string& phys_conn_settings); const std::string& GetControlConnectionSettings() const; void SetControlConnectionSettings(const std::string& control_conn_settings); + const std::string& GetUnrealVehicleClass() const; bool GetStartLanded() const; void SetStartLanded(bool start_landed); @@ -90,6 +91,7 @@ class Robot : public Actor { MethodHandler method_handler); void SetController(std::unique_ptr controller); + IController* GetController() const; void PublishRobotPose(const PoseStampedMessage& pose); diff --git a/core_sim/src/actor/robot.cpp b/core_sim/src/actor/robot.cpp index 33eeb12a..41828270 100644 --- a/core_sim/src/actor/robot.cpp +++ b/core_sim/src/actor/robot.cpp @@ -102,6 +102,7 @@ class Robot::Impl : public ActorImpl { bool SetGroundTruthKinematics(const KinematicsMessage& kinematics); void SetController(std::unique_ptr controller); + IController* GetController() const; void OnBeginUpdate() override; @@ -138,6 +139,7 @@ class Robot::Impl : public ActorImpl { void SetPhysicsConnectionSettings(const std::string& phys_conn_settings); const std::string& GetControlConnectionSettings() const; void SetControlConnectionSettings(const std::string& control_conn_settings); + const std::string& GetUnrealVehicleClass() const; bool GetStartLanded() const; void SetStartLanded(bool start_landed); @@ -184,6 +186,7 @@ class Robot::Impl : public ActorImpl { PhysicsType physics_type_; std::string physics_connection_settings_; std::string control_connection_settings_; + std::string unreal_vehicle_class_; bool start_landed_; std::string controller_type_; @@ -333,6 +336,10 @@ void Robot::SetController(std::unique_ptr controller) { static_cast(pimpl_.get())->SetController(std::move(controller)); } +IController* Robot::GetController() const { + return static_cast(pimpl_.get())->GetController(); +} + void Robot::PublishRobotPose(const PoseStampedMessage& pose) { static_cast(pimpl_.get())->PublishRobotPose(pose); } @@ -394,6 +401,11 @@ void Robot::SetControlConnectionSettings( ->SetControlConnectionSettings(control_conn_settings); } +const std::string& Robot::GetUnrealVehicleClass() const { + return static_cast(pimpl_.get()) + ->GetUnrealVehicleClass(); +} + bool Robot::GetStartLanded() const { return static_cast(pimpl_.get())->GetStartLanded(); } @@ -745,6 +757,10 @@ void Robot::Impl::SetController(std::unique_ptr controller) { controller_ = std::move(controller); } +IController* Robot::Impl::GetController() const { + return controller_.get(); +} + Link* Robot::Impl::GetLink(const std::string& id) { for (auto& link : links_) { if (link.GetID() == id) return (&link); @@ -982,6 +998,10 @@ void Robot::Impl::SetControlConnectionSettings( control_connection_settings_ = control_conn_settings; } +const std::string& Robot::Impl::GetUnrealVehicleClass() const { + return unreal_vehicle_class_; +} + bool Robot::Impl::GetStartLanded() const { return start_landed_; } void Robot::Impl::SetStartLanded(bool start_landed) { @@ -1284,6 +1304,14 @@ void Robot::Loader::LoadLinks(const json& json) { auto links_json = JsonUtils::GetArray(json, Constant::Config::links); if (JsonUtils::IsEmptyArray(links_json)) { + if (impl_.physics_type_ == PhysicsType::kUnrealPhysics && + !impl_.unreal_vehicle_class_.empty()) { + impl_.logger_.LogWarning( + impl_.name_, + "[%s] 'links' missing or empty. Continuing for unreal vehicle physics.", + impl_.id_.c_str()); + return; + } impl_.logger_.LogError(impl_.name_, "[%s] 'links' missing or empty.", impl_.id_.c_str()); throw Error("Robot must have at least one link."); @@ -1405,6 +1433,8 @@ void Robot::Loader::LoadPhysicsType(const json& json) { impl_.physics_type_ = PhysicsType::kMatlabPhysics; } else if (physics_type == Constant::Config::unreal_physics) { impl_.physics_type_ = PhysicsType::kUnrealPhysics; + impl_.unreal_vehicle_class_ = JsonUtils::GetString( + json, Constant::Config::unreal_vehicle_class, ""); } else { impl_.physics_type_ = PhysicsType::kNonPhysics; impl_.logger_.LogWarning( diff --git a/core_sim/src/constant.hpp b/core_sim/src/constant.hpp index aa51e31a..520d3001 100644 --- a/core_sim/src/constant.hpp +++ b/core_sim/src/constant.hpp @@ -390,6 +390,7 @@ class Constant { static constexpr const char* control_connection = "control-connection"; static constexpr const char* start_landed = "start-landed"; static constexpr const char* unreal_physics = "unreal-physics"; + static constexpr const char* unreal_vehicle_class = "unreal-vehicle-class"; static constexpr const char* restitution = "restitution"; static constexpr const char* friction = "friction"; static constexpr const char* body_box_xyz = "body-box-xyz"; diff --git a/docs/images/unreal_vehicle/begin-play-setup.png b/docs/images/unreal_vehicle/begin-play-setup.png new file mode 100644 index 00000000..ea58a8bd Binary files /dev/null and b/docs/images/unreal_vehicle/begin-play-setup.png differ diff --git a/docs/images/unreal_vehicle/implemented-interface.png b/docs/images/unreal_vehicle/implemented-interface.png new file mode 100644 index 00000000..ec7b1247 Binary files /dev/null and b/docs/images/unreal_vehicle/implemented-interface.png differ diff --git a/docs/images/unreal_vehicle/set-actuator-signal.png b/docs/images/unreal_vehicle/set-actuator-signal.png new file mode 100644 index 00000000..84dbac33 Binary files /dev/null and b/docs/images/unreal_vehicle/set-actuator-signal.png differ diff --git a/docs/index.md b/docs/index.md index b74aed9a..4de64ba6 100644 --- a/docs/index.md +++ b/docs/index.md @@ -28,6 +28,7 @@ development/dev_setup_linux.md development/dev_setup_win.md development/vscode_user_settings.md development/headless_cloud.md +unreal_vehicle_blueprint_setup.md ``` ```{toctree} diff --git a/docs/unreal_vehicle_blueprint_setup.md b/docs/unreal_vehicle_blueprint_setup.md new file mode 100644 index 00000000..c774533d --- /dev/null +++ b/docs/unreal_vehicle_blueprint_setup.md @@ -0,0 +1,286 @@ +# Unreal Vehicle example + +Project AirSim can use an Unreal Engine vehicle for its dynamics while exposing +named control parameters to Python. The included example uses the Chaos-based SUV +from [`iamaisim/ProjectAirSim-GM`](https://github.com/iamaisim/ProjectAirSim-GM) +with Project AirSim's `unreal-vehicle-api` controller. + +The user-facing API is `UnrealVehicle.set_parameter(name, value)`. The +`SetActuatorSignal` Blueprint event is the internal endpoint used by the Unreal +bridge; it is not called directly from Python. + +## Included files + +The repository already contains everything needed by the example: + +| Purpose | Path | +| --- | --- | +| Unreal SUV Blueprint and assets | `unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/` | +| Robot configuration | `client/python/example_user_scripts/sim_config/robot_unreal_vehicle.jsonc` | +| Scene configuration | `client/python/example_user_scripts/sim_config/scene_unreal_vehicle.jsonc` | +| Runnable Python example | `client/python/example_user_scripts/hello_unreal_vehicle.py` | + +The robot configuration selects the included Blueprint class: + +```jsonc +{ + "physics-type": "unreal-physics", + "unreal-vehicle-class": "/ProjectAirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C", + "controller": { + "id": "UnrealVehicleController", + "type": "unreal-vehicle-api", + "unreal-vehicle-api-settings": { + "actuators": [ + {"name": "throttle", "default-value": 0.0}, + {"name": "brake", "default-value": 0.0}, + {"name": "steering", "default-value": 0.0} + ] + } + } +} +``` + +The actuator names are the public parameter names accepted by +`UnrealVehicle.set_parameter`. Their array order maps internally to throttle, +brake, and steering Blueprint signals. + +## Run the example + +### 1. Build and start Blocks + +Build Project AirSim and launch the Blocks Unreal project using the normal +[source build instructions](development/use_source.md). Start Play in the +Unreal Editor, or launch a built Blocks executable, and leave it running while +the Python client connects. + +The included SUV assets must be present under the ProjectAirSim plugin content +directory shown above. They are already included in this repository. + +### 2. Install the Python client + +Follow the [Python client setup](client_setup.md) and activate its environment. + +### 3. Run the client script + +From the repository root: + +```console +cd client/python/example_user_scripts +python hello_unreal_vehicle.py +``` + +The script loads `scene_unreal_vehicle.jsonc`, sends named throttle, brake, and +steering parameters, and prints kinematics while the SUV moves. + +The essential Python API is: + +```python +from projectairsim import ProjectAirSimClient, World +from projectairsim.unreal_vehicle import UnrealVehicle + +client = ProjectAirSimClient() +client.connect() + +world = World( + client, + "scene_unreal_vehicle.jsonc", + delay_after_load_sec=2, +) +vehicle = UnrealVehicle(client, world, "UnrealVehicle") + +vehicle.set_parameter("throttle", 0.5) +vehicle.set_parameter("brake", 0.0) +vehicle.set_parameter("steering", 0.2) +``` + +Kinematics are available through `vehicle.get_kinematics()`. The +`set_actuator()` method remains only as a compatibility alias for +`set_parameter()` and should not be used in new examples. + +## How control reaches Unreal Engine + +```text +Python UnrealVehicle.set_parameter(name, value) + -> SetParameter service + -> unreal-vehicle-api controller + -> ordered throttle / brake / steering signals + -> Project AirSim Unreal bridge + -> Blueprint SetActuatorSignal(Index, Signal) + -> Chaos vehicle movement component +``` + +This distinction is important: `SetParameter` is the public named API; +`SetActuatorSignal` is the internal index-based Blueprint interface. + +## Blueprint setup for a custom vehicle + +The supplied GM SUV is ready to run without Blueprint changes. Follow this +section when integrating a different Chaos vehicle or an actor with custom +Unreal physics. + +### Choose a Blueprint base + +There are two supported approaches: + +1. **Existing Chaos Pawn:** keep its current parent class and add the + `ProjectAirSimVehicle` interface under **Class Settings > Implemented + Interfaces**. +2. **New force-driven actor:** create a Blueprint derived from **Project AirSim + Vehicle Actor Base**. This class implements the interface, kinematic getters, + signal storage, and a basic force response. + +For a Chaos vehicle, keeping its vehicle Pawn parent is normally the right +choice because its movement component already owns engine, wheel, suspension, +steering, and brake behavior. + +### Prepare a Chaos vehicle Pawn + +The Blueprint needs a configured `ChaosWheeledVehicleMovementComponent`, wheel +setups, skeletal mesh, physics asset, and collision. Verify that the vehicle can +move normally inside Unreal before connecting it to Project AirSim. + +When a runtime-spawned vehicle requires explicit activation, add this setup to +**Event BeginPlay**: + +1. Get the vehicle movement component. +2. Cast it to `ChaosWheeledVehicleMovementComponent`. +3. Store the result in a Blueprint variable for the control events. +4. Call **Activate** on the movement component. +5. Call **Spawn Default Controller** if the Pawn requires possession. + +![BeginPlay setup that activates the Chaos vehicle movement component and spawns the default controller](images/unreal_vehicle/begin-play-setup.png) + +Whether `Spawn Default Controller` is required depends on the Pawn. Do not add +it if the vehicle deliberately uses another possession or controller setup. + +### Add the ProjectAirSimVehicle interface + +For an existing Pawn: + +1. Open the Blueprint and select **Class Settings**. +2. Find **Interfaces** in the Details panel. +3. Add **ProjectAirSimVehicle**. +4. Compile and save the Blueprint. + +![Project Air Sim Vehicle listed under Implemented Interfaces](images/unreal_vehicle/implemented-interface.png) + +The current C++ interface is `IProjectAirSimVehicle`. Older documentation called +it `IUnrealVehicleActor`; that name is obsolete. + +The interface exposes: + +| Function | Purpose | +| --- | --- | +| `SetActuatorSignal(Index, Signal)` | Receive mapped throttle, brake, and steering values | +| `GetActuatorSignal(Name)` | Optional custom signal lookup | +| `GetPosition` / `GetRotation` | Override pose reporting when actor transform is insufficient | +| `GetLinearVelocity` / `GetAngularVelocity` | Override velocity reporting | +| `GetLinearAcceleration` / `GetAngularAcceleration` | Optional acceleration reporting | +| `ResetToSpawnPose` | Restore custom vehicle state during simulation reset | + +Project AirSim can derive common kinematics from the actor and its physics +component. Override the getters only when the vehicle stores or calculates them +differently. + +### Map controller signals to Chaos + +`SetActuatorSignal` is not called from Python. It is the final Blueprint-facing +part of the internal controller bridge. Its index order is fixed for Unreal +vehicles: + +| Index | Signal | Typical Chaos node | +| ---: | --- | --- | +| `0` | Throttle | `Set Throttle Input` | +| `1` | Brake | `Set Brake Input` | +| `2` | Steering | `Set Steering Input` | + +Implement the event with a **Switch on Int**, then forward `Signal` to the +corresponding function on the stored Chaos movement component. + +![SetActuatorSignal Blueprint mapping actuator indices to throttle, brake, and steering](images/unreal_vehicle/set-actuator-signal.png) + +Typical Blueprint flow: + +```text +Event SetActuatorSignal(Index, Signal) + -> Switch on Int + 0 -> Set Throttle Input(Signal) + 1 -> Set Brake Input(Signal) + 2 -> Set Steering Input(Signal) +``` + +This mapping is an implementation detail of the Blueprint. Python continues to +use named calls such as `vehicle.set_parameter("throttle", 0.5)`. + +If the Blueprint derives from `ProjectAirSimVehicleActorBase`, the base class +stores these three signals and can apply a basic force/torque response. Set +**Apply Default Actuator Forces** to false before implementing a different +force model, otherwise both responses may act on the vehicle. + +### Configure the custom Blueprint + +Point `unreal-vehicle-class` at the generated Blueprint class and configure the +named Unreal Vehicle controller parameters: + +```jsonc +{ + "physics-type": "unreal-physics", + "unreal-vehicle-class": "/Game/Vehicles/BP_MyVehicle.BP_MyVehicle_C", + "controller": { + "id": "UnrealVehicleController", + "type": "unreal-vehicle-api", + "unreal-vehicle-api-settings": { + "actuators": [ + {"name": "throttle", "default-value": 0.0}, + {"name": "brake", "default-value": 0.0}, + {"name": "steering", "default-value": 0.0} + ] + } + }, + "sensors": [] +} +``` + +Blueprint class paths end in `_C`. Content inside the ProjectAirSim plugin uses +the `/ProjectAirSim/...` mount point; project content normally uses `/Game/...`. + +Add this robot configuration to a scene and control the actor with +`UnrealVehicle`, just like the supplied SUV example. + +## Optional SimpleDrive controller + +GM also includes `hello_unreal_vehicle_simpledrive.py` and the corresponding +`robot_unreal_vehicle_simpledrive.jsonc` and +`scene_unreal_vehicle_simpledrive.jsonc` files. That alternative exposes the +standard `Rover` API, including `set_rover_controls` and `move_on_path_async`. +It does not replace the named `SetParameter` example documented above. + +## Troubleshooting + +### The vehicle does not spawn + +- Confirm the class path is + `/ProjectAirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C`. +- Confirm the SUV assets are present in the ProjectAirSim plugin. +- Check `projectairsim_server.log` for class-loading errors. + +### The script cannot find the scene configuration + +Run it from `client/python/example_user_scripts`, as shown above, so the default +simulation configuration directory resolves to its `sim_config` subdirectory. + +### The vehicle spawns but does not move + +- Confirm Python uses `UnrealVehicle.set_parameter()` with `throttle`, `brake`, + and `steering`. +- Confirm the robot controller type is `unreal-vehicle-api`. +- Confirm the Blueprint implements `ProjectAirSimVehicle` and maps indices 0, + 1, and 2 to its Chaos movement component. +- Confirm the Unreal vehicle movement component is active and the wheels have + valid Chaos configurations. + +### The repository was downloaded without the SUV asset + +The Blueprint depends on the complete `VehicleAdv/SUV` content directory. A Git +LFS pointer or a partial asset download is not sufficient; obtain the full +example content before opening the Unreal project. diff --git a/physics/src/physics_world.cpp b/physics/src/physics_world.cpp index b0389c36..f7857c4b 100644 --- a/physics/src/physics_world.cpp +++ b/physics/src/physics_world.cpp @@ -44,6 +44,12 @@ void PhysicsWorld::AddRobot(const Robot& robot) { std::in_place_type); } } else if (robot.GetPhysicsType() == PhysicsType::kUnrealPhysics) { + if (!robot.GetUnrealVehicleClass().empty()) { + // Unreal vehicle actors own their Chaos physics in UE. ProjectAirSim only + // mirrors kinematics and sensor state, so no core sim body is needed. + return; + } + // Make an UnrealPhysicsBody to aggregate actuator outputs and pass wrench // to UnrealRobot to be applied at Unreal's next physics step auto unreal_physics_body = std::make_shared(robot); diff --git a/simserver/src/CMakeLists.txt b/simserver/src/CMakeLists.txt index 940392ed..8c37177f 100644 --- a/simserver/src/CMakeLists.txt +++ b/simserver/src/CMakeLists.txt @@ -36,6 +36,7 @@ add_dependencies( physics multirotor_api rover_api + generic_vehicle_api rendering_scene ) @@ -66,6 +67,7 @@ if(WIN32) mavlinkcom multirotor_api rover_api + generic_vehicle_api rendering_scene nng ws2_32 # req by nng on Win @@ -82,6 +84,7 @@ else() mavlinkcom multirotor_api rover_api + generic_vehicle_api rendering_scene nng ) diff --git a/simserver/src/simserver.cpp b/simserver/src/simserver.cpp index 08f7ccff..76536fab 100644 --- a/simserver/src/simserver.cpp +++ b/simserver/src/simserver.cpp @@ -11,6 +11,7 @@ #include "arducopter_api.hpp" #include "core_sim/simulator.hpp" +#include "unreal_vehicle_api.hpp" #include "manual_controller_api.hpp" #include "matlab_controller_api.hpp" #include "mavlink_api.hpp" @@ -310,6 +311,10 @@ void SimServer::LoadControllers(Scene& scene) { } else if (controller_type == "simple-drive-api") { auto simple_drive_api = new simple_drive::SimpleDriveApi(sim_robot, ptransformtree); sim_robot.SetController(std::unique_ptr(simple_drive_api)); + } else if (controller_type == "unreal-vehicle-api") { + auto unreal_vehicle_api = new UnrealVehicleApi(sim_robot); + sim_robot.SetController( + std::unique_ptr(unreal_vehicle_api)); } else { simulator_->GetLogger().LogWarning("SimServer::LoadControllers", "Unsupported controller type '%s'", diff --git a/unreal/Blocks/Blocks.uproject b/unreal/Blocks/Blocks.uproject index ba3e1b24..c3e40f0d 100644 --- a/unreal/Blocks/Blocks.uproject +++ b/unreal/Blocks/Blocks.uproject @@ -35,10 +35,22 @@ { "Name": "SunPosition", "Enabled": true + }, + { + "Name": "ChaosVehiclesPlugin", + "Enabled": true + }, + { + "Name": "Drone", + "Enabled": true + }, + { + "Name": "Rover", + "Enabled": true } ], "TargetPlatforms": [ "LinuxNoEditor", "WindowsNoEditor" ] -} \ No newline at end of file +} diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/Blueprints/BP_Ball.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/Blueprints/BP_Ball.uasset new file mode 100644 index 00000000..fcf8d97b Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/Blueprints/BP_Ball.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Black.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Black.uasset new file mode 100644 index 00000000..428a5ecc Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Black.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Orange.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Orange.uasset new file mode 100644 index 00000000..203336ed Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Orange.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Red.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Red.uasset new file mode 100644 index 00000000..07f2a815 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/MI_Caliper_Red.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper.uasset new file mode 100644 index 00000000..94c1f279 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper_Flake.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper_Flake.uasset new file mode 100644 index 00000000..c08fb7a3 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CaliperPaint/M_Caliper_Flake.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/MI_CarPaint_blue.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/MI_CarPaint_blue.uasset new file mode 100644 index 00000000..b602c435 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/MI_CarPaint_blue.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/M_Carpaint.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/M_Carpaint.uasset new file mode 100644 index 00000000..30e99145 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/CarPaint/M_Carpaint.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/MI_Fabric_ArchLiner.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/MI_Fabric_ArchLiner.uasset new file mode 100644 index 00000000..69e70151 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/MI_Fabric_ArchLiner.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/M_Fabric_ArchLiner.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/M_Fabric_ArchLiner.uasset new file mode 100644 index 00000000..8fe35708 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Fabric/M_Fabric_ArchLiner.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Functions/MF_Sparkle_random.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Functions/MF_Sparkle_random.uasset new file mode 100644 index 00000000..ff4e1dfa Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Functions/MF_Sparkle_random.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/Headlights.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/Headlights.uasset new file mode 100644 index 00000000..f4c60534 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/Headlights.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/M_Glass.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/M_Glass.uasset new file mode 100644 index 00000000..240fa0db Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Glass/M_Glass.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_02_Black_Dark.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_02_Black_Dark.uasset new file mode 100644 index 00000000..1b4b5be8 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_02_Black_Dark.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_05_Brown.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_05_Brown.uasset new file mode 100644 index 00000000..3223368b Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_05_Brown.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_08_Tan_Sandy.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_08_Tan_Sandy.uasset new file mode 100644 index 00000000..dfe41540 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/MI_Leather_08_Tan_Sandy.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/M_Leather.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/M_Leather.uasset new file mode 100644 index 00000000..53f1cfb6 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Leather/M_Leather.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Anodized_03_Dark.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Anodized_03_Dark.uasset new file mode 100644 index 00000000..d6c8a37f Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Anodized_03_Dark.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_02_Medium.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_02_Medium.uasset new file mode 100644 index 00000000..0488c37f Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_02_Medium.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_Dirt_02_Medium.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_Dirt_02_Medium.uasset new file mode 100644 index 00000000..6cbedbaf Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/MI_Metal_Chrome_Dirt_02_Medium.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/M_Metal.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/M_Metal.uasset new file mode 100644 index 00000000..d0b836cc Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Metal/M_Metal.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/MI_Plastic_Matte_Black.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/MI_Plastic_Matte_Black.uasset new file mode 100644 index 00000000..f673b58c Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/MI_Plastic_Matte_Black.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/M_Plastic_Matte.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/M_Plastic_Matte.uasset new file mode 100644 index 00000000..403e2325 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Plastic/M_Plastic_Matte.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_Orange.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_Orange.uasset new file mode 100644 index 00000000..dc11b171 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_Orange.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_RED.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_RED.uasset new file mode 100644 index 00000000..82706a6e Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/MI_Reflector_RED.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/M_Reflector.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/M_Reflector.uasset new file mode 100644 index 00000000..2daea3e1 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Reflector/M_Reflector.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Rubber/M_TireRubber.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Rubber/M_TireRubber.uasset new file mode 100644 index 00000000..2888d0c9 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Materials/Rubber/M_TireRubber.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_Bump_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_Bump_N.uasset new file mode 100644 index 00000000..8081abe7 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_Bump_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_FlakeMask.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_FlakeMask.uasset new file mode 100644 index 00000000..d11682c5 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_FlakeMask.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_MetalFlakes_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_MetalFlakes_N.uasset new file mode 100644 index 00000000..06923bb0 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/CarPaint/T_MetalFlakes_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_D.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_D.uasset new file mode 100644 index 00000000..31cce41e Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_D.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_N.uasset new file mode 100644 index 00000000..d7cc62d0 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Fabric/T_Fabric_ArchLiner_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_AO.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_AO.uasset new file mode 100644 index 00000000..78949a68 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_AO.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_N.uasset new file mode 100644 index 00000000..2004ffd1 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_R.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_R.uasset new file mode 100644 index 00000000..a76676a0 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Leather_01_R.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_PerforationMask.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_PerforationMask.uasset new file mode 100644 index 00000000..60efca3f Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_PerforationMask.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Perforation_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Perforation_N.uasset new file mode 100644 index 00000000..b2e97ec7 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Leather/T_Perforation_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Metal/T_WheelHub_R.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Metal/T_WheelHub_R.uasset new file mode 100644 index 00000000..70f3b919 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Metal/T_WheelHub_R.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_PlasticDirt.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_PlasticDirt.uasset new file mode 100644 index 00000000..3c6d8db5 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_PlasticDirt.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_ReflectorHexagon_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_ReflectorHexagon_N.uasset new file mode 100644 index 00000000..581d60c2 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Reflector/T_ReflectorHexagon_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireRubber_R.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireRubber_R.uasset new file mode 100644 index 00000000..1d56aba8 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireRubber_R.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireSidewall_EpicVer_Release_N.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireSidewall_EpicVer_Release_N.uasset new file mode 100644 index 00000000..16dd5e00 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/AutomotiveMaterials/Textures/Rubber/T_TireSidewall_EpicVer_Release_N.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SUV.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SUV.uasset new file mode 100644 index 00000000..e234bd63 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SUV.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvAnim.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvAnim.uasset new file mode 100644 index 00000000..9d452f55 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvAnim.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvCarPawn.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvCarPawn.uasset new file mode 100644 index 00000000..dbd3392f Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/SuvCarPawn.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Pa.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Pa.uasset new file mode 100644 index 00000000..013017c1 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Pa.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Skel.uasset b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Skel.uasset new file mode 100644 index 00000000..5f97cce7 Binary files /dev/null and b/unreal/Blocks/Plugins/ProjectAirSim/Content/VehicleAdv/SUV/Suv_Skel.uasset differ diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/ProjectAirSimVehicleActorBase.cpp b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/ProjectAirSimVehicleActorBase.cpp new file mode 100644 index 00000000..ec94fbc4 --- /dev/null +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/ProjectAirSimVehicleActorBase.cpp @@ -0,0 +1,53 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "ProjectAirSimVehicleActorBase.h" + +#include "Components/PrimitiveComponent.h" + +AProjectAirSimVehicleActorBase::AProjectAirSimVehicleActorBase() { + PrimaryActorTick.bCanEverTick = true; +} + +void AProjectAirSimVehicleActorBase::SetActuatorSignal_Implementation(int32 Index, float Signal) { + if (Index < 0) return; + if (Index >= StoredActuatorSignals_.Num()) { + StoredActuatorSignals_.SetNum(Index + 1, false); + } + StoredActuatorSignals_[Index] = Signal; +} + +float AProjectAirSimVehicleActorBase::GetActuatorSignal_Implementation(const FString& Name) { + // Simple index-based lookup by converting name to index if possible. + // Blueprints that need name-based access should override this. + return 0.f; +} + +FVector AProjectAirSimVehicleActorBase::GetPosition_Implementation() { + return GetActorLocation(); +} + +FQuat AProjectAirSimVehicleActorBase::GetRotation_Implementation() { + return GetActorQuat(); +} + +FVector AProjectAirSimVehicleActorBase::GetLinearVelocity_Implementation() { + if (UPrimitiveComponent* Root = + Cast(GetRootComponent())) { + if (Root->IsSimulatingPhysics()) { + return Root->GetPhysicsLinearVelocity(); + } + } + return GetVelocity(); +} + +FVector AProjectAirSimVehicleActorBase::GetAngularVelocity_Implementation() { + if (UPrimitiveComponent* Root = + Cast(GetRootComponent())) { + if (Root->IsSimulatingPhysics()) { + return Root->GetPhysicsAngularVelocityInRadians(); + } + } + return FVector::ZeroVector; +} diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.cpp b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.cpp index 72e6049f..0b5770ad 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.cpp +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.cpp @@ -15,7 +15,10 @@ #include "Camera/CameraComponent.h" #include "Components/StaticMeshComponent.h" #include "Engine/TextureRenderTarget2D.h" +#include "EngineUtils.h" #include "GameFramework/GameUserSettings.h" +#include "ProjectAirSimVehicleActorBase.h" +#include "IProjectAirSimVehicle.h" #include "Misc/ScopeLock.h" #include "ProjectAirSim.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" @@ -27,6 +30,7 @@ #include "core_sim/math_utils.hpp" #include "core_sim/physics_common_types.hpp" #include "core_sim/transforms/transform_utils.hpp" +#include "vehicle_control_signal_mapping.hpp" namespace projectairsim = microsoft::projectairsim; @@ -51,12 +55,19 @@ void AUnrealRobot::Initialize(const projectairsim::Robot& InSimRobot, // Detect which links are roots based on their joint attachments auto RootLinks = GetRootLinks(InSimRobot.GetLinks(), InSimRobot.GetJoints()); - bool bWithUnrealPhysics = (InSimRobot.GetPhysicsType() == - projectairsim::PhysicsType::kUnrealPhysics); + bool bIsProjectAirSimVehicle = + (InSimRobot.GetPhysicsType() == + projectairsim::PhysicsType::kUnrealPhysics) && + !InSimRobot.GetUnrealVehicleClass().empty(); - if (bWithUnrealPhysics) { - // For Unreal-calculated physics, do the updates after Unreal has completed - // the physics tick calculations. + bool bWithUnrealPhysics = + (InSimRobot.GetPhysicsType() == + projectairsim::PhysicsType::kUnrealPhysics) && + !bIsProjectAirSimVehicle; + + if (bWithUnrealPhysics || bIsProjectAirSimVehicle) { + // For Unreal-calculated or ProjectAirSim vehicle physics, do the updates after + // Unreal has completed the physics tick calculations. PrimaryActorTick.TickGroup = TG_PostPhysics; } else { // For -calculated physics, do the updates during Unreal's world @@ -68,6 +79,14 @@ void AUnrealRobot::Initialize(const projectairsim::Robot& InSimRobot, InitializeId(InSimRobot.GetID()); InitializeLinks(InSimRobot.GetLinks(), RootLinks, bWithUnrealPhysics); InitializeJoints(InSimRobot.GetJoints()); + + if (bIsProjectAirSimVehicle) { + // ProjectAirSim vehicle physics: initialize the root component and + // find/spawn the vehicle BEFORE sensors so sensors attach to the correct + // root. + InitializeProjectAirSimVehicle(); + } + InitializeSensors(InSimRobot.GetSensors()); StreamingCameraActiveIdx = 0; @@ -362,6 +381,427 @@ std::set AUnrealRobot::GetRootLinks( return Roots; } +void AUnrealRobot::InitializeProjectAirSimVehicle() { + // Create a minimal invisible root component for sensor attachment + auto* SceneRoot = + NewObject(this, TEXT("ProjectAirSimVehicleRoot")); + SceneRoot->SetMobility(EComponentMobility::Movable); + SceneRoot->RegisterComponent(); + RootComponent = SceneRoot; + RobotRootLink = nullptr; // No physics link for ProjectAirSim vehicle + + UWorld* World = GetWorld(); + if (World == nullptr) return; + + // Compute the spawn transform from the robot's initial kinematics (NED_m). + // Convert NED_m → NEU_cm for Unreal world coordinates. + const auto& InitKin = SimRobot.GetKinematics(); + const FVector SpawnLoc = + UnrealHelpers::ToFVector(projectairsim::TransformUtils::NedToNeuLinear( + projectairsim::TransformUtils::ToCentimeters( + InitKin.pose.position))); + const FRotator SpawnRot = + UnrealHelpers::ToFRotator(InitKin.pose.orientation); + FTransform SpawnTransform(SpawnRot, SpawnLoc); + + FString RobotName = FString(SimRobot.GetID().c_str()); + std::string ProjectAirSimVehicleClassPath = SimRobot.GetUnrealVehicleClass(); + + if (!ProjectAirSimVehicleClassPath.empty()) { + // Spawn or find by class path from config (e.g. Blueprint class path) + FString ClassPath = FString(ProjectAirSimVehicleClassPath.c_str()); + UClass* ActorClass = LoadClass(nullptr, *ClassPath); + if (ActorClass != nullptr) { + // First try to find an existing instance of this class + for (TActorIterator It(World, ActorClass); It; ++It) { + ProjectAirSimVehicleActor = *It; + UnrealLogger::Log( + projectairsim::LogLevel::kTrace, + TEXT("[%s] Found existing ProjectAirSim vehicle of class %s: %s"), + *RobotName, *ClassPath, *ProjectAirSimVehicleActor->GetName()); + break; + } + // If no existing instance, spawn one at the robot's initial position + if (ProjectAirSimVehicleActor == nullptr) { + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = + ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + ProjectAirSimVehicleActor = + World->SpawnActor(ActorClass, SpawnTransform, + SpawnParams); + if (ProjectAirSimVehicleActor != nullptr) { + UnrealLogger::Log( + projectairsim::LogLevel::kTrace, + TEXT("[%s] Spawned ProjectAirSim vehicle of class %s at (%.1f, %.1f, %.1f)"), + *RobotName, *ClassPath, SpawnLoc.X, SpawnLoc.Y, SpawnLoc.Z); + } else { + UnrealLogger::Log( + projectairsim::LogLevel::kError, + TEXT("[%s] Failed to spawn ProjectAirSim vehicle of class %s"), + *RobotName, *ClassPath); + } + } + // Check if the actor implements the extended interface + if (ProjectAirSimVehicleActor != nullptr) { + bProjectAirSimVehicleHasInterface = ProjectAirSimVehicleActor->GetClass()-> + ImplementsInterface(UProjectAirSimVehicle::StaticClass()); + if (!bProjectAirSimVehicleHasInterface) { + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] ProjectAirSim vehicle %s does not implement " + "IProjectAirSimVehicle. Kinematics will use standard UE " + "API (GetVelocity). Actuator forwarding disabled."), + *RobotName, *ProjectAirSimVehicleActor->GetName()); + } + } + } else { + UnrealLogger::Log( + projectairsim::LogLevel::kError, + TEXT("[%s] Could not load ProjectAirSim vehicle class: %s"), + *RobotName, *ClassPath); + } + } else { + UnrealLogger::Log( + projectairsim::LogLevel::kError, + TEXT("[%s] Missing required 'unreal-vehicle-class' in robot config. " + "Automatic actor discovery is disabled."), + *RobotName); + } + + if (ProjectAirSimVehicleActor == nullptr) { + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] No ProjectAirSim vehicle found or spawned. " + "ProjectAirSim vehicle physics will not function."), + *RobotName); + } + + const auto ResetProjectAirSimVehiclePose = + [this, &RobotName, &SpawnLoc, &SpawnRot](UPrimitiveComponent* PhysicsComponent) { + PrevExtPosition = SpawnLoc; + PrevExtQuat = SpawnRot.Quaternion(); + PrevEstLinearVelocity = FVector::ZeroVector; + PrevEstAngularVelocity = FVector::ZeroVector; + bHasPrevExtState = false; + + if (ProjectAirSimVehicleActor == nullptr) return; + + // Always teleport using the config's initial position (source of truth). + ProjectAirSimVehicleActor->SetActorLocationAndRotation( + SpawnLoc, SpawnRot, false, nullptr, ETeleportType::TeleportPhysics); + + if (PhysicsComponent != nullptr) { + PhysicsComponent->SetWorldLocationAndRotation( + SpawnLoc, SpawnRot, false, nullptr, ETeleportType::TeleportPhysics); + + if (PhysicsComponent->IsSimulatingPhysics()) { + PhysicsComponent->SetPhysicsLinearVelocity(FVector::ZeroVector); + PhysicsComponent->SetPhysicsAngularVelocityInRadians(FVector::ZeroVector); + PhysicsComponent->SetAllPhysicsPosition(SpawnLoc); + PhysicsComponent->SetAllPhysicsRotation(SpawnRot.Quaternion()); + } + } + + // Notify the actor so Blueprint subclasses can reset their own state + // (AI variables, throttle values, animation state, etc.). + if (bProjectAirSimVehicleHasInterface) { + IProjectAirSimVehicle::Execute_ResetToSpawnPose(ProjectAirSimVehicleActor); + } + + UnrealLogger::Log( + projectairsim::LogLevel::kTrace, + TEXT("[%s] Reset ProjectAirSim vehicle '%s' to initial pose (%.1f, %.1f, %.1f)"), + *RobotName, *ProjectAirSimVehicleActor->GetName(), SpawnLoc.X, SpawnLoc.Y, + SpawnLoc.Z); + }; + + // Position this actor at the ProjectAirSim vehicle's location so sensors + // start at the right place. We sync every tick in TickProjectAirSimVehicle() + // because UE attachment does not propagate to/from physics-simulated actors. + if (ProjectAirSimVehicleActor != nullptr) { + // Find the first UPrimitiveComponent on the ProjectAirSim vehicle. + // At init time physics may not be active yet, so we accept ANY + // UPrimitiveComponent (preferring one with a body instance). + // We will re-check during tick if needed. + TInlineComponentArray PrimComps; + ProjectAirSimVehicleActor->GetComponents(PrimComps); + + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] ProjectAirSim vehicle has %d primitive components"), + *RobotName, PrimComps.Num()); + + for (UPrimitiveComponent* PC : PrimComps) { + if (PC != nullptr) { + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] Component: %s Class: %s SimPhysics: %s"), + *RobotName, *PC->GetName(), + *PC->GetClass()->GetName(), + PC->IsSimulatingPhysics() ? TEXT("YES") : TEXT("NO")); + // Take the first one we find (prefer one already simulating) + if (ProjectAirSimVehicleComponent == nullptr || PC->IsSimulatingPhysics()) { + ProjectAirSimVehicleComponent = PC; + if (PC->IsSimulatingPhysics()) break; + } + } + } + + if (ProjectAirSimVehicleComponent != nullptr) { + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] Using physics component: %s"), + *RobotName, *ProjectAirSimVehicleComponent->GetName()); + } else { + UnrealLogger::Log( + projectairsim::LogLevel::kError, + TEXT("[%s] No UPrimitiveComponent found on ProjectAirSim vehicle!"), + *RobotName); + } + + ResetProjectAirSimVehiclePose(ProjectAirSimVehicleComponent); + + // Initial position sync + FVector ExtLoc; + FRotator ExtRot; + if (ProjectAirSimVehicleComponent != nullptr) { + ExtLoc = ProjectAirSimVehicleComponent->GetComponentLocation(); + ExtRot = ProjectAirSimVehicleComponent->GetComponentRotation(); + } else { + ExtLoc = ProjectAirSimVehicleActor->GetActorLocation(); + ExtRot = ProjectAirSimVehicleActor->GetActorRotation(); + } + this->SetActorLocationAndRotation(ExtLoc, ExtRot, false, nullptr, + ETeleportType::TeleportPhysics); + } +} + +void AUnrealRobot::TickProjectAirSimVehicle(float DeltaTime) { + if (ProjectAirSimVehicleActor == nullptr) return; + + // Lazy re-check: if we found a component at init but it wasn't simulating + // physics yet, check again now that the game is running. + if (ProjectAirSimVehicleComponent != nullptr && + !ProjectAirSimVehicleComponent->IsSimulatingPhysics()) { + // Search again for a simulating component + TInlineComponentArray PrimComps; + ProjectAirSimVehicleActor->GetComponents(PrimComps); + for (UPrimitiveComponent* PC : PrimComps) { + if (PC != nullptr && PC->IsSimulatingPhysics()) { + ProjectAirSimVehicleComponent = PC; + break; + } + } + } + + // Forward actuator signals to the ProjectAirSim vehicle. + // unreal-vehicle-api: [throttle, brake, steering] via GetOrderedSignals. + // simple-drive-api: [throttle, steering, brake] via GetControlSignals. + auto* Controller = SimRobot.GetController(); + if (Controller != nullptr) { + std::vector Signals; + const std::string& ControllerType = SimRobot.GetControllerType(); + if (ControllerType == projectairsim::kControllerTypeUnrealVehicleApi) { + auto* UnrealVehicleController = + static_cast(Controller); + Signals = UnrealVehicleController->GetOrderedSignals(); + } else { + Signals = Controller->GetControlSignals(""); + } + + float Throttle = 0.f; + float Brake = 0.f; + float Steering = 0.f; + projectairsim::MapVehicleControlSignals(ControllerType, Signals, Throttle, + Brake, Steering); + + // Notify the Blueprint via the interface so custom event graphs can react. + // Blueprint index order is always throttle, brake, steering. + if (bProjectAirSimVehicleHasInterface) { + IProjectAirSimVehicle::Execute_SetActuatorSignal( + ProjectAirSimVehicleActor, 0, Throttle); + IProjectAirSimVehicle::Execute_SetActuatorSignal( + ProjectAirSimVehicleActor, 1, Brake); + IProjectAirSimVehicle::Execute_SetActuatorSignal( + ProjectAirSimVehicleActor, 2, Steering); + } + + // Always apply throttle/brake/steering forces directly from C++. + if (ProjectAirSimVehicleComponent != nullptr && + ProjectAirSimVehicleComponent->IsSimulatingPhysics()) { + if (!FMath::IsNearlyZero(Throttle)) { + ProjectAirSimVehicleComponent->AddForce( + ProjectAirSimVehicleComponent->GetForwardVector() * Throttle * 600.f, + NAME_None, /*bAcceleration=*/true); + } + if (!FMath::IsNearlyZero(Brake)) { + const FVector Velocity = + ProjectAirSimVehicleComponent->GetPhysicsLinearVelocity(); + const FVector PlanarVelocity(Velocity.X, Velocity.Y, 0.f); + if (!PlanarVelocity.IsNearlyZero()) { + ProjectAirSimVehicleComponent->AddForce( + -PlanarVelocity.GetSafeNormal() * Brake * 900.f, NAME_None, + /*bAcceleration=*/true); + } + } + if (!FMath::IsNearlyZero(Steering)) { + ProjectAirSimVehicleComponent->AddTorqueInRadians( + FVector(0.f, 0.f, Steering * FMath::DegreesToRadians(180.f)), + NAME_None, /*bAcceleration=*/true); + } + } + } + + // Read kinematics from the ProjectAirSim vehicle. + // Use the physics component's transform (not GetActorLocation) because in + // many Blueprints the root is a static DefaultSceneRoot while the mesh + // that simulates physics is a child component that moves independently. + bHasUnrealPoseUpdated = true; + + TimeNano DeltaTimeThisTick = UnrealHelpers::DeltaTimeToNanos(DeltaTime); + TimeNano LastSimtime = projectairsim::SimClock::Get()->NowSimNanos(); + UnrealPoseUpdatedTimeStamp = LastSimtime + DeltaTimeThisTick; + + projectairsim::Kinematics NewKin; + + FVector ActorPos; + FQuat ActorQuat; + if (ProjectAirSimVehicleComponent != nullptr && + ProjectAirSimVehicleComponent->IsSimulatingPhysics()) { + // Prefer the physics component directly — the most reliable source when + // the actor root is a non-physics DefaultSceneRoot (common in Blueprints). + // This also handles actors that inherit AProjectAirSimVehicleActorBase correctly + // since their root IS the physics mesh. + ActorPos = ProjectAirSimVehicleComponent->GetComponentLocation(); + ActorQuat = ProjectAirSimVehicleComponent->GetComponentQuat(); + } else if (bProjectAirSimVehicleHasInterface) { + ActorPos = IProjectAirSimVehicle::Execute_GetPosition(ProjectAirSimVehicleActor); + ActorQuat = IProjectAirSimVehicle::Execute_GetRotation(ProjectAirSimVehicleActor); + } else if (ProjectAirSimVehicleComponent != nullptr) { + ActorPos = ProjectAirSimVehicleComponent->GetComponentLocation(); + ActorQuat = ProjectAirSimVehicleComponent->GetComponentQuat(); + } else { + ActorPos = ProjectAirSimVehicleActor->GetActorLocation(); + ActorQuat = ProjectAirSimVehicleActor->GetActorQuat(); + } + + // NEU_cm -> NEU_m -> NED_m + NewKin.pose.position = projectairsim::TransformUtils::NeuToNedLinear( + projectairsim::TransformUtils::ToMeters( + projectairsim::Vector3(ActorPos.X, ActorPos.Y, ActorPos.Z))); + + NewKin.pose.orientation = projectairsim::Quaternion( + ActorQuat.W, ActorQuat.X, ActorQuat.Y, ActorQuat.Z); + + FVector VelLin, VelAng, AccLin, AccAng; + AccLin = FVector::ZeroVector; + AccAng = FVector::ZeroVector; + + // Strategy: read velocity from the best available source. + // 1) If the interface provides non-zero velocity, use it (explicit override). + // 2) Else if physics is simulating, read directly from the engine. + // 3) Else estimate from finite differences. + bool bGotVelocity = false; + + if (bProjectAirSimVehicleHasInterface) { + VelLin = IProjectAirSimVehicle::Execute_GetLinearVelocity(ProjectAirSimVehicleActor); + VelAng = IProjectAirSimVehicle::Execute_GetAngularVelocity(ProjectAirSimVehicleActor); + if (!VelLin.IsNearlyZero() || !VelAng.IsNearlyZero()) { + // Blueprint provided an explicit velocity override + AccLin = IProjectAirSimVehicle::Execute_GetLinearAcceleration(ProjectAirSimVehicleActor); + AccAng = IProjectAirSimVehicle::Execute_GetAngularAcceleration(ProjectAirSimVehicleActor); + bGotVelocity = true; + } + } + + if (!bGotVelocity && ProjectAirSimVehicleComponent != nullptr && + ProjectAirSimVehicleComponent->IsSimulatingPhysics()) { + // Read directly from the physics engine (Chaos / PhysX). + VelLin = ProjectAirSimVehicleComponent->GetPhysicsLinearVelocity(); // cm/s + VelAng = ProjectAirSimVehicleComponent->GetPhysicsAngularVelocityInRadians(); + bGotVelocity = true; + } + + if (!bGotVelocity) { + // Fallback: finite differences for kinematic / Blueprint-driven actors. + if (bHasPrevExtState && DeltaTime > 0.0f) { + VelLin = (ActorPos - PrevExtPosition) / DeltaTime; // cm/s + + // Angular velocity from quaternion finite difference + FQuat DeltaQuat = ActorQuat * PrevExtQuat.Inverse(); + DeltaQuat.Normalize(); + FVector Axis; + float AngleRad; + DeltaQuat.ToAxisAndAngle(Axis, AngleRad); + VelAng = Axis * (AngleRad / DeltaTime); + + AccLin = (VelLin - PrevEstLinearVelocity) / DeltaTime; + AccAng = (VelAng - PrevEstAngularVelocity) / DeltaTime; + } else { + VelLin = FVector::ZeroVector; + VelAng = FVector::ZeroVector; + } + } + + // Store current state for next-tick finite differences + PrevExtPosition = ActorPos; + PrevExtQuat = ActorQuat; + PrevEstLinearVelocity = VelLin; + PrevEstAngularVelocity = VelAng; + bHasPrevExtState = true; + + // Debug: log raw values to diagnose which branch was taken + static int VelDebugCounter = 0; + if (++VelDebugCounter % 60 == 0) { + bool bSimPhys = ProjectAirSimVehicleComponent != nullptr && + ProjectAirSimVehicleComponent->IsSimulatingPhysics(); + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[ExtActorVel] HasInterface=%d PhysComp=%s SimPhys=%d " + "Pos=(%.1f,%.1f,%.1f) VelLin=(%.1f,%.1f,%.1f) dt=%.4f"), + bProjectAirSimVehicleHasInterface ? 1 : 0, + ProjectAirSimVehicleComponent ? *ProjectAirSimVehicleComponent->GetName() : TEXT("NULL"), + bSimPhys ? 1 : 0, + ActorPos.X, ActorPos.Y, ActorPos.Z, + VelLin.X, VelLin.Y, VelLin.Z, + DeltaTime); + } + + // NEU_cm/s -> NEU_m/s -> NED_m/s + NewKin.twist.linear = projectairsim::TransformUtils::NeuToNedLinear( + projectairsim::TransformUtils::ToMeters( + projectairsim::Vector3(VelLin.X, VelLin.Y, VelLin.Z))); + + // NEU -> NED + NewKin.twist.angular = projectairsim::TransformUtils::NeuToNedAngular( + projectairsim::Vector3(VelAng.X, VelAng.Y, VelAng.Z)); + + // If interface provided accelerations, use them; otherwise estimate from deltas + if (bProjectAirSimVehicleHasInterface && !AccLin.IsNearlyZero()) { + NewKin.accels.linear = projectairsim::TransformUtils::NeuToNedLinear( + projectairsim::TransformUtils::ToMeters( + projectairsim::Vector3(AccLin.X, AccLin.Y, AccLin.Z))); + NewKin.accels.angular = projectairsim::TransformUtils::NeuToNedAngular( + projectairsim::Vector3(AccAng.X, AccAng.Y, AccAng.Z)); + } else if (DeltaTime > 0.0f) { + auto DeltaVelLin = NewKin.twist.linear - RobotKinematics.twist.linear; + auto DeltaVelAng = NewKin.twist.angular - RobotKinematics.twist.angular; + NewKin.accels.linear = DeltaVelLin / DeltaTime; + NewKin.accels.angular = DeltaVelAng / DeltaTime; + } + + // Update kinematics — both on the local AUnrealRobot copy AND on the + // sim Robot so the controller's GetKinematics reads fresh data. + SetRobotKinematics(NewKin, UnrealPoseUpdatedTimeStamp); + SimRobot.UpdateKinematics(NewKin, UnrealPoseUpdatedTimeStamp); + + // Move this entire actor (and all attached sensor components) to follow + // the ProjectAirSim vehicle. Standard UE attachment does not work when the + // target actor is physics-simulated, so we teleport every tick. + this->SetActorLocationAndRotation(ActorPos, ActorQuat, false, nullptr, + ETeleportType::TeleportPhysics); +} + void AUnrealRobot::InitializeSensors( const std::vector>& InSensors) { @@ -385,6 +825,31 @@ void AUnrealRobot::InitializeSensors( } if (Parent == nullptr) Parent = GetRootComponent(); + // External-actor robots may have no links/root yet at this point. + // Ensure sensor creation always has a valid UObject outer. + if (Parent == nullptr) { + USceneComponent* AutoRoot = + NewObject(this, TEXT("ProjectAirSimVehicleSensorRoot")); + if (AutoRoot != nullptr) { + AutoRoot->RegisterComponent(); + SetRootComponent(AutoRoot); + Parent = AutoRoot; + UnrealLogger::Log( + projectairsim::LogLevel::kWarning, + TEXT("[%s] Root component was null during sensor init; " + "created ProjectAirSimVehicleSensorRoot."), + *GetName()); + } + } + + if (Parent == nullptr) { + UnrealLogger::Log( + projectairsim::LogLevel::kError, + TEXT("[%s] Failed to create sensor '%hs': null parent component."), + *GetName(), CurSensor.get().GetId().c_str()); + return; + } + std::pair Pair = UnrealSensorFactory::CreateSensor(CurSensor.get(), Parent, UnrealScene); @@ -578,6 +1043,14 @@ void AUnrealRobot::Tick(float DeltaTime) { // Main conditions by physics type if (SimRobot.GetPhysicsType() == projectairsim::PhysicsType::kUnrealPhysics && + !SimRobot.GetUnrealVehicleClass().empty()) { + //------------------------------------------------------------------------- + // ProjectAirSimVehicle + + // Read kinematics from the ProjectAirSim vehicle and forward actuator signals + TickProjectAirSimVehicle(DeltaTime); + } else if (SimRobot.GetPhysicsType() == + projectairsim::PhysicsType::kUnrealPhysics && SimPhysicsBody != nullptr) { //------------------------------------------------------------------------- // UnrealPhysics @@ -696,5 +1169,14 @@ void AUnrealRobot::CalcCamera(float DeltaTime, FMinimalViewInfo& OutResult) { } void AUnrealRobot::EndPlay(const EEndPlayReason::Type EndPlayReason) { + // Destroy the ProjectAirSim vehicle actor so it doesn't persist across scene + // reloads. This covers both actors that were found in the world and those + // that were spawned by InitializeProjectAirSimVehicle(). + if (ProjectAirSimVehicleActor != nullptr && IsValid(ProjectAirSimVehicleActor)) { + ProjectAirSimVehicleActor->Destroy(); + ProjectAirSimVehicleActor = nullptr; + } + ProjectAirSimVehicleComponent = nullptr; + Super::EndPlay(EndPlayReason); } diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.h b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.h index 61db5c03..07a643c4 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.h +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Private/Robot/UnrealRobot.h @@ -23,6 +23,7 @@ #include "core_sim/physics_common_types.hpp" #include "core_sim/sensors/camera.hpp" #include "core_sim/sensors/sensor.hpp" +#include "unreal_vehicle_api.hpp" #include "unreal_physics.hpp" // comment so that generated.h is always the last include file with clang-format @@ -103,6 +104,10 @@ class AUnrealRobot : public AActor { void SetExternalWrench(microsoft::projectairsim::Wrench InWrench); + void InitializeProjectAirSimVehicle(); + + void TickProjectAirSimVehicle(float DeltaTime); + std::set GetRootLinks( const std::vector& InLinks, const std::vector& InJoints); @@ -130,4 +135,27 @@ class AUnrealRobot : public AActor { TimeNano UnrealPoseUpdatedTimeStamp = 0; TMap RobotActuatedTransforms; + + // For unreal-physics robots with unreal-vehicle-class: reference to the + // ProjectAirSim vehicle AActor that provides its own physics simulation. + UPROPERTY() + AActor* ProjectAirSimVehicleActor = nullptr; + + // Cached pointer to the first physics-simulating UPrimitiveComponent on the + // ProjectAirSim vehicle. In many Blueprints the root is a plain USceneComponent + // (DefaultSceneRoot) while the actual mesh that simulates physics is a child. + UPROPERTY() + UPrimitiveComponent* ProjectAirSimVehicleComponent = nullptr; + + // Whether the ProjectAirSimVehicleActor implements IProjectAirSimVehicle. + // If false, kinematics are read via standard UE API (GetVelocity, etc.) + bool bProjectAirSimVehicleHasInterface = false; + + // Previous-tick state for finite-difference velocity/acceleration estimation + // when the ProjectAirSim vehicle does not use Chaos physics. + FVector PrevExtPosition = FVector::ZeroVector; + FQuat PrevExtQuat = FQuat::Identity; + FVector PrevEstLinearVelocity = FVector::ZeroVector; + FVector PrevEstAngularVelocity = FVector::ZeroVector; + bool bHasPrevExtState = false; }; diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs index a7bb9397..d332fceb 100644 --- a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/ProjectAirSim.Build.cs @@ -48,6 +48,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/rendering_scene/include", PluginDirectory + "/SimLibs/multirotor_api/include", PluginDirectory + "/SimLibs/rover_api/include", + PluginDirectory + "/SimLibs/generic_vehicle_api/include", PluginDirectory + "/SimLibs/mavlinkcom/include", PluginDirectory + "/SimLibs/eigen/include", PluginDirectory + "/SimLibs/assimp/include", @@ -72,6 +73,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/physics/include", PluginDirectory + "/SimLibs/multirotor_api/include", PluginDirectory + "/SimLibs/rover_api/include", + PluginDirectory + "/SimLibs/generic_vehicle_api/include", PluginDirectory + "/SimLibs/rendering_scene/include", PluginDirectory + "/SimLibs/mavlinkcom/include", PluginDirectory + "/SimLibs/eigen/include", @@ -144,6 +146,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/physics/" + buildType + "/physics.lib", PluginDirectory + "/SimLibs/multirotor_api/" + buildType + "/multirotor_api.lib", PluginDirectory + "/SimLibs/rover_api/" + buildType + "/rover_api.lib", + PluginDirectory + "/SimLibs/generic_vehicle_api/" + buildType + "/generic_vehicle_api.lib", PluginDirectory + "/SimLibs/rendering_scene/" + buildType + "/rendering_scene.lib", PluginDirectory + "/SimLibs/mavlinkcom/" + buildType + "/mavlinkcom.lib", PluginDirectory + "/SimLibs/nng/" + buildType + "/nng.lib", @@ -182,6 +185,7 @@ public ProjectAirSim(ReadOnlyTargetRules Target) : base(Target) PluginDirectory + "/SimLibs/physics/" + buildType + "/libphysics.a", PluginDirectory + "/SimLibs/multirotor_api/" + buildType + "/libmultirotor_api.a", PluginDirectory + "/SimLibs/rover_api/" + buildType + "/librover_api.a", + PluginDirectory + "/SimLibs/generic_vehicle_api/" + buildType + "/libgeneric_vehicle_api.a", PluginDirectory + "/SimLibs/rendering_scene/" + buildType + "/librendering_scene.a", PluginDirectory + "/SimLibs/mavlinkcom/" + buildType + "/libmavlinkcom.a", PluginDirectory + "/SimLibs/nng/" + buildType + "/libnng.a", diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/IProjectAirSimVehicle.h b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/IProjectAirSimVehicle.h new file mode 100644 index 00000000..63244070 --- /dev/null +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/IProjectAirSimVehicle.h @@ -0,0 +1,80 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once + +#include "CoreMinimal.h" +#include "UObject/Interface.h" + +#include "IProjectAirSimVehicle.generated.h" + +UINTERFACE(MinimalAPI, BlueprintType, Blueprintable) +class UProjectAirSimVehicle : public UInterface { + GENERATED_BODY() +}; + +/** + * Interface that any AActor (Blueprint or C++) can implement to receive + * actuator signals from ProjectAirSim. + * + * Kinematics (position, velocity, acceleration) are read automatically + * from the actor's transform and physics — no need to implement anything + * for those. + * + * To use in Blueprint: Class Settings -> Interfaces -> Add "ProjectAirSimVehicle" + * Then implement SetActuatorSignal from the My Blueprint panel. + */ +class PROJECTAIRSIM_API IProjectAirSimVehicle { + GENERATED_BODY() + + public: + /** Called when an actuator signal is sent from the Python client. + * Override in Blueprint to react (e.g. apply throttle, steering, rudder). + * @param Index Actuator index matching the order defined in the robot JSONC + * @param Signal Value typically in [-1, 1] or [0, 1] range */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + void SetActuatorSignal(int32 Index, float Signal); + + /** Get the current value of an actuator signal by name. + * Override only if you need custom storage; default returns 0. */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + float GetActuatorSignal(const FString& Name); + + /** Get the actor's linear velocity in cm/s (Unreal coords). + * Default: returns GetVelocity() of the owning actor. */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FVector GetLinearVelocity(); + + /** Get the actor's angular velocity in rad/s (Unreal coords). + * Default: returns physics angular velocity of root component. */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FVector GetAngularVelocity(); + + /** Get the actor's linear acceleration in cm/s^2 (Unreal coords). + * Default: returns (0,0,0). */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FVector GetLinearAcceleration(); + + /** Get the actor's angular acceleration in rad/s^2 (Unreal coords). + * Default: returns (0,0,0). */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FVector GetAngularAcceleration(); + + /** Get the actor's world position in cm (Unreal coords). + * Default: returns GetActorLocation() of the owning actor. */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FVector GetPosition(); + + /** Get the actor's world rotation as a quaternion (Unreal coords). + * Default: returns GetActorQuat() of the owning actor. */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + FQuat GetRotation(); + + /** Reset the actor to its spawn-time pose (position, rotation, zero velocity). + * Called by ProjectAirSim whenever the simulation is reset/reloaded. + * Default implementation does nothing — AProjectAirSimVehicleActorBase provides + * a full default. Override in Blueprint for custom reset logic (e.g. AI state). */ + UFUNCTION(BlueprintNativeEvent, BlueprintCallable, Category = "ProjectAirSimVehicle") + void ResetToSpawnPose(); +}; diff --git a/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/ProjectAirSimVehicleActorBase.h b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/ProjectAirSimVehicleActorBase.h new file mode 100644 index 00000000..0c211a9a --- /dev/null +++ b/unreal/Blocks/Plugins/ProjectAirSim/Source/ProjectAirSim/Public/ProjectAirSimVehicleActorBase.h @@ -0,0 +1,77 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "IProjectAirSimVehicle.h" + +#include "ProjectAirSimVehicleActorBase.generated.h" + +/** + * Optional base class for ProjectAirSim vehicle actors controlled by ProjectAirSim. + * + * Inherit your Blueprint from this class instead of Actor/Pawn to get free + * default implementations of all kinematic getters and a default force-based + * actuator response. Override SetActuatorSignal in your Blueprint for custom + * behaviour. + * + * Default behaviour: + * - GetPosition() -> GetActorLocation() + * - GetRotation() -> GetActorQuat() + * - GetLinearVelocity() -> root component physics linear velocity (cm/s) + * - GetAngularVelocity() -> root component physics angular velocity (rad/s) + * - GetLinearAcceleration() -> returns (0,0,0) (override if needed) + * - GetAngularAcceleration() -> returns (0,0,0) (override if needed) + * - GetActuatorSignal() -> returns stored value (index-based) + * - SetActuatorSignal() -> stores signal; Tick applies forces when + * bApplyDefaultActuatorForces is true (default): + * Index 0 (throttle) -> forward acceleration + * Index 1 (brake) -> reverse acceleration + * Index 2 (steering) -> yaw torque + */ +UCLASS(Blueprintable, BlueprintType, ClassGroup = "ProjectAirSim", + meta = (DisplayName = "Project AirSim Vehicle Actor Base")) +class PROJECTAIRSIM_API AProjectAirSimVehicleActorBase + : public AActor, + public IProjectAirSimVehicle { + GENERATED_BODY() + + public: + AProjectAirSimVehicleActorBase(); + + // ---- Configurable default force parameters ---- + + /** When true (default), Tick applies forces to the root physics component + * based on the standard throttle/brake/steering actuator signals. + * Set to false in your Blueprint if you want to handle forces yourself. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ProjectAirSimVehicle|Defaults") + bool bApplyDefaultActuatorForces = true; + + /** Acceleration (cm/s²) applied per unit of throttle minus brake. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ProjectAirSimVehicle|Defaults") + float ThrottleAcceleration = 600.0f; + + /** Yaw angular acceleration (deg/s²) applied per unit of steering. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ProjectAirSimVehicle|Defaults") + float SteeringTorqueDegPerSec2 = 180.0f; + + // ---- IProjectAirSimVehicle ---- + + /** Stores the signal for use in Tick. Override in Blueprint for custom logic. */ + virtual void SetActuatorSignal_Implementation(int32 Index, float Signal) override; + + virtual float GetActuatorSignal_Implementation(const FString& Name) override; + + virtual FVector GetPosition_Implementation() override; + virtual FQuat GetRotation_Implementation() override; + virtual FVector GetLinearVelocity_Implementation() override; + virtual FVector GetAngularVelocity_Implementation() override; + virtual FVector GetLinearAcceleration_Implementation() override { return FVector::ZeroVector; } + virtual FVector GetAngularAcceleration_Implementation() override { return FVector::ZeroVector; } + + private: + TArray StoredActuatorSignals_; +}; diff --git a/vehicle_apis/CMakeLists.txt b/vehicle_apis/CMakeLists.txt index d8dbecfd..e8ebea2b 100644 --- a/vehicle_apis/CMakeLists.txt +++ b/vehicle_apis/CMakeLists.txt @@ -17,3 +17,4 @@ add_subdirectory(multirotor_api) add_subdirectory(rover_api) +add_subdirectory(generic_vehicle_api) diff --git a/vehicle_apis/generic_vehicle_api/CMakeLists.txt b/vehicle_apis/generic_vehicle_api/CMakeLists.txt new file mode 100644 index 00000000..07a61214 --- /dev/null +++ b/vehicle_apis/generic_vehicle_api/CMakeLists.txt @@ -0,0 +1,60 @@ +# --------------------------------------------------------------------------------------------------------------------- +# +# Copyright (C) 2025 IAMAI CONSULTING CORP +# +# MIT License. All rights reserved. +# +# Module Name: +# +# CMakeLists.txt +# +# Abstract: +# +# CMake build file for the generic vehicle API library. +# +# --------------------------------------------------------------------------------------------------------------------- + +set(TARGET_NAME generic_vehicle_api) + +add_library( + ${TARGET_NAME} + STATIC + src/unreal_vehicle_api.cpp +) + +set_target_properties(${TARGET_NAME} PROPERTIES + POSITION_INDEPENDENT_CODE ON + COMPILE_PDB_NAME ${TARGET_NAME} + COMPILE_PDB_OUTPUT_DIR ${CMAKE_BINARY_DIR} +) + +add_dependencies(${TARGET_NAME} core_sim) + +target_include_directories( + ${TARGET_NAME} + PUBLIC + $ + $ + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${EIGEN_INCLUDE_DIR} + ${JSON_INCLUDE_DIR} +) + +target_link_libraries( + ${TARGET_NAME} + PRIVATE + core_sim +) + +add_custom_command(TARGET ${TARGET_NAME} + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E echo "Packaging [${TARGET_NAME}] build outputs to ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>" + COMMAND ${CMAKE_COMMAND} -E copy_directory $ ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug> + COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_SOURCE_DIR}/include ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/include + COMMAND ${CMAKE_COMMAND} -E remove_directory ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/CMakeFiles + COMMAND ${CMAKE_COMMAND} -E remove -f ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/cmake_install.cmake + COMMAND ${CMAKE_COMMAND} -E remove -f ${UE_PLUGIN_SIMLIBS_DIR}/${TARGET_NAME}/$,Release,Debug>/CTestTestfile.cmake +) diff --git a/vehicle_apis/generic_vehicle_api/include/unreal_vehicle_api.hpp b/vehicle_apis/generic_vehicle_api/include/unreal_vehicle_api.hpp new file mode 100644 index 00000000..bc7c5308 --- /dev/null +++ b/vehicle_apis/generic_vehicle_api/include/unreal_vehicle_api.hpp @@ -0,0 +1,66 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#ifndef GENERIC_VEHICLE_API_INCLUDE_UNREAL_VEHICLE_API_HPP_ +#define GENERIC_VEHICLE_API_INCLUDE_UNREAL_VEHICLE_API_HPP_ + +#include +#include +#include +#include + +#include "core_sim/actor/robot.hpp" +#include "core_sim/physics_common_types.hpp" +#include "core_sim/runtime_components.hpp" +#include "core_sim/service_method.hpp" + +namespace microsoft { +namespace projectairsim { + +// Controller for unreal vehicle physics. This controller does not compute +// any control signals itself. It exposes SetParameter service methods so +// clients can send named control parameters directly to the Unreal vehicle +// actor that manages its own physics. +class UnrealVehicleApi : public IController { + public: + explicit UnrealVehicleApi(const Robot& robot); + virtual ~UnrealVehicleApi() = default; + + // IController overrides + void BeginUpdate() override; + void EndUpdate() override; + void Reset() override; + void SetKinematics(const Kinematics* kinematics) override; + void Update() override; + std::vector GetControlSignals( + const std::string& actuator_id) override; + const GimbalState& GetGimbalSignal(const std::string& gimbal_id) override; + + // Actuator signal accessors (thread-safe) + void SetActuatorSignal(const std::string& name, float signal); + float GetActuatorSignal(const std::string& name) const; + std::unordered_map GetAllActuatorSignals() const; + std::vector GetOrderedSignals() const; + + protected: + Logger GetLogger() { return sim_robot_.GetLogger(); } + + private: + void RegisterServiceMethods(); + + // Service method handlers + bool SetParameter(const std::string& name, float value); + bool SetActuator(const std::string& name, float signal); + + Robot sim_robot_; + const Kinematics* kinematics_ = nullptr; + mutable std::mutex actuator_mutex_; + std::unordered_map actuator_signals_; + std::vector actuator_order_; +}; + +} // namespace projectairsim +} // namespace microsoft + +#endif // GENERIC_VEHICLE_API_INCLUDE_UNREAL_VEHICLE_API_HPP_ diff --git a/vehicle_apis/generic_vehicle_api/include/vehicle_control_signal_mapping.hpp b/vehicle_apis/generic_vehicle_api/include/vehicle_control_signal_mapping.hpp new file mode 100644 index 00000000..5f1a4755 --- /dev/null +++ b/vehicle_apis/generic_vehicle_api/include/vehicle_control_signal_mapping.hpp @@ -0,0 +1,51 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#ifndef GENERIC_VEHICLE_API_INCLUDE_VEHICLE_CONTROL_SIGNAL_MAPPING_HPP_ +#define GENERIC_VEHICLE_API_INCLUDE_VEHICLE_CONTROL_SIGNAL_MAPPING_HPP_ + +#include +#include + +namespace microsoft { +namespace projectairsim { + +// Controller type string constants used by scene/robot config and SimServer. +inline constexpr const char* kControllerTypeUnrealVehicleApi = + "unreal-vehicle-api"; +inline constexpr const char* kControllerTypeSimpleDriveApi = "simple-drive-api"; + +// Maps controller-native control signals into the Blueprint / force-application +// order used by AUnrealRobot: throttle, brake, steering. +// +// Input layouts: +// unreal-vehicle-api: [throttle, brake, steering] +// simple-drive-api: [throttle, steering, brake] +// +// Missing channels are zero-filled. +inline void MapVehicleControlSignals(const std::string& controller_type, + const std::vector& signals, + float& throttle, float& brake, + float& steering) { + throttle = 0.f; + brake = 0.f; + steering = 0.f; + + if (controller_type == kControllerTypeSimpleDriveApi) { + if (signals.size() > 0) throttle = signals[0]; + if (signals.size() > 1) steering = signals[1]; + if (signals.size() > 2) brake = signals[2]; + return; + } + + // Default / unreal-vehicle-api: [throttle, brake, steering] + if (signals.size() > 0) throttle = signals[0]; + if (signals.size() > 1) brake = signals[1]; + if (signals.size() > 2) steering = signals[2]; +} + +} // namespace projectairsim +} // namespace microsoft + +#endif // GENERIC_VEHICLE_API_INCLUDE_VEHICLE_CONTROL_SIGNAL_MAPPING_HPP_ diff --git a/vehicle_apis/generic_vehicle_api/src/unreal_vehicle_api.cpp b/vehicle_apis/generic_vehicle_api/src/unreal_vehicle_api.cpp new file mode 100644 index 00000000..45a87e46 --- /dev/null +++ b/vehicle_apis/generic_vehicle_api/src/unreal_vehicle_api.cpp @@ -0,0 +1,127 @@ +// Copyright (C) 2025 IAMAI CONSULTING CORP +// +// MIT License. All rights reserved. + +#include "unreal_vehicle_api.hpp" + +#include + +#include "json.hpp" + +namespace microsoft { +namespace projectairsim { + +UnrealVehicleApi::UnrealVehicleApi(const Robot& robot) : sim_robot_(robot) { + // Parse actuators from controller settings JSON + const auto& settings_str = robot.GetControllerSettings(); + if (!settings_str.empty()) { + try { + auto settings_json = nlohmann::json::parse(settings_str); + if (settings_json.contains("unreal-vehicle-api-settings")) { + auto& api_settings = settings_json["unreal-vehicle-api-settings"]; + if (api_settings.contains("actuators") && api_settings["actuators"].is_array()) { + for (const auto& act : api_settings["actuators"]) { + std::string name = act.value("name", ""); + float default_val = act.value("default-value", 0.0f); + if (!name.empty()) { + actuator_signals_[name] = default_val; + actuator_order_.push_back(name); + } + } + } + } + } catch (...) { + // If parsing fails, start with empty actuator map + GetLogger().LogWarning("UnrealVehicleApi", + "Failed to parse actuator settings: %s", + settings_str.c_str()); + } + } +} + +void UnrealVehicleApi::BeginUpdate() { RegisterServiceMethods(); } + +void UnrealVehicleApi::EndUpdate() {} + +void UnrealVehicleApi::Reset() { + std::lock_guard lock(actuator_mutex_); + for (auto& kv : actuator_signals_) kv.second = 0.0f; +} + +void UnrealVehicleApi::SetKinematics(const Kinematics* kinematics) { + kinematics_ = kinematics; +} + +void UnrealVehicleApi::Update() { + // No-op: the unreal vehicle manages its own physics. +} + +std::vector UnrealVehicleApi::GetControlSignals( + const std::string& actuator_id) { + // Unreal vehicle actuators are not managed by ProjectAirSim's actuator system + return {}; +} + +const IController::GimbalState& UnrealVehicleApi::GetGimbalSignal( + const std::string& gimbal_id) { + throw std::runtime_error( + "UnrealVehicleApi does not support gimbal devices."); +} + +void UnrealVehicleApi::SetActuatorSignal(const std::string& name, + float signal) { + std::lock_guard lock(actuator_mutex_); + actuator_signals_[name] = signal; +} + +float UnrealVehicleApi::GetActuatorSignal(const std::string& name) const { + std::lock_guard lock(actuator_mutex_); + auto it = actuator_signals_.find(name); + if (it != actuator_signals_.end()) { + return it->second; + } + return 0.0f; +} + +std::unordered_map +UnrealVehicleApi::GetAllActuatorSignals() const { + std::lock_guard lock(actuator_mutex_); + return actuator_signals_; +} + +std::vector UnrealVehicleApi::GetOrderedSignals() const { + std::lock_guard lock(actuator_mutex_); + std::vector result; + result.reserve(actuator_order_.size()); + for (const auto& name : actuator_order_) { + auto it = actuator_signals_.find(name); + result.push_back(it != actuator_signals_.end() ? it->second : 0.0f); + } + return result; +} + +// Service method handlers +bool UnrealVehicleApi::SetParameter(const std::string& name, float value) { + SetActuatorSignal(name, value); + return true; +} + +bool UnrealVehicleApi::SetActuator(const std::string& name, float signal) { + SetActuatorSignal(name, signal); + return true; +} + +void UnrealVehicleApi::RegisterServiceMethods() { + auto set_parameter = ServiceMethod("SetParameter", {"name", "value"}); + auto set_parameter_handler = + set_parameter.CreateMethodHandler(&UnrealVehicleApi::SetParameter, *this); + sim_robot_.RegisterServiceMethod(set_parameter, set_parameter_handler); + + auto set_actuator = ServiceMethod("SetActuator", {"name", "signal"}); + auto set_actuator_handler = + set_actuator.CreateMethodHandler(&UnrealVehicleApi::SetActuator, *this); + sim_robot_.RegisterServiceMethod(set_actuator, set_actuator_handler); +} + +} // namespace projectairsim +} // namespace microsoft diff --git a/vehicle_apis/rover_api/include/irover_api.hpp b/vehicle_apis/rover_api/include/irover_api.hpp index e5d1219d..df3de40d 100644 --- a/vehicle_apis/rover_api/include/irover_api.hpp +++ b/vehicle_apis/rover_api/include/irover_api.hpp @@ -9,6 +9,7 @@ #include #include +#include #include "core_sim/physics_common_types.hpp" @@ -43,6 +44,12 @@ struct IRoverApi { float duration, float heading_margin, float yaw_rate, float timeout_sec, int64_t command_start_time_nanos) = 0; + + // Follow a sequence of NED waypoints (x/y used; z ignored if present). + virtual bool MoveOnPath(std::vector> path, float velocity, + float timeout_sec, float yaw_rate_max, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos) = 0; }; // interface IRoverApi } // namespace projectairsim diff --git a/vehicle_apis/rover_api/include/rover_api_base.hpp b/vehicle_apis/rover_api/include/rover_api_base.hpp index 8bc33ef2..136bc261 100644 --- a/vehicle_apis/rover_api/include/rover_api_base.hpp +++ b/vehicle_apis/rover_api/include/rover_api_base.hpp @@ -53,6 +53,11 @@ class RoverApiBase : public IController, public IRoverApi { float yaw_rate, float timeout_sec, int64_t command_start_time_nanos) = 0; + virtual bool MoveOnPath(std::vector> path, float velocity, + float timeout_sec, float yaw_rate_max, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos) = 0; + protected: typedef std::function WaitFunction; diff --git a/vehicle_apis/rover_api/include/simple_drive/simple_drive_api.hpp b/vehicle_apis/rover_api/include/simple_drive/simple_drive_api.hpp index fb7a87fc..7313707a 100644 --- a/vehicle_apis/rover_api/include/simple_drive/simple_drive_api.hpp +++ b/vehicle_apis/rover_api/include/simple_drive/simple_drive_api.hpp @@ -60,6 +60,10 @@ class SimpleDriveApi : public AckermannApiBase { float duration, float heading_margin, float yaw_rate, float timeout_sec, int64_t command_start_time_nanos) override; + bool MoveOnPath(std::vector> path, float velocity, + float timeout_sec, float yaw_rate_max, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos) override; //--------------------------------------------------------------------------- // IController Method Overrides diff --git a/vehicle_apis/rover_api/include/simple_drive/vehicle_state_estimator.hpp b/vehicle_apis/rover_api/include/simple_drive/vehicle_state_estimator.hpp index 006471af..df2c9092 100644 --- a/vehicle_apis/rover_api/include/simple_drive/vehicle_state_estimator.hpp +++ b/vehicle_apis/rover_api/include/simple_drive/vehicle_state_estimator.hpp @@ -28,8 +28,9 @@ class VehicleStateEstimator : public vehicle_apis::IStateEstimator { } vehicle_apis::Axis3r GetAngles() const override { + const auto& kinematics = GetKinematics(); vehicle_apis::Axis3r angles; - Vector3 rpy = TransformUtils::ToRPY(kinematics_->pose.orientation); + Vector3 rpy = TransformUtils::ToRPY(kinematics.pose.orientation); angles.Roll() = rpy[0]; angles.Pitch() = rpy[1]; angles.Yaw() = rpy[2]; @@ -41,7 +42,7 @@ class VehicleStateEstimator : public vehicle_apis::IStateEstimator { } vehicle_apis::Axis3r GetAngularVelocity() const override { - const auto& angular = kinematics_->twist.angular; + const auto& angular = GetKinematics().twist.angular; vehicle_apis::Axis3r conv; conv.X() = angular.x(); @@ -52,22 +53,24 @@ class VehicleStateEstimator : public vehicle_apis::IStateEstimator { } vehicle_apis::Axis3r GetPosition() const override { - return Utils::ToAxis3r(kinematics_->pose.position); + return Utils::ToAxis3r(GetKinematics().pose.position); } vehicle_apis::Axis3r TransformVectorToBodyFrame( const vehicle_apis::Axis3r& world_frame_val) const override { + const auto& kinematics = GetKinematics(); const Vector3& vec = Utils::ToVector3(world_frame_val); const Vector3& trans = PhysicsUtils::TransformVectorToBodyFrame( - vec, kinematics_->pose.orientation); + vec, kinematics.pose.orientation); return Utils::ToAxis3r(trans); } vehicle_apis::Axis3r TransformVectorToBodyFromHorizontalPlaneFrame( const vehicle_apis::Axis3r& horizontal_plane_frame_val) const override { + const auto& kinematics = GetKinematics(); const Vector3& vec = Utils::ToVector3(horizontal_plane_frame_val); - auto orientation = kinematics_->pose.orientation; + auto orientation = kinematics.pose.orientation; // quaternion to roll pitch yaw Vector3 rpy = TransformUtils::ToRPY(orientation); // set yaw to zero to correct for current roll and pitch only @@ -102,29 +105,39 @@ class VehicleStateEstimator : public vehicle_apis::IStateEstimator { // } vehicle_apis::Axis3r GetLinearVelocity() const override { - return Utils::ToAxis3r(kinematics_->twist.linear); + return Utils::ToAxis3r(GetKinematics().twist.linear); } vehicle_apis::Axis4r GetOrientation() const override { - return Utils::ToAxis4r(kinematics_->pose.orientation); + return Utils::ToAxis4r(GetKinematics().pose.orientation); } vehicle_apis::KinematicsState GetKinematicsEstimated() const override { + const auto& kinematics = GetKinematics(); vehicle_apis::KinematicsState state; - state.position = GetPosition(); - state.orientation = GetOrientation(); - state.linear_velocity = GetLinearVelocity(); - state.angular_velocity = GetAngularVelocity(); + state.position = Utils::ToAxis3r(kinematics.pose.position); + state.orientation = Utils::ToAxis4r(kinematics.pose.orientation); + state.linear_velocity = Utils::ToAxis3r(kinematics.twist.linear); + state.angular_velocity = Utils::ToAxis3r(kinematics.twist.angular); state.linear_acceleration = - Utils::ToAxis3r(kinematics_->accels.linear); + Utils::ToAxis3r(kinematics.accels.linear); state.angular_acceleration = - Utils::ToAxis3r(kinematics_->accels.angular); + Utils::ToAxis3r(kinematics.accels.angular); return state; } private: - const Kinematics* kinematics_; + const Kinematics& GetKinematics() const { + return kinematics_ == nullptr ? ZeroKinematics() : *kinematics_; + } + + static const Kinematics& ZeroKinematics() { + static const Kinematics zero_kinematics = Kinematics::Zero(); + return zero_kinematics; + } + + const Kinematics* kinematics_ = nullptr; }; // class VehicleEstimator } // namespace simple_drive diff --git a/vehicle_apis/rover_api/src/rover_api_base.cpp b/vehicle_apis/rover_api/src/rover_api_base.cpp index 2850cea5..7f5816cc 100644 --- a/vehicle_apis/rover_api/src/rover_api_base.cpp +++ b/vehicle_apis/rover_api/src/rover_api_base.cpp @@ -111,6 +111,15 @@ void RoverApiBase::RegisterServiceMethods() { method_handler = method.CreateMethodHandler(&RoverApiBase::MoveByHeading, *this); sim_robot_.RegisterServiceMethod(method, method_handler); + + // Register MoveOnPath + method = ServiceMethod( + "MoveOnPath", + {"path", "velocity", "timeout_sec", "yaw_rate_max", "lookahead", + "adaptive_lookahead", "_service_method_start_time"}); + method_handler = + method.CreateMethodHandler(&RoverApiBase::MoveOnPath, *this); + sim_robot_.RegisterServiceMethod(method, method_handler); } } // namespace projectairsim diff --git a/vehicle_apis/rover_api/src/simple_drive/simple_drive_api.cpp b/vehicle_apis/rover_api/src/simple_drive/simple_drive_api.cpp index 250ce8d1..69d10bfd 100644 --- a/vehicle_apis/rover_api/src/simple_drive/simple_drive_api.cpp +++ b/vehicle_apis/rover_api/src/simple_drive/simple_drive_api.cpp @@ -227,6 +227,10 @@ bool SimpleDriveApi::Disarm(void) { bool SimpleDriveApi::CanArm(void) const { return true; } Kinematics SimpleDriveApi::GetKinematicsEstimated(void) const { + if (pvehicle_state_estimator_ == nullptr) { + return Kinematics::Zero(); + } + return Utils::ToKinematicsState3r( pvehicle_state_estimator_->GetKinematicsEstimated()); } @@ -329,6 +333,48 @@ bool SimpleDriveApi::MoveByHeading(float heading, float speed, float duration, return f_result; } +bool SimpleDriveApi::MoveOnPath(std::vector> path, + float velocity, float timeout_sec, + float yaw_rate_max, float lookahead, + float adaptive_lookahead, + int64_t command_start_time_nanos) { + if (!api_control_enabled_) { + GetLogger().LogError(GetControllerName(), + "Vehicle cannot be commanded via API because API has " + "not been given control"); + return false; + } + + if (vehicle_state_ != VehicleStateType::kArmed && + vehicle_state_ != VehicleStateType::kActive) { + GetLogger().LogError(GetControllerName(), + "Vehicle cannot MoveOnPath because it is not armed"); + return false; + } + + if (path.empty()) { + return true; + } + + for (const auto& point : path) { + if (point.size() < 2) { + GetLogger().LogError(GetControllerName(), + "MoveOnPath terminated because a path point has " + "fewer than 2 coordinates"); + return false; + } + + // Ground vehicles use NED x/y only; ignore z if present. + if (!MoveToPosition(point[0], point[1], velocity, timeout_sec, yaw_rate_max, + lookahead, adaptive_lookahead, + command_start_time_nanos)) { + return false; + } + } + + return true; +} + } // namespace simple_drive } // namespace projectairsim } // namespace microsoft diff --git a/vehicle_apis/rover_api/src/simple_drive/throttle_controller.cpp b/vehicle_apis/rover_api/src/simple_drive/throttle_controller.cpp index fec045b2..06ee729b 100644 --- a/vehicle_apis/rover_api/src/simple_drive/throttle_controller.cpp +++ b/vehicle_apis/rover_api/src/simple_drive/throttle_controller.cpp @@ -3,6 +3,8 @@ // MIT License. All rights reserved. +#include + #include namespace microsoft { @@ -24,7 +26,9 @@ void ThrottleController::Update(void) { if (goal.mode == Goal::Mode::kPassthrough) output_ = goal.value; else if (goal.mode == Goal::Mode::kVelocityWorld) { - auto velocity_cur = pistate_estimator_->GetLinearVelocity().X(); + auto velocity_world = pistate_estimator_->GetLinearVelocity(); + auto velocity_cur = std::sqrt((velocity_world.X() * velocity_world.X()) + + (velocity_world.Y() * velocity_world.Y())); if (fgoalmodes_changed || (goal.value != pid_controller_throttle_.getPoint())) {