From 5b1d14e6caa0efd8662ab0c2c2c28f2c7cf6b995 Mon Sep 17 00:00:00 2001 From: Giovanni Muhammad Raditya Date: Fri, 19 Jul 2024 16:32:12 +0900 Subject: [PATCH] feat(carla_autoware): add interface to easily use CARLA with Autoware (#6859) Signed-off-by: mraditya01 Signed-off-by: Maxime CLEMENT Co-authored-by: Minsu Kim --- .../autoware_carla_interface/CMakeLists.txt | 31 + simulator/autoware_carla_interface/README.md | 161 ++++ .../calibration_maps/accel_map.csv | 7 + .../calibration_maps/brake_map.csv | 10 + .../calibration_maps/steer_map.csv | 4 + .../config/objects.json | 62 ++ .../raw_vehicle_cmd_converter.param.yaml | 28 + .../autoware_carla_interface.launch.xml | 50 + .../autoware_carla_interface/package.xml | 29 + .../resource/carla_autoware | 0 simulator/autoware_carla_interface/setup.cfg | 4 + simulator/autoware_carla_interface/setup.py | 33 + .../carla_autoware.py | 195 ++++ .../src/autoware_carla_interface/carla_ros.py | 486 ++++++++++ .../modules/carla_data_provider.py | 867 ++++++++++++++++++ .../modules/carla_utils.py | 109 +++ .../modules/carla_wrapper.py | 231 +++++ 17 files changed, 2307 insertions(+) create mode 100644 simulator/autoware_carla_interface/CMakeLists.txt create mode 100644 simulator/autoware_carla_interface/README.md create mode 100644 simulator/autoware_carla_interface/calibration_maps/accel_map.csv create mode 100644 simulator/autoware_carla_interface/calibration_maps/brake_map.csv create mode 100644 simulator/autoware_carla_interface/calibration_maps/steer_map.csv create mode 100644 simulator/autoware_carla_interface/config/objects.json create mode 100644 simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml create mode 100644 simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml create mode 100644 simulator/autoware_carla_interface/package.xml create mode 100644 simulator/autoware_carla_interface/resource/carla_autoware create mode 100644 simulator/autoware_carla_interface/setup.cfg create mode 100644 simulator/autoware_carla_interface/setup.py create mode 100644 simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py create mode 100644 simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py create mode 100644 simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py create mode 100644 simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py create mode 100644 simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py diff --git a/simulator/autoware_carla_interface/CMakeLists.txt b/simulator/autoware_carla_interface/CMakeLists.txt new file mode 100644 index 0000000000000..d643b9dad45fe --- /dev/null +++ b/simulator/autoware_carla_interface/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_carla_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package( + catkin REQUIRED COMPONENTS std_msgs) + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +ament_export_dependencies(rclpy) + +find_package(ros_environment REQUIRED) +set(ROS_VERSION $ENV{ROS_VERSION}) + + +# Install launch files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) + + +ament_auto_package( + launch + resource + config + src +) +ament_package() diff --git a/simulator/autoware_carla_interface/README.md b/simulator/autoware_carla_interface/README.md new file mode 100644 index 0000000000000..a44cb4708b5c3 --- /dev/null +++ b/simulator/autoware_carla_interface/README.md @@ -0,0 +1,161 @@ +# autoware_carla_interface + +## ROS 2/Autoware.universe bridge for CARLA simulator + +Thanks to for ROS 2 Humble support for CARLA Communication. +This ros package enables communication between Autoware and CARLA for autonomous driving simulation. + +## Supported Environment + +| ubuntu | ros | carla | autoware | +| :----: | :----: | :----: | :------: | +| 22.04 | humble | 0.9.15 | Main | + +## Setup + +### Install + +- [CARLA Installation](https://carla.readthedocs.io/en/latest/start_quickstart/) +- [Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) +- [Python Package for CARLA 0.9.15 ROS 2 Humble communication](https://github.com/gezp/carla_ros/releases/tag/carla-0.9.15-ubuntu-22.04) + + - Install the wheel using pip. + - OR add the egg file to the `PYTHONPATH`. + +1. Download maps (y-axis inverted version) to arbitrary location +2. Change names and create the map folder (example: Town01) inside `autoware_map`. (`point_cloud/Town01.pcd` -> `autoware_map/Town01/pointcloud_map.pcd`, `vector_maps/lanelet2/Town01.osm`-> `autoware_map/Town01/lanelet2_map.osm`) +3. Create `map_projector_info.yaml` on the folder and add `projector_type: local` on the first line. + +### Build + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +### Run + +1. Run carla, change map, spawn object if you need + + + ```bash + cd CARLA + ./CarlaUE4.sh -prefernvidia -quality-level=Low -RenderOffScreen + ``` + +2. Run ros nodes + + ```bash + ros2 launch autoware_launch e2e_simulator.launch.xml map_path:=$HOME/autoware_map/Town01 vehicle_model:=sample_vehicle sensor_model:=awsim_sensor_kit simulator_type:=carla carla_map:=Town01 + ``` + +3. Set initial pose (Init by GNSS) +4. Set goal position +5. Wait for planning +6. Engage + +## Inner-workings / Algorithms + +The `InitializeInterface` class is key to setting up both the CARLA world and the ego vehicle. It fetches configuration parameters through the `autoware_carla_interface.launch.xml`. + +The main simulation loop runs within the `carla_ros2_interface` class. This loop ticks simulation time inside the CARLA simulator at `fixed_delta_seconds` time, where data is received and published as ROS 2 messages at frequencies defined in `self.sensor_frequencies`. + +Ego vehicle commands from Autoware are processed through the `autoware_raw_vehicle_cmd_converter`, which calibrates these commands for CARLA. The calibrated commands are then fed directly into CARLA control via `CarlaDataProvider`. + +### Configurable Parameters for World Loading + +All the key parameters can be configured in `autoware_carla_interface.launch.xml`. + +| Name | Type | Default Value | Description | +| ------------------------- | ------ | --------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `host` | string | "localhost" | Hostname for the CARLA server | +| `port` | int | "2000" | Port number for the CARLA server | +| `timeout` | int | 20 | Timeout for the CARLA client | +| `ego_vehicle_role_name` | string | "ego_vehicle" | Role name for the ego vehicle | +| `vehicle_type` | string | "vehicle.toyota.prius" | Blueprint ID of the vehicle to spawn. The Blueprint ID of vehicles can be found in [CARLA Blueprint ID](https://carla.readthedocs.io/en/latest/catalogue_vehicles/) | +| `spawn_point` | string | None | Coordinates for spawning the ego vehicle (None is random). Format = [x, y, z, roll, pitch, yaw] | +| `carla_map` | string | "Town01" | Name of the map to load in CARLA | +| `sync_mode` | bool | True | Boolean flag to set synchronous mode in CARLA | +| `fixed_delta_seconds` | double | 0.05 | Time step for the simulation (related to client FPS) | +| `objects_definition_file` | string | "$(find-pkg-share autoware_carla_interface)/objects.json" | Sensor parameters file that are used for spawning sensor in CARLA | +| `use_traffic_manager` | bool | True | Boolean flag to set traffic manager in CARLA | +| `max_real_delta_seconds` | double | 0.05 | Parameter to limit the simulation speed below `fixed_delta_seconds` | +| `config_file` | string | "$(find-pkg-share autoware_carla_interface)/raw_vehicle_cmd_converter.param.yaml" | Control mapping file to be used in `autoware_raw_vehicle_cmd_converter`. Current control are calibrated based on `vehicle.toyota.prius` Blueprints ID in CARLA. Changing the vehicle type may need a recalibration. | + +### Configurable Parameters for Sensors + +Below parameters can be configured in `carla_ros.py`. + +| Name | Type | Default Value | Description | +| ------------------------- | ---- | -------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `self.sensor_frequencies` | dict | {"top": 11, "left": 11, "right": 11, "camera": 11, "imu": 50, "status": 50, "pose": 2} | (line 67) Calculates the time interval since the last publication and checks if this interval meets the minimum required to not exceed the desired frequency. It will only affect ROS publishing frequency not CARLA sensor tick. | + +- CARLA sensor parameters can be configured in `config/objects.json`. + - For more details regarding the parameters that can be modified in CARLA are explained in [Carla Ref Sensor](https://carla.readthedocs.io/en/latest/ref_sensors/). + +### World Loading + +The `carla_ros.py` sets up the CARLA world: + +1. **Client Connection**: + + ```python + client = carla.Client(self.local_host, self.port) + client.set_timeout(self.timeout) + ``` + +2. **Load the Map**: + + Map loaded in CARLA world with map according to `carla_map` parameter. + + ```python + client.load_world(self.map_name) + self.world = client.get_world() + ``` + +3. **Spawn Ego Vehicle**: + + Vehicle are spawn according to `vehicle_type`, `spawn_point`, and `agent_role_name` parameter. + + ```python + spawn_point = carla.Transform() + point_items = self.spawn_point.split(",") + if len(point_items) == 6: + spawn_point.location.x = float(point_items[0]) + spawn_point.location.y = float(point_items[1]) + spawn_point.location.z = float(point_items[2]) + 2 + spawn_point.rotation.roll = float(point_items[3]) + spawn_point.rotation.pitch = float(point_items[4]) + spawn_point.rotation.yaw = float(point_items[5]) + CarlaDataProvider.request_new_actor(self.vehicle_type, spawn_point, self.agent_role_name) + ``` + +## Traffic Light Recognition + +The maps provided by the Carla Simulator ([Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/)) currently lack proper traffic light components for Autoware and have different latitude and longitude coordinates compared to the pointcloud map. To enable traffic light recognition, follow the steps below to modify the maps. + +- Options to Modify the Map + + - A. Create a New Map from Scratch + - Use the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/) to create a new map. + + - B. Modify the Existing Carla Lanelet2 Maps + - Adjust the longitude and latitude of the [Carla Lanelet2 Maps](https://bitbucket.org/carla-simulator/autoware-contents/src/master/maps/) to align with the PCD (origin). + - Use this [tool](https://github.com/mraditya01/offset_lanelet2/tree/main) to modify the coordinates. + - Snap Lanelet with PCD and add the traffic lights using the [Tier4 Vector Map Builder](https://tools.tier4.jp/feature/vector_map_builder_ll2/). + +- When using the Tier4 Vector Map Builder, you must convert the PCD format from `binary_compressed` to `ascii`. You can use `pcl_tools` for this conversion. +- For reference, an example of Town01 with added traffic lights at one intersection can be downloaded [here](https://drive.google.com/drive/folders/1QFU0p3C8NW71sT5wwdnCKXoZFQJzXfTG?usp=sharing). + +## Tips + +- Misalignment might occurs during initialization, pressing `init by gnss` button should fix it. +- Changing the `fixed_delta_seconds` can increase the simulation tick (default 0.05 s), some sensors params in `objects.json` need to be adjusted when it is changed (example: LIDAR rotation frequency have to match the FPS). + +## Known Issues and Future Works + +- Testing on procedural map (Adv Digital Twin). + - Currently unable to test it due to failing in the creation of the Adv digital twin map. +- Automatic sensor configuration of the CARLA sensors from the Autoware sensor kit. + - Sensor currently not automatically configured to have the same location as the Autoware Sensor kit. The current work around is to create a new frame of each sensors with (0, 0, 0, 0, 0, 0) coordinate relative to base_link and attach each sensor on the new frame (`autoware_carla_interface.launch.xml` Line 28). This work around is very limited and restrictive, as when the sensor_kit is changed the sensor location will be wrongly attached. +- Traffic light recognition. + - Currently the HDmap of CARLA did not have information regarding the traffic light which is necessary for Autoware to conduct traffic light recognition. diff --git a/simulator/autoware_carla_interface/calibration_maps/accel_map.csv b/simulator/autoware_carla_interface/calibration_maps/accel_map.csv new file mode 100644 index 0000000000000..18718c31df87e --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/accel_map.csv @@ -0,0 +1,7 @@ +default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89 +0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500 +0.100,0.167,0.166,-0.093,-0.243,-0.244,-0.245,-0.246,-0.247,-0.248,-0.249,-0.280 +0.200,0.941,0.464,0.186,0.004,-0.100,-0.101,-0.102,-0.103,-0.104,-0.105,-0.106 +0.300,1.747,1.332,0.779,0.778,0.777,0.776,0.775,0.774,0.720,0.640,0.580 +0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100 +0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610 diff --git a/simulator/autoware_carla_interface/calibration_maps/brake_map.csv b/simulator/autoware_carla_interface/calibration_maps/brake_map.csv new file mode 100644 index 0000000000000..62b18b45bd415 --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/brake_map.csv @@ -0,0 +1,10 @@ +default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89 +0,0.090,-0.204,-0.490,-0.490,-0.491,-0.492,-0.493,-0.494,-0.495,-0.496,-0.500 +0.100,0.089,-0.226,-0.535,-0.536,-0.537,-0.538,-0.539,-0.540,-0.541,-0.542,-0.543 +0.200,-0.380,-0.414,-0.746,-0.800,-0.820,-0.850,-0.870,-0.890,-0.910,-0.940,-0.960 +0.300,-1.000,-1.040,-1.480,-1.550,-1.570,-1.590,-1.610,-1.630,-1.631,-1.632,-1.633 +0.400,-1.480,-1.500,-1.850,-2.050,-2.100,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 +0.500,-1.490,-1.510,-1.860,-2.060,-2.110,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 +0.600,-1.500,-1.520,-1.870,-2.070,-2.120,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 +0.700,-1.510,-1.530,-1.880,-2.080,-2.130,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 +0.800,-2.180,-2.200,-2.700,-2.800,-2.900,-2.950,-2.951,-2.952,-2.953,-2.954,-2.955 diff --git a/simulator/autoware_carla_interface/calibration_maps/steer_map.csv b/simulator/autoware_carla_interface/calibration_maps/steer_map.csv new file mode 100644 index 0000000000000..077efb9f9e200 --- /dev/null +++ b/simulator/autoware_carla_interface/calibration_maps/steer_map.csv @@ -0,0 +1,4 @@ +default,-10,0,10 +-1,-0.9,-0.9,-0.9 +0,0,0,0 +1,0.9,0.9,0.9 diff --git a/simulator/autoware_carla_interface/config/objects.json b/simulator/autoware_carla_interface/config/objects.json new file mode 100644 index 0000000000000..7103118937548 --- /dev/null +++ b/simulator/autoware_carla_interface/config/objects.json @@ -0,0 +1,62 @@ +{ + "sensors": [ + { + "type": "sensor.camera.rgb", + "id": "rgb_front", + "spawn_point": { + "x": 0.7, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 90.0 + }, + { + "type": "sensor.lidar.ray_cast", + "id": "top", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 3.1, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + }, + "range": 100, + "channels": 64, + "points_per_second": 300000, + "upper_fov": 10.0, + "lower_fov": -30.0, + "rotation_frequency": 20, + "noise_stddev": 0.0 + }, + { + "type": "sensor.other.gnss", + "id": "gnss", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + }, + { + "type": "sensor.other.imu", + "id": "imu", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.6, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0 + } + } + ] +} diff --git a/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml b/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml new file mode 100644 index 0000000000000..cb604bc686075 --- /dev/null +++ b/simulator/autoware_carla_interface/config/raw_vehicle_cmd_converter.param.yaml @@ -0,0 +1,28 @@ +/**: + ros__parameters: + csv_path_accel_map: $(find-pkg-share autoware_carla_interface)/accel_map.csv + csv_path_brake_map: $(find-pkg-share autoware_carla_interface)/brake_map.csv + csv_path_steer_map: $(find-pkg-share autoware_carla_interface)/steer_map.csv + convert_accel_cmd: true + convert_brake_cmd: true + convert_steer_cmd: false + use_steer_ff: true + use_steer_fb: true + is_debugging: false + max_throttle: 0.4 + max_brake: 0.8 + max_steer: 1.0 + min_steer: -1.0 + steer_pid: + kp: 150.0 + ki: 15.0 + kd: 0.0 + max: 8.0 + min: -8.0 + max_p: 8.0 + min_p: -8.0 + max_i: 8.0 + min_i: -8.0 + max_d: 0.0 + min_d: 0.0 + invalid_integration_decay: 0.97 diff --git a/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml b/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml new file mode 100644 index 0000000000000..bdef2563777fc --- /dev/null +++ b/simulator/autoware_carla_interface/launch/autoware_carla_interface.launch.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/autoware_carla_interface/package.xml b/simulator/autoware_carla_interface/package.xml new file mode 100644 index 0000000000000..136138a0101b0 --- /dev/null +++ b/simulator/autoware_carla_interface/package.xml @@ -0,0 +1,29 @@ + + + autoware_carla_interface + 0.0.0 + The carla autoware bridge package + Muhammad Raditya GIOVANNI + Maxime CLEMENT + Apache License 2.0 + + std_msgs + autoware_perception_msgs + autoware_vehicle_msgs + geometry_msgs + rclpy + sensor_msgs + sensor_msgs_py + tf2 + tf2_ros + tier4_vehicle_msgs + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/simulator/autoware_carla_interface/resource/carla_autoware b/simulator/autoware_carla_interface/resource/carla_autoware new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/simulator/autoware_carla_interface/setup.cfg b/simulator/autoware_carla_interface/setup.cfg new file mode 100644 index 0000000000000..c749fdbef61a2 --- /dev/null +++ b/simulator/autoware_carla_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autoware_carla_interface +[install] +install_scripts=$base/lib/autoware_carla_interface diff --git a/simulator/autoware_carla_interface/setup.py b/simulator/autoware_carla_interface/setup.py new file mode 100644 index 0000000000000..f4bf37db01fb9 --- /dev/null +++ b/simulator/autoware_carla_interface/setup.py @@ -0,0 +1,33 @@ +from glob import glob +import os + +from setuptools import setup + +ROS_VERSION = int(os.environ["ROS_VERSION"]) + +package_name = "autoware_carla_interface" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/" + package_name, glob("config/*")), + ("share/" + package_name, glob("calibration_maps/*.csv")), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/autoware_carla_interface.launch.xml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Muhammad Raditya GIOVANNI, Maxime CLEMENT", + maintainer_email="mradityagio@gmail.com, maxime.clement@tier4.jp", + description="CARLA ROS 2 bridge for AUTOWARE", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "autoware_carla_interface = autoware_carla_interface.carla_autoware:main" + ], + }, + package_dir={"": "src"}, +) diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py new file mode 100644 index 0000000000000..b9c72d6c137d9 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_autoware.py @@ -0,0 +1,195 @@ +# Copyright 2024 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.sr/bin/env python + +import random +import signal +import time + +import carla + +from .carla_ros import carla_ros2_interface +from .modules.carla_data_provider import CarlaDataProvider +from .modules.carla_data_provider import GameTime +from .modules.carla_wrapper import SensorReceivedNoData +from .modules.carla_wrapper import SensorWrapper + + +class SensorLoop(object): + def __init__(self): + self.start_game_time = None + self.start_system_time = None + self.sensor = None + self.ego_actor = None + self.running = False + self.timestamp_last_run = 0.0 + self.timeout = 20.0 + + def _stop_loop(self): + self.running = False + + def _tick_sensor(self, timestamp): + if self.timestamp_last_run < timestamp.elapsed_seconds and self.running: + self.timestamp_last_run = timestamp.elapsed_seconds + GameTime.on_carla_tick(timestamp) + CarlaDataProvider.on_carla_tick() + try: + ego_action = self.sensor() + except SensorReceivedNoData as e: + raise RuntimeError(e) + self.ego_actor.apply_control(ego_action) + if self.running: + CarlaDataProvider.get_world().tick() + + +class InitializeInterface(object): + def __init__(self): + self.interface = carla_ros2_interface() + self.param_ = self.interface.get_param() + self.world = None + self.sensor_wrapper = None + self.ego_actor = None + self.prev_tick_wall_time = 0.0 + + # Parameter for Initializing Carla World + self.local_host = self.param_["host"] + self.port = self.param_["port"] + self.timeout = self.param_["timeout"] + self.sync_mode = self.param_["sync_mode"] + self.fixed_delta_seconds = self.param_["fixed_delta_seconds"] + self.carla_map = self.param_["carla_map"] + self.agent_role_name = self.param_["ego_vehicle_role_name"] + self.vehicle_type = self.param_["vehicle_type"] + self.spawn_point = self.param_["spawn_point"] + self.use_traffic_manager = self.param_["use_traffic_manager"] + self.max_real_delta_seconds = self.param_["max_real_delta_seconds"] + + def load_world(self): + client = carla.Client(self.local_host, self.port) + client.set_timeout(self.timeout) + client.load_world(self.carla_map) + self.world = client.get_world() + settings = self.world.get_settings() + settings.fixed_delta_seconds = self.fixed_delta_seconds + settings.synchronous_mode = self.sync_mode + self.world.apply_settings(settings) + CarlaDataProvider.set_world(self.world) + CarlaDataProvider.set_client(client) + spawn_point = carla.Transform() + point_items = self.spawn_point.split(",") + randomize = False + if len(point_items) == 6: + spawn_point.location.x = float(point_items[0]) + spawn_point.location.y = float(point_items[1]) + spawn_point.location.z = ( + float(point_items[2]) + 2 + ) # +2 is used so the car did not stuck on the road when spawned. + spawn_point.rotation.roll = float(point_items[3]) + spawn_point.rotation.pitch = float(point_items[4]) + spawn_point.rotation.yaw = float(point_items[5]) + else: + randomize = True + self.ego_actor = CarlaDataProvider.request_new_actor( + self.vehicle_type, spawn_point, self.agent_role_name, random_location=randomize + ) + self.interface.ego_actor = self.ego_actor # TODO improve design + self.interface.physics_control = self.ego_actor.get_physics_control() + + self.sensor_wrapper = SensorWrapper(self.interface) + self.sensor_wrapper.setup_sensors(self.ego_actor, False) + ########################################################################################################################################################## + # TRAFFIC MANAGER + ########################################################################################################################################################## + # cspell:ignore trafficmanager + if self.use_traffic_manager: + traffic_manager = client.get_trafficmanager() + traffic_manager.set_synchronous_mode(True) + traffic_manager.set_random_device_seed(0) + random.seed(0) + spawn_points_tm = self.world.get_map().get_spawn_points() + for i, spawn_point in enumerate(spawn_points_tm): + self.world.debug.draw_string(spawn_point.location, str(i), life_time=10) + models = [ + "dodge", + "audi", + "model3", + "mini", + "mustang", + "lincoln", + "prius", + "nissan", + "crown", + "impala", + ] + blueprints = [] + for vehicle in self.world.get_blueprint_library().filter("*vehicle*"): + if any(model in vehicle.id for model in models): + blueprints.append(vehicle) + max_vehicles = 30 + max_vehicles = min([max_vehicles, len(spawn_points_tm)]) + vehicles = [] + for i, spawn_point in enumerate(random.sample(spawn_points_tm, max_vehicles)): + temp = self.world.try_spawn_actor(random.choice(blueprints), spawn_point) + if temp is not None: + vehicles.append(temp) + + for vehicle in vehicles: + vehicle.set_autopilot(True) + + def run_bridge(self): + self.bridge_loop = SensorLoop() + self.bridge_loop.sensor = self.sensor_wrapper + self.bridge_loop.ego_actor = self.ego_actor + self.bridge_loop.start_system_time = time.time() + self.bridge_loop.start_game_time = GameTime.get_time() + self.bridge_loop.running = True + while self.bridge_loop.running: + timestamp = None + world = CarlaDataProvider.get_world() + if world: + snapshot = world.get_snapshot() + if snapshot: + timestamp = snapshot.timestamp + if timestamp: + delta_step = time.time() - self.prev_tick_wall_time + if delta_step <= self.max_real_delta_seconds: + # Add a wait to match the max_real_delta_seconds + time.sleep(self.max_real_delta_seconds - delta_step) + self.prev_tick_wall_time = time.time() + self.bridge_loop._tick_sensor(timestamp) + + def _stop_loop(self, sign, frame): + self.bridge_loop._stop_loop() + + def _cleanup(self): + self.sensor_wrapper.cleanup() + CarlaDataProvider.cleanup() + if self.ego_actor: + self.ego_actor.destroy() + self.ego_actor = None + + if self.interface: + self.interface.shutdown() + self.interface = None + + +def main(): + carla_bridge = InitializeInterface() + carla_bridge.load_world() + signal.signal(signal.SIGINT, carla_bridge._stop_loop) + carla_bridge.run_bridge() + carla_bridge._cleanup() + + +if __name__ == "__main__": + main() diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py new file mode 100644 index 0000000000000..a4f8dee7af1a0 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -0,0 +1,486 @@ +# Copyright 2024 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.sr/bin/env python + +import json +import math +import threading + +from autoware_vehicle_msgs.msg import ControlModeReport +from autoware_vehicle_msgs.msg import GearReport +from autoware_vehicle_msgs.msg import SteeringReport +from autoware_vehicle_msgs.msg import VelocityReport +from builtin_interfaces.msg import Time +import carla +from cv_bridge import CvBridge +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseWithCovarianceStamped +import numpy +import rclpy +from rosgraph_msgs.msg import Clock +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from sensor_msgs.msg import Imu +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from std_msgs.msg import Header +from tier4_vehicle_msgs.msg import ActuationCommandStamped +from tier4_vehicle_msgs.msg import ActuationStatusStamped +from transforms3d.euler import euler2quat + +from .modules.carla_data_provider import GameTime +from .modules.carla_data_provider import datetime +from .modules.carla_utils import carla_location_to_ros_point +from .modules.carla_utils import carla_rotation_to_ros_quaternion +from .modules.carla_utils import create_cloud +from .modules.carla_utils import ros_pose_to_carla_transform +from .modules.carla_wrapper import SensorInterface + + +class carla_ros2_interface(object): + def __init__(self): + self.sensor_interface = SensorInterface() + self.timestamp = None + self.ego_actor = None + self.physics_control = None + self.channels = 0 + self.id_to_sensor_type_map = {} + self.id_to_camera_info_map = {} + self.cv_bridge = CvBridge() + self.first_ = True + self.pub_lidar = {} + self.sensor_frequencies = { + "top": 11, + "left": 11, + "right": 11, + "camera": 11, + "imu": 50, + "status": 50, + "pose": 2, + } + self.publish_prev_times = { + sensor: datetime.datetime.now() for sensor in self.sensor_frequencies + } + + # initialize ros2 node + rclpy.init(args=None) + self.ros2_node = rclpy.create_node("carla_ros2_interface") + self.parameters = { + "host": rclpy.Parameter.Type.STRING, + "port": rclpy.Parameter.Type.INTEGER, + "sync_mode": rclpy.Parameter.Type.BOOL, + "timeout": rclpy.Parameter.Type.INTEGER, + "fixed_delta_seconds": rclpy.Parameter.Type.DOUBLE, + "carla_map": rclpy.Parameter.Type.STRING, + "ego_vehicle_role_name": rclpy.Parameter.Type.STRING, + "spawn_point": rclpy.Parameter.Type.STRING, + "vehicle_type": rclpy.Parameter.Type.STRING, + "objects_definition_file": rclpy.Parameter.Type.STRING, + "use_traffic_manager": rclpy.Parameter.Type.BOOL, + "max_real_delta_seconds": rclpy.Parameter.Type.DOUBLE, + } + self.param_values = {} + for param_name, param_type in self.parameters.items(): + self.ros2_node.declare_parameter(param_name, param_type) + self.param_values[param_name] = self.ros2_node.get_parameter(param_name).value + + # Publish clock + self.clock_publisher = self.ros2_node.create_publisher(Clock, "/clock", 10) + obj_clock = Clock() + obj_clock.clock = Time(sec=0) + self.clock_publisher.publish(obj_clock) + + # Sensor Config (Edit your sensor here) + self.sensors = json.load(open(self.param_values["objects_definition_file"])) + + # Subscribing Autoware Control messages and converting to CARLA control + self.sub_control = self.ros2_node.create_subscription( + ActuationCommandStamped, "/control/command/actuation_cmd", self.control_callback, 1 + ) + + self.sub_vehicle_initialpose = self.ros2_node.create_subscription( + PoseWithCovarianceStamped, "initialpose", self.initialpose_callback, 1 + ) + + self.current_control = carla.VehicleControl() + + # Direct data publishing from CARLA for Autoware + self.pub_pose_with_cov = self.ros2_node.create_publisher( + PoseWithCovarianceStamped, "/sensing/gnss/pose_with_covariance", 1 + ) + self.pub_vel_state = self.ros2_node.create_publisher( + VelocityReport, "/vehicle/status/velocity_status", 1 + ) + self.pub_steering_state = self.ros2_node.create_publisher( + SteeringReport, "/vehicle/status/steering_status", 1 + ) + self.pub_ctrl_mode = self.ros2_node.create_publisher( + ControlModeReport, "/vehicle/status/control_mode", 1 + ) + self.pub_gear_state = self.ros2_node.create_publisher( + GearReport, "/vehicle/status/gear_status", 1 + ) + self.pub_actuation_status = self.ros2_node.create_publisher( + ActuationStatusStamped, "/vehicle/status/actuation_status", 1 + ) + + # Create Publisher for each Physical Sensors + for sensor in self.sensors["sensors"]: + self.id_to_sensor_type_map[sensor["id"]] = sensor["type"] + if sensor["type"] == "sensor.camera.rgb": + self.pub_camera = self.ros2_node.create_publisher( + Image, "/sensing/camera/traffic_light/image_raw", 1 + ) + self.pub_camera_info = self.ros2_node.create_publisher( + CameraInfo, "/sensing/camera/traffic_light/camera_info", 1 + ) + elif sensor["type"] == "sensor.lidar.ray_cast": + if sensor["id"] in self.sensor_frequencies: + self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + ) + else: + self.ros2_node.get_logger().info( + "Please use Top, Right, or Left as the LIDAR ID" + ) + elif sensor["type"] == "sensor.other.imu": + self.pub_imu = self.ros2_node.create_publisher( + Imu, "/sensing/imu/tamagawa/imu_raw", 1 + ) + else: + self.ros2_node.get_logger().info(f'No Publisher for {sensor["type"]} Sensor') + pass + + self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.ros2_node,)) + self.spin_thread.start() + + def __call__(self): + input_data = self.sensor_interface.get_data() + timestamp = GameTime.get_time() + control = self.run_step(input_data, timestamp) + return control + + def get_param(self): + return self.param_values + + def checkFrequency(self, sensor): + time_delta = ( + datetime.datetime.now() - self.publish_prev_times[sensor] + ).microseconds / 1000000.0 + if 1.0 / time_delta >= self.sensor_frequencies[sensor]: + return True + return False + + def get_msg_header(self, frame_id): + """Obtain and modify ROS message header.""" + header = Header() + header.frame_id = frame_id + seconds = int(self.timestamp) + nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) + header.stamp = Time(sec=seconds, nanosec=nanoseconds) + return header + + def lidar(self, carla_lidar_measurement, id_): + """Transform the received lidar measurement into a ROS point cloud message.""" + if self.checkFrequency(id_): + return + self.publish_prev_times[id_] = datetime.datetime.now() + + header = self.get_msg_header(frame_id="velodyne_top_changed") + fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name="intensity", offset=12, datatype=PointField.UINT8, count=1), + PointField(name="return_type", offset=13, datatype=PointField.UINT8, count=1), + PointField(name="channel", offset=14, datatype=PointField.UINT16, count=1), + ] + + lidar_data = numpy.frombuffer( + carla_lidar_measurement.raw_data, dtype=numpy.float32 + ).reshape(-1, 4) + intensity = lidar_data[:, 3] + intensity = ( + numpy.clip(intensity, 0, 1) * 255 + ) # CARLA lidar intensity values are between 0 and 1 + intensity = intensity.astype(numpy.uint8).reshape(-1, 1) + + return_type = numpy.zeros((lidar_data.shape[0], 1), dtype=numpy.uint8) + channel = numpy.empty((0, 1), dtype=numpy.uint16) + self.channels = self.sensors["sensors"] + + for i in range(self.channels[1]["channels"]): + current_ring_points_count = carla_lidar_measurement.get_point_count(i) + channel = numpy.vstack( + (channel, numpy.full((current_ring_points_count, 1), i, dtype=numpy.uint16)) + ) + + lidar_data = numpy.hstack((lidar_data[:, :3], intensity, return_type, channel)) + lidar_data[:, 1] *= -1 + + dtype = [ + ("x", "f4"), + ("y", "f4"), + ("z", "f4"), + ("intensity", "u1"), + ("return_type", "u1"), + ("channel", "u2"), + ] + + structured_lidar_data = numpy.zeros(lidar_data.shape[0], dtype=dtype) + structured_lidar_data["x"] = lidar_data[:, 0] + structured_lidar_data["y"] = lidar_data[:, 1] + structured_lidar_data["z"] = lidar_data[:, 2] + structured_lidar_data["intensity"] = lidar_data[:, 3].astype(numpy.uint8) + structured_lidar_data["return_type"] = lidar_data[:, 4].astype(numpy.uint8) + structured_lidar_data["channel"] = lidar_data[:, 5].astype(numpy.uint16) + + point_cloud_msg = create_cloud(header, fields, structured_lidar_data) + self.pub_lidar[id_].publish(point_cloud_msg) + + def initialpose_callback(self, data): + """Transform RVIZ initial pose to CARLA.""" + pose = data.pose.pose + pose.position.z += 2.0 + carla_pose_transform = ros_pose_to_carla_transform(pose) + if self.ego_actor is not None: + self.ego_actor.set_transform(carla_pose_transform) + else: + print("Can't find Ego Vehicle") + + def pose(self): + """Transform odometry data to Pose and publish Pose with Covariance message.""" + if self.checkFrequency("pose"): + return + self.publish_prev_times["pose"] = datetime.datetime.now() + + header = self.get_msg_header(frame_id="map") + out_pose_with_cov = PoseWithCovarianceStamped() + pose_carla = Pose() + pose_carla.position = carla_location_to_ros_point(self.ego_actor.get_transform().location) + pose_carla.orientation = carla_rotation_to_ros_quaternion( + self.ego_actor.get_transform().rotation + ) + out_pose_with_cov.header = header + out_pose_with_cov.pose.pose = pose_carla + out_pose_with_cov.pose.covariance = [ + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + self.pub_pose_with_cov.publish(out_pose_with_cov) + + def _build_camera_info(self, camera_actor): + """Build camera info.""" + camera_info = CameraInfo() + camera_info.width = camera_actor.width + camera_info.height = camera_actor.height + camera_info.distortion_model = "plumb_bob" + cx = camera_info.width / 2.0 + cy = camera_info.height / 2.0 + fx = camera_info.width / (2.0 * math.tan(camera_actor.fov * math.pi / 360.0)) + fy = fx + camera_info.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] + camera_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] + camera_info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + camera_info.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] + self._camera_info = camera_info + + def camera(self, carla_camera_data): + """Transform the received carla camera data into a ROS image and info message and publish.""" + while self.first_: + self._camera_info_ = self._build_camera_info(carla_camera_data) + self.first_ = False + + if self.checkFrequency("camera"): + return + self.publish_prev_times["camera"] = datetime.datetime.now() + + image_data_array = numpy.ndarray( + shape=(carla_camera_data.height, carla_camera_data.width, 4), + dtype=numpy.uint8, + buffer=carla_camera_data.raw_data, + ) + # cspell:ignore interp bgra + img_msg = self.cv_bridge.cv2_to_imgmsg(image_data_array, encoding="bgra8") + img_msg.header = self.get_msg_header( + frame_id="traffic_light_left_camera/camera_optical_link" + ) + cam_info = self._camera_info + cam_info.header = img_msg.header + self.pub_camera_info.publish(cam_info) + self.pub_camera.publish(img_msg) + + def imu(self, carla_imu_measurement): + """Transform a received imu measurement into a ROS Imu message and publish Imu message.""" + if self.checkFrequency("imu"): + return + self.publish_prev_times["imu"] = datetime.datetime.now() + + imu_msg = Imu() + imu_msg.header = self.get_msg_header(frame_id="tamagawa/imu_link_changed") + imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x + imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y + imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z + + imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x + imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y + imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z + + roll = math.radians(carla_imu_measurement.transform.rotation.roll) + pitch = -math.radians(carla_imu_measurement.transform.rotation.pitch) + yaw = -math.radians(carla_imu_measurement.transform.rotation.yaw) + + quat = euler2quat(roll, pitch, yaw) + imu_msg.orientation.w = quat[0] + imu_msg.orientation.x = quat[1] + imu_msg.orientation.y = quat[2] + imu_msg.orientation.z = quat[3] + + self.pub_imu.publish(imu_msg) + + def control_callback(self, in_cmd): + """Convert and publish CARLA Ego Vehicle Control to AUTOWARE.""" + out_cmd = carla.VehicleControl() + out_cmd.throttle = in_cmd.actuation.accel_cmd + # convert base on steer curve of the vehicle + steer_curve = self.physics_control.steering_curve + current_vel = self.ego_actor.get_velocity() + max_steer_ratio = numpy.interp( + abs(current_vel.x), [v.x for v in steer_curve], [v.y for v in steer_curve] + ) + out_cmd.steer = ( + -in_cmd.actuation.steer_cmd + * max_steer_ratio + * math.radians(self.physics_control.wheels[0].max_steer_angle) + ) + out_cmd.brake = in_cmd.actuation.brake_cmd + self.current_control = out_cmd + + def ego_status(self): + """Publish ego vehicle status.""" + if self.checkFrequency("status"): + return + + self.publish_prev_times["status"] = datetime.datetime.now() + + # convert velocity from cartesian to ego frame + trans_mat = numpy.array(self.ego_actor.get_transform().get_matrix()).reshape(4, 4) + rot_mat = trans_mat[0:3, 0:3] + inv_rot_mat = rot_mat.T + vel_vec = numpy.array( + [ + self.ego_actor.get_velocity().x, + self.ego_actor.get_velocity().y, + self.ego_actor.get_velocity().z, + ] + ).reshape(3, 1) + ego_velocity = (inv_rot_mat @ vel_vec).T[0] + + out_vel_state = VelocityReport() + out_steering_state = SteeringReport() + out_ctrl_mode = ControlModeReport() + out_gear_state = GearReport() + out_actuation_status = ActuationStatusStamped() + + out_vel_state.header = self.get_msg_header(frame_id="base_link") + out_vel_state.longitudinal_velocity = ego_velocity[0] + out_vel_state.lateral_velocity = ego_velocity[1] + out_vel_state.heading_rate = ( + self.ego_actor.get_transform().transform_vector(self.ego_actor.get_angular_velocity()).z + ) + + out_steering_state.stamp = out_vel_state.header.stamp + out_steering_state.steering_tire_angle = -math.radians( + self.ego_actor.get_wheel_steer_angle(carla.VehicleWheelLocation.FL_Wheel) + ) + + out_gear_state.stamp = out_vel_state.header.stamp + out_gear_state.report = GearReport.DRIVE + + out_ctrl_mode.stamp = out_vel_state.header.stamp + out_ctrl_mode.mode = ControlModeReport.AUTONOMOUS + + control = self.ego_actor.get_control() + out_actuation_status.header = self.get_msg_header(frame_id="base_link") + out_actuation_status.status.accel_status = control.throttle + out_actuation_status.status.brake_status = control.brake + out_actuation_status.status.steer_status = -control.steer + + self.pub_actuation_status.publish(out_actuation_status) + self.pub_vel_state.publish(out_vel_state) + self.pub_steering_state.publish(out_steering_state) + self.pub_ctrl_mode.publish(out_ctrl_mode) + self.pub_gear_state.publish(out_gear_state) + + def run_step(self, input_data, timestamp): + self.timestamp = timestamp + seconds = int(self.timestamp) + nanoseconds = int((self.timestamp - int(self.timestamp)) * 1000000000.0) + obj_clock = Clock() + obj_clock.clock = Time(sec=seconds, nanosec=nanoseconds) + self.clock_publisher.publish(obj_clock) + + # publish data of all sensors + for key, data in input_data.items(): + sensor_type = self.id_to_sensor_type_map[key] + if sensor_type == "sensor.camera.rgb": + self.camera(data[1]) + elif sensor_type == "sensor.other.gnss": + self.pose() + elif sensor_type == "sensor.lidar.ray_cast": + self.lidar(data[1], key) + elif sensor_type == "sensor.other.imu": + self.imu(data[1]) + else: + self.ros2_node.get_logger().info("No Publisher for [{key}] Sensor") + + # Publish ego vehicle status + self.ego_status() + return self.current_control + + def shutdown(self): + self.ros2_node.destroy_node() diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py new file mode 100644 index 0000000000000..7d6608585f582 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_data_provider.py @@ -0,0 +1,867 @@ +# Copyright 2024 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.sr/bin/env python + +"""Modified CARLA Data Provider from CARLA scenario runner.""" + +from __future__ import print_function + +import datetime +import math +import re +import threading + +import carla +from numpy import random +from six import iteritems + + +def calculate_velocity(actor): + """Calculate the velocity of a actor.""" + velocity_squared = actor.get_velocity().x ** 2 + velocity_squared += actor.get_velocity().y ** 2 + return math.sqrt(velocity_squared) + + +class CarlaDataProvider(object): # pylint: disable=too-many-public-methods + _actor_velocity_map = {} + _actor_location_map = {} + _actor_transform_map = {} + _traffic_light_map = {} + _carla_actor_pool = {} + _global_osc_parameters = {} + _client = None + _world = None + _map = None + _sync_flag = False + _spawn_points = None + _spawn_index = 0 + _blueprint_library = None + _all_actors = None + _ego_vehicle_route = None + _traffic_manager_port = 8000 + _random_seed = 2000 + _rng = random.RandomState(_random_seed) + _local_planner = None + _runtime_init_flag = False + _lock = threading.Lock() + + @staticmethod + def set_local_planner(plan): + CarlaDataProvider._local_planner = plan + + @staticmethod + def get_local_planner(): + return CarlaDataProvider._local_planner + + @staticmethod + def register_actor(actor, transform=None): + """Add new actor to dictionaries.""" + with CarlaDataProvider._lock: + if actor in CarlaDataProvider._actor_velocity_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + else: + CarlaDataProvider._actor_velocity_map[actor] = 0.0 + if actor in CarlaDataProvider._actor_location_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + elif transform: + CarlaDataProvider._actor_location_map[actor] = transform.location + else: + CarlaDataProvider._actor_location_map[actor] = None + + if actor in CarlaDataProvider._actor_transform_map: + raise KeyError( + "Vehicle '{}' already registered. Cannot register twice!".format(actor.id) + ) + else: + CarlaDataProvider._actor_transform_map[actor] = transform + + @staticmethod + def update_osc_global_params(parameters): + """Updates/initializes global osc parameters.""" + CarlaDataProvider._global_osc_parameters.update(parameters) + + @staticmethod + def get_osc_global_param_value(ref): + """Return updated global osc parameter value.""" + return CarlaDataProvider._global_osc_parameters.get(ref.replace("$", "")) + + @staticmethod + def register_actors(actors, transforms=None): + """Add new set of actors to dictionaries.""" + if transforms is None: + transforms = [None] * len(actors) + + for actor, transform in zip(actors, transforms): + CarlaDataProvider.register_actor(actor, transform) + + @staticmethod + def on_carla_tick(): + with CarlaDataProvider._lock: + for actor in CarlaDataProvider._actor_velocity_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_velocity_map[actor] = calculate_velocity(actor) + + for actor in CarlaDataProvider._actor_location_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_location_map[actor] = actor.get_location() + + for actor in CarlaDataProvider._actor_transform_map: + if actor is not None and actor.is_alive: + CarlaDataProvider._actor_transform_map[actor] = actor.get_transform() + + world = CarlaDataProvider._world + if world is None: + print("WARNING: CarlaDataProvider couldn't find the world") + + CarlaDataProvider._all_actors = None + + @staticmethod + def get_velocity(actor): + """Return the absolute velocity for the given actor.""" + for key in CarlaDataProvider._actor_velocity_map: + if key.id == actor.id: + return CarlaDataProvider._actor_velocity_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_velocity: {} not found!".format(__name__, actor)) + return 0.0 + + @staticmethod + def get_location(actor): + """Return the location for the given actor.""" + for key in CarlaDataProvider._actor_location_map: + if key.id == actor.id: + return CarlaDataProvider._actor_location_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_location: {} not found!".format(__name__, actor)) + return None + + @staticmethod + def get_transform(actor): + """Return the transform for the given actor.""" + for key in CarlaDataProvider._actor_transform_map: + if key.id == actor.id: + # The velocity location information is the entire behavior tree updated every tick + # The ego vehicle is created before the behavior tree tick, so exception handling needs to be added + if CarlaDataProvider._actor_transform_map[key] is None: + return actor.get_transform() + return CarlaDataProvider._actor_transform_map[key] + + # We are intentionally not throwing here + # This may cause exception loops in py_trees + print("{}.get_transform: {} not found!".format(__name__, actor)) + return None + + @staticmethod + def get_random_seed(): + """Return the random seed.""" + return CarlaDataProvider._rng + + @staticmethod + def set_client(client): + """Set the CARLA client.""" + CarlaDataProvider._client = client + + @staticmethod + def get_client(): + """Get the CARLA client.""" + return CarlaDataProvider._client + + @staticmethod + def set_world(world): + """Set the world and world settings.""" + CarlaDataProvider._world = world + CarlaDataProvider._sync_flag = world.get_settings().synchronous_mode + CarlaDataProvider._map = world.get_map() + CarlaDataProvider._blueprint_library = world.get_blueprint_library() + CarlaDataProvider.generate_spawn_points() + CarlaDataProvider.prepare_map() + + @staticmethod + def get_world(): + """Return world.""" + return CarlaDataProvider._world + + @staticmethod + def get_map(world=None): + """Get the current map.""" + if CarlaDataProvider._map is None: + if world is None: + if CarlaDataProvider._world is None: + raise ValueError("class member 'world'' not initialized yet") + else: + CarlaDataProvider._map = CarlaDataProvider._world.get_map() + else: + CarlaDataProvider._map = world.get_map() + + return CarlaDataProvider._map + + @staticmethod + def get_all_actors(): + """Return all the world actors.""" + if CarlaDataProvider._all_actors: + return CarlaDataProvider._all_actors + + CarlaDataProvider._all_actors = CarlaDataProvider._world.get_actors() + return CarlaDataProvider._all_actors + + @staticmethod + def set_runtime_init_mode(flag): + """Set the runtime init mode.""" + CarlaDataProvider._runtime_init_flag = flag + + @staticmethod + def is_runtime_init_mode(): + """Return true if runtime init mode is used.""" + return CarlaDataProvider._runtime_init_flag + + @staticmethod + def find_weather_presets(): + """Get weather presets from CARLA.""" + rgx = re.compile(".+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)") + + def format_name(x): + return " ".join(m.group(0) for m in rgx.finditer(x)) + + presets = [x for x in dir(carla.WeatherParameters) if re.match("[A-Z].+", x)] + return [(getattr(carla.WeatherParameters, x), format_name(x)) for x in presets] + + @staticmethod + def prepare_map(): + """Set the current map and loads all traffic lights for this map to_traffic_light_map.""" + if CarlaDataProvider._map is None: + CarlaDataProvider._map = CarlaDataProvider._world.get_map() + + # Parse all traffic lights + CarlaDataProvider._traffic_light_map.clear() + for traffic_light in CarlaDataProvider._world.get_actors().filter("*traffic_light*"): + if traffic_light not in list(CarlaDataProvider._traffic_light_map): + CarlaDataProvider._traffic_light_map[traffic_light] = traffic_light.get_transform() + else: + raise KeyError( + "Traffic light '{}' already registered. Cannot register twice!".format( + traffic_light.id + ) + ) + + @staticmethod + def generate_spawn_points(): + """Generate spawn points for the current map.""" + spawn_points = list(CarlaDataProvider.get_map(CarlaDataProvider._world).get_spawn_points()) + CarlaDataProvider._rng.shuffle(spawn_points) + CarlaDataProvider._spawn_points = spawn_points + CarlaDataProvider._spawn_index = 0 + + @staticmethod + def check_road_length(wp, length: float): + waypoint_separation = 5 + + cur_len = 0 + road_id, lane_id = wp.road_id, wp.lane_id + while True: + wps = wp.next(waypoint_separation) + # The same road_id and lane_id，judged to be in the same section to be tested + next_wp = None + for p in wps: + if p.road_id == road_id and p.lane_id == lane_id: + next_wp = p + break + if next_wp is None: + break + cur_len += waypoint_separation + if cur_len >= length: + return True + wp = next_wp + return False + + @staticmethod + def get_road_lanes(wp): + if wp.is_junction: + return [] + # find the most left lane's waypoint + + lane_id_set = set() + pre_left = wp + while wp and wp.lane_type == carla.LaneType.Driving: + if wp.lane_id in lane_id_set: + break + lane_id_set.add(wp.lane_id) + + # carla bug: get_left_lane Return error，and never Return none. It's a infinite loop. + pre_left = wp + wp = wp.get_left_lane() + + # # Store data from the left lane to the right lane + # # list, key=laneid, value=waypoint + lane_list = [] + lane_id_set.clear() + wp = pre_left + while wp and wp.lane_type == carla.LaneType.Driving: + if wp.lane_id in lane_id_set: + break + lane_id_set.add(wp.lane_id) + + lane_list.append(wp) + + # carla bug: Return error, never return none, endless loop + wp = wp.get_right_lane() + + return lane_list + + @staticmethod + def get_road_lane_cnt(wp): + lanes = CarlaDataProvider.get_road_lanes(wp) + return len(lanes) + + @staticmethod + def get_waypoint_by_laneid(lane_num: int): + if CarlaDataProvider._spawn_points is None: + CarlaDataProvider.generate_spawn_points() + + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print("No more spawn points to use") + return None + else: + pos = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + wp = CarlaDataProvider.get_map().get_waypoint( + pos.location, project_to_road=True, lane_type=carla.LaneType.Driving + ) + + road_lanes = CarlaDataProvider.get_road_lanes(wp) + + lane = int(float(lane_num)) + if lane > len(road_lanes): + return None + else: + return road_lanes[lane - 1] + + # cspell:ignore rolename + @staticmethod + def create_blueprint( + model, rolename="scenario", color=None, actor_category="car", attribute_filter=None + ): + """Set up the blueprint of an actor given its model and other relevant parameters.""" + + def check_attribute_value(blueprint, name, value): + """Check if the blueprint has that attribute with that value.""" + if not blueprint.has_attribute(name): + return False + + attribute_type = blueprint.get_attribute(key).type + if attribute_type == carla.ActorAttributeType.Bool: + return blueprint.get_attribute(name).as_bool() == value + elif attribute_type == carla.ActorAttributeType.Int: + return blueprint.get_attribute(name).as_int() == value + elif attribute_type == carla.ActorAttributeType.Float: + return blueprint.get_attribute(name).as_float() == value + elif attribute_type == carla.ActorAttributeType.String: + return blueprint.get_attribute(name).as_str() == value + + return False + + # cspell:ignore carlacola carlamotors + _actor_blueprint_categories = { + "car": "vehicle.tesla.model3", + "van": "vehicle.volkswagen.t2", + "truck": "vehicle.carlamotors.carlacola", + "trailer": "", + "semitrailer": "", + "bus": "vehicle.volkswagen.t2", + "motorbike": "vehicle.kawasaki.ninja", + "bicycle": "vehicle.diamondback.century", + "train": "", + "tram": "", + "pedestrian": "walker.pedestrian.0001", + } + + # Set the model + try: + blueprints = CarlaDataProvider._blueprint_library.filter(model) + if attribute_filter is not None: + for key, value in attribute_filter.items(): + blueprints = [x for x in blueprints if check_attribute_value(x, key, value)] + + blueprint = CarlaDataProvider._rng.choice(blueprints) + except ValueError: + # The model is not part of the blueprint library. Let's take a default one for the given category + bp_filter = "vehicle.*" + new_model = _actor_blueprint_categories[actor_category] + if new_model != "": + bp_filter = new_model + print( + "WARNING: Actor model {} not available. Using instead {}".format(model, new_model) + ) + blueprint = CarlaDataProvider._rng.choice( + CarlaDataProvider._blueprint_library.filter(bp_filter) + ) + + # Set the color + if color: + if not blueprint.has_attribute("color"): + print( + "WARNING: Cannot set Color ({}) for actor {} due to missing blueprint attribute".format( + color, blueprint.id + ) + ) + else: + default_color_rgba = blueprint.get_attribute("color").as_color() + default_color = "({}, {}, {})".format( + default_color_rgba.r, default_color_rgba.g, default_color_rgba.b + ) + try: + blueprint.set_attribute("color", color) + except ValueError: + # Color can't be set for this vehicle + print( + "WARNING: Color ({}) cannot be set for actor {}. Using instead: ({})".format( + color, blueprint.id, default_color + ) + ) + blueprint.set_attribute("color", default_color) + else: + if blueprint.has_attribute("color") and rolename != "hero": + color = CarlaDataProvider._rng.choice( + blueprint.get_attribute("color").recommended_values + ) + blueprint.set_attribute("color", color) + + # Make pedestrians mortal + if blueprint.has_attribute("is_invincible"): + blueprint.set_attribute("is_invincible", "false") + + # Set the rolename + if blueprint.has_attribute("role_name"): + blueprint.set_attribute("role_name", rolename) + + return blueprint + + @staticmethod + def handle_actor_batch(batch, tick=True): + """Forward a CARLA command batch to spawn actors to CARLA, and gather the responses.""" + sync_mode = CarlaDataProvider.is_sync_mode() + actors = [] + + if CarlaDataProvider._client: + responses = CarlaDataProvider._client.apply_batch_sync(batch, sync_mode and tick) + else: + raise ValueError("class member 'client'' not initialized yet") + + # Wait (or not) for the actors to be spawned properly before we do anything + if not tick: + pass + elif CarlaDataProvider.is_runtime_init_mode(): + CarlaDataProvider._world.wait_for_tick() + elif sync_mode: + CarlaDataProvider._world.tick() + else: + CarlaDataProvider._world.wait_for_tick() + + actor_ids = [r.actor_id for r in responses if not r.error] + for r in responses: + if r.error: + print("WARNING: Not all actors were spawned") + break + actors = list(CarlaDataProvider._world.get_actors(actor_ids)) + return actors + + @staticmethod + def request_new_actor( + model, + spawn_point, + rolename="scenario", + autopilot=False, + random_location=False, + color=None, + actor_category="car", + attribute_filter=None, + tick=True, + ): + """Create a new actor, returning it if successful (None otherwise).""" + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, color, actor_category, attribute_filter + ) + + if random_location: + actor = None + while not actor: + spawn_point = CarlaDataProvider._rng.choice(CarlaDataProvider._spawn_points) + actor = CarlaDataProvider._world.try_spawn_actor(blueprint, spawn_point) + + else: + # For non prop models, slightly lift the actor to avoid collisions with the ground + z_offset = 0.2 if "prop" not in model else 0 + + # DO NOT USE spawn_point directly, as this will modify spawn_point permanently + _spawn_point = carla.Transform(carla.Location(), spawn_point.rotation) + _spawn_point.location.x = spawn_point.location.x + _spawn_point.location.y = spawn_point.location.y + _spawn_point.location.z = spawn_point.location.z + z_offset + actor = CarlaDataProvider._world.try_spawn_actor(blueprint, _spawn_point) + + if actor is None: + print( + "WARNING: Cannot spawn actor {} at position {}".format(model, spawn_point.location) + ) + return None + + # De/activate the autopilot of the actor if it belongs to vehicle + if autopilot: + if isinstance(actor, carla.Vehicle): + actor.set_autopilot(autopilot, CarlaDataProvider._traffic_manager_port) + else: + print("WARNING: Tried to set the autopilot of a non vehicle actor") + + # Wait for the actor to be spawned properly before we do anything + if not tick: + pass + elif CarlaDataProvider.is_runtime_init_mode(): + CarlaDataProvider._world.wait_for_tick() + elif CarlaDataProvider.is_sync_mode(): + CarlaDataProvider._world.tick() + else: + CarlaDataProvider._world.wait_for_tick() + + if actor is None: + return None + + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, spawn_point) + return actor + + @staticmethod + def request_new_actors(actor_list, attribute_filter=None, tick=True): + """Series of actor in batch. If this was successful, the new actors are returned, None otherwise.""" + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + PhysicsCommand = carla.command.SetSimulatePhysics # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + ApplyTransform = carla.command.ApplyTransform # pylint: disable=invalid-name + SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name + SetVehicleLightState = carla.command.SetVehicleLightState # pylint: disable=invalid-name + + batch = [] + actors = [] + + CarlaDataProvider.generate_spawn_points() + + for actor in actor_list: + # Get the blueprint + blueprint = CarlaDataProvider.create_blueprint( + actor.model, actor.rolename, actor.color, actor.category, attribute_filter + ) + + # Get the spawn point + transform = actor.transform + if not transform: + continue + if actor.random_location: + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print("No more spawn points to use") + break + else: + _spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + + else: + _spawn_point = carla.Transform() + _spawn_point.rotation = transform.rotation + _spawn_point.location.x = transform.location.x + _spawn_point.location.y = transform.location.y + if blueprint.has_tag("walker"): + # On imported OpenDRIVE maps, spawning of pedestrians can fail. + # By increasing the z-value the chances of success are increased. + map_name = CarlaDataProvider._map.name.split("/")[-1] + if not map_name.startswith("OpenDrive"): + _spawn_point.location.z = transform.location.z + 0.2 + else: + _spawn_point.location.z = transform.location.z + 0.8 + else: + _spawn_point.location.z = transform.location.z + 0.2 + + # Get the command + command = SpawnActor(blueprint, _spawn_point) + command.then( + SetAutopilot(FutureActor, actor.autopilot, CarlaDataProvider._traffic_manager_port) + ) + + if ( + actor.args is not None + and "physics" in actor.args + and actor.args["physics"] == "off" + ): + command.then(ApplyTransform(FutureActor, _spawn_point)).then( + PhysicsCommand(FutureActor, False) + ) + elif actor.category == "misc": + command.then(PhysicsCommand(FutureActor, True)) + if actor.args is not None and "lights" in actor.args and actor.args["lights"] == "on": + command.then(SetVehicleLightState(FutureActor, carla.VehicleLightState.All)) + + batch.append(command) + + actors = CarlaDataProvider.handle_actor_batch(batch, tick) + + if not actors: + return None + + for actor in actors: + if actor is None: + continue + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, _spawn_point) + + return actors + + @staticmethod + def request_new_batch_actors( + model, + amount, + spawn_points, + autopilot=False, + random_location=False, + rolename="scenario", + attribute_filter=None, + tick=True, + ): + SpawnActor = carla.command.SpawnActor # pylint: disable=invalid-name + SetAutopilot = carla.command.SetAutopilot # pylint: disable=invalid-name + FutureActor = carla.command.FutureActor # pylint: disable=invalid-name + + CarlaDataProvider.generate_spawn_points() + + batch = [] + + for i in range(amount): + # Get vehicle by model + blueprint = CarlaDataProvider.create_blueprint( + model, rolename, attribute_filter=attribute_filter + ) + + if random_location: + if CarlaDataProvider._spawn_index >= len(CarlaDataProvider._spawn_points): + print( + "No more spawn points to use. Spawned {} actors out of {}".format( + i + 1, amount + ) + ) + break + else: + spawn_point = CarlaDataProvider._spawn_points[ + CarlaDataProvider._spawn_index + ] # pylint: disable=unsubscriptable-object + CarlaDataProvider._spawn_index += 1 + else: + try: + spawn_point = spawn_points[i] + except IndexError: + print("The amount of spawn points is lower than the amount of vehicles spawned") + break + + if spawn_point: + batch.append( + SpawnActor(blueprint, spawn_point).then( + SetAutopilot( + FutureActor, autopilot, CarlaDataProvider._traffic_manager_port + ) + ) + ) + + actors = CarlaDataProvider.handle_actor_batch(batch, tick) + for actor in actors: + if actor is None: + continue + CarlaDataProvider._carla_actor_pool[actor.id] = actor + CarlaDataProvider.register_actor(actor, spawn_point) + + return actors + + @staticmethod + def get_actors(): + """Return list of actors and their ids.""" + return iteritems(CarlaDataProvider._carla_actor_pool) + + @staticmethod + def actor_id_exists(actor_id): + """Check if a certain id is still at the simulation.""" + if actor_id in CarlaDataProvider._carla_actor_pool: + return True + + return False + + @staticmethod + def get_hero_actor(): + """Get the actor object of the hero actor if it exists, Return none otherwise.""" + for actor_id in CarlaDataProvider._carla_actor_pool: + if CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] == "hero": + return CarlaDataProvider._carla_actor_pool[actor_id] + return None + + @staticmethod + def get_actor_by_id(actor_id): + """Get an actor from the pool by using its ID. If the actor does not exist, None is returned.""" + print(CarlaDataProvider._carla_actor_pool) + if actor_id in CarlaDataProvider._carla_actor_pool: + return CarlaDataProvider._carla_actor_pool[actor_id] + + print("Non-existing actor id {}".format(actor_id)) + return None + + @staticmethod + def get_actor_by_name(role_name: str): + for actor_id in CarlaDataProvider._carla_actor_pool: + if CarlaDataProvider._carla_actor_pool[actor_id].attributes["role_name"] == role_name: + return CarlaDataProvider._carla_actor_pool[actor_id] + print(f"Non-existing actor name {role_name}") + return None + + @staticmethod + def remove_actor_by_id(actor_id): + """Remove an actor from the pool using its ID.""" + if actor_id in CarlaDataProvider._carla_actor_pool: + CarlaDataProvider._carla_actor_pool[actor_id].destroy() + CarlaDataProvider._carla_actor_pool[actor_id] = None + CarlaDataProvider._carla_actor_pool.pop(actor_id) + else: + print("Trying to remove a non-existing actor id {}".format(actor_id)) + + @staticmethod + def is_sync_mode(): + """Return true if synchronous mode is used.""" + return CarlaDataProvider._sync_flag + + @staticmethod + def remove_actors_in_surrounding(location, distance): + """Remove all actors from the pool that are closer than distance to the provided location.""" + for actor_id in CarlaDataProvider._carla_actor_pool.copy(): + if ( + CarlaDataProvider._carla_actor_pool[actor_id].get_location().distance(location) + < distance + ): + CarlaDataProvider._carla_actor_pool[actor_id].destroy() + CarlaDataProvider._carla_actor_pool.pop(actor_id) + + # Remove all keys with None values + CarlaDataProvider._carla_actor_pool = dict( + {k: v for k, v in CarlaDataProvider._carla_actor_pool.items() if v} + ) + + @staticmethod + def get_traffic_manager_port(): + """Get the port of the traffic manager.""" + return CarlaDataProvider._traffic_manager_port + + @staticmethod + def set_traffic_manager_port(tm_port): + """Set the port to use for the traffic manager.""" + CarlaDataProvider._traffic_manager_port = tm_port + + @staticmethod + def cleanup(): + """Cleanup and remove all entries from all dictionaries.""" + DestroyActor = carla.command.DestroyActor # pylint: disable=invalid-name + batch = [] + + for actor_id in CarlaDataProvider._carla_actor_pool.copy(): + actor = CarlaDataProvider._carla_actor_pool[actor_id] + if actor is not None and actor.is_alive: + batch.append(DestroyActor(actor)) + + if CarlaDataProvider._client: + try: + CarlaDataProvider._client.apply_batch_sync(batch) + except RuntimeError as e: + if "time-out" in str(e): + pass + else: + raise e + + CarlaDataProvider._actor_velocity_map.clear() + CarlaDataProvider._actor_location_map.clear() + CarlaDataProvider._actor_transform_map.clear() + CarlaDataProvider._traffic_light_map.clear() + CarlaDataProvider._map = None + CarlaDataProvider._world = None + CarlaDataProvider._sync_flag = False + CarlaDataProvider._ego_vehicle_route = None + CarlaDataProvider._all_actors = None + CarlaDataProvider._carla_actor_pool = {} + CarlaDataProvider._client = None + CarlaDataProvider._spawn_points = None + CarlaDataProvider._spawn_index = 0 + CarlaDataProvider._rng = random.RandomState(CarlaDataProvider._random_seed) + CarlaDataProvider._runtime_init_flag = False + + @property + def world(self): + return self._world + + +class GameTime(object): + """Provides access to the CARLA game time.""" + + _current_game_time = 0.0 # Elapsed game time after starting this Timer + _carla_time = 0.0 + _last_frame = 0 + _platform_timestamp = 0 + _init = False + + @staticmethod + def on_carla_tick(timestamp): + """Handle the callback receiving the CARLA time. Update time only when the frame is more recent than the last frame.""" + if GameTime._last_frame < timestamp.frame: + frames = timestamp.frame - GameTime._last_frame if GameTime._init else 1 + GameTime._current_game_time += timestamp.delta_seconds * frames + GameTime._last_frame = timestamp.frame + GameTime._platform_timestamp = datetime.datetime.now() + GameTime._init = True + GameTime._carla_time = timestamp.elapsed_seconds + + @staticmethod + def restart(): + """Reset game timer to 0.""" + GameTime._current_game_time = 0.0 + GameTime._carla_time = 0.0 + GameTime._last_frame = 0 + GameTime._init = False + + @staticmethod + def get_time(): + """Return elapsed game time.""" + return GameTime._current_game_time + + @staticmethod + def get_carla_time(): + """Return elapsed game time.""" + return GameTime._carla_time + + @staticmethod + def get_wall_clock_time(): + """Return elapsed game time.""" + return GameTime._platform_timestamp + + @staticmethod + def get_frame(): + """Return elapsed game time.""" + return GameTime._last_frame diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py new file mode 100644 index 0000000000000..72eb176515f73 --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_utils.py @@ -0,0 +1,109 @@ +import ctypes +import math +import struct +import sys + +import carla +from geometry_msgs.msg import Point +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from transforms3d.euler import euler2quat +from transforms3d.euler import quat2euler + + +def _get_struct_fmt(is_bigendian, fields, field_names=None): + _DATATYPES = {} + _DATATYPES[PointField.INT8] = ("b", 1) + _DATATYPES[PointField.UINT8] = ("B", 1) + _DATATYPES[PointField.INT16] = ("h", 2) + _DATATYPES[PointField.UINT16] = ("H", 2) + _DATATYPES[PointField.INT32] = ("i", 4) + _DATATYPES[PointField.UINT32] = ("I", 4) + _DATATYPES[PointField.FLOAT32] = ("f", 4) + _DATATYPES[PointField.FLOAT64] = ("d", 8) + + fmt = ">" if is_bigendian else "<" + + offset = 0 + for field in ( + f + for f in sorted(fields, key=lambda f: f.offset) + if field_names is None or f.name in field_names + ): + if offset < field.offset: + fmt += "x" * (field.offset - offset) + offset = field.offset + if field.datatype not in _DATATYPES: + print("Skipping unknown PointField datatype [{}]" % field.datatype, file=sys.stderr) + else: + datatype_fmt, datatype_length = _DATATYPES[field.datatype] + fmt += field.count * datatype_fmt + offset += field.count * datatype_length + + return fmt + + +def create_cloud(header, fields, points): + """Create a L{sensor_msgs.msg.PointCloud2} message with different datatype (Modified create_cloud function).""" + cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) + + buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) + + point_step, pack_into = cloud_struct.size, cloud_struct.pack_into + offset = 0 + for p in points: + pack_into(buff, offset, *p) + offset += point_step + + return PointCloud2( + header=header, + height=1, + width=len(points), + is_dense=False, + is_bigendian=False, + fields=fields, + point_step=cloud_struct.size, + row_step=cloud_struct.size * len(points), + data=buff.raw, + ) + + +def carla_location_to_ros_point(carla_location): + """Convert a carla location to a ROS point.""" + ros_point = Point() + ros_point.x = carla_location.x + ros_point.y = -carla_location.y + ros_point.z = carla_location.z + + return ros_point + + +def carla_rotation_to_ros_quaternion(carla_rotation): + """Convert a carla rotation to a ROS quaternion.""" + roll = math.radians(carla_rotation.roll) + pitch = -math.radians(carla_rotation.pitch) + yaw = -math.radians(carla_rotation.yaw) + quat = euler2quat(roll, pitch, yaw) + ros_quaternion = Quaternion(w=quat[0], x=quat[1], y=quat[2], z=quat[3]) + + return ros_quaternion + + +def ros_quaternion_to_carla_rotation(ros_quaternion): + """Convert ROS quaternion to carla rotation.""" + roll, pitch, yaw = quat2euler( + [ros_quaternion.w, ros_quaternion.x, ros_quaternion.y, ros_quaternion.z] + ) + + return carla.Rotation( + roll=math.degrees(roll), pitch=-math.degrees(pitch), yaw=-math.degrees(yaw) + ) + + +def ros_pose_to_carla_transform(ros_pose): + """Convert ROS pose to carla transform.""" + return carla.Transform( + carla.Location(ros_pose.position.x, -ros_pose.position.y, ros_pose.position.z), + ros_quaternion_to_carla_rotation(ros_pose.orientation), + ) diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py new file mode 100644 index 0000000000000..ed036a47ea9fe --- /dev/null +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/modules/carla_wrapper.py @@ -0,0 +1,231 @@ +# Copyright 2024 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License.sr/bin/env python + +from __future__ import print_function + +import logging +from queue import Empty +from queue import Queue + +import carla +import numpy as np + +from .carla_data_provider import CarlaDataProvider + + +# Sensor Wrapper for Agent +class SensorReceivedNoData(Exception): + """Exceptions when no data received from the sensors.""" + + +class GenericMeasurement(object): + def __init__(self, data, frame): + self.data = data + self.frame = frame + + +class CallBack(object): + def __init__(self, tag, sensor, data_provider): + self._tag = tag + self._data_provider = data_provider + + self._data_provider.register_sensor(tag, sensor) + + def __call__(self, data): + if isinstance(data, carla.Image): + self._parse_image_cb(data, self._tag) + elif isinstance(data, carla.LidarMeasurement): + self._parse_lidar_cb(data, self._tag) + elif isinstance(data, carla.GnssMeasurement): + self._parse_gnss_cb(data, self._tag) + elif isinstance(data, carla.IMUMeasurement): + self._parse_imu_cb(data, self._tag) + elif isinstance(data, GenericMeasurement): + self._parse_pseudo_sensor(data, self._tag) + else: + logging.error("No callback method for this sensor.") + + # Parsing CARLA physical Sensors + def _parse_image_cb(self, image, tag): + self._data_provider.update_sensor(tag, image, image.frame) + + def _parse_lidar_cb(self, lidar_data, tag): + self._data_provider.update_sensor(tag, lidar_data, lidar_data.frame) + + def _parse_imu_cb(self, imu_data, tag): + self._data_provider.update_sensor(tag, imu_data, imu_data.frame) + + def _parse_gnss_cb(self, gnss_data, tag): + array = np.array( + [gnss_data.latitude, gnss_data.longitude, gnss_data.altitude], dtype=np.float64 + ) + self._data_provider.update_sensor(tag, array, gnss_data.frame) + + def _parse_pseudo_sensor(self, package, tag): + self._data_provider.update_sensor(tag, package.data, package.frame) + + +class SensorInterface(object): + def __init__(self): + self._sensors_objects = {} + self._new_data_buffers = Queue() + self._queue_timeout = 10 + self.tag = "" + + def register_sensor(self, tag, sensor): + self.tag = tag + if tag in self._sensors_objects: + raise ValueError(f"Duplicated sensor tag [{tag}]") + + self._sensors_objects[tag] = sensor + + def update_sensor(self, tag, data, timestamp): + if tag not in self._sensors_objects: + raise ValueError(f"Sensor with tag [{tag}] has not been created") + + self._new_data_buffers.put((tag, timestamp, data)) + + def get_data(self): + try: + data_dict = {} + while len(data_dict.keys()) < len(self._sensors_objects.keys()): + sensor_data = self._new_data_buffers.get(True, self._queue_timeout) + data_dict[sensor_data[0]] = (sensor_data[1], sensor_data[2]) + except Empty: + raise SensorReceivedNoData( + f"Sensor with tag [{self.tag}] took too long to send its data" + ) + + return data_dict + + +# Sensor Wrapper + + +class SensorWrapper(object): + _agent = None + _sensors_list = [] + + def __init__(self, agent): + self._agent = agent + + def __call__(self): + return self._agent() + + def setup_sensors(self, vehicle, debug_mode=False): + """Create and attach the sensor defined in objects.json.""" + bp_library = CarlaDataProvider.get_world().get_blueprint_library() + + for sensor_spec in self._agent.sensors["sensors"]: + bp = bp_library.find(str(sensor_spec["type"])) + + if sensor_spec["type"].startswith("sensor.camera"): + bp.set_attribute("image_size_x", str(sensor_spec["image_size_x"])) + bp.set_attribute("image_size_y", str(sensor_spec["image_size_y"])) + bp.set_attribute("fov", str(sensor_spec["fov"])) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.lidar"): + bp.set_attribute("range", str(sensor_spec["range"])) + bp.set_attribute("rotation_frequency", str(sensor_spec["rotation_frequency"])) + bp.set_attribute("channels", str(sensor_spec["channels"])) + bp.set_attribute("upper_fov", str(sensor_spec["upper_fov"])) + bp.set_attribute("lower_fov", str(sensor_spec["lower_fov"])) + bp.set_attribute("points_per_second", str(sensor_spec["points_per_second"])) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.other.gnss"): + bp.set_attribute("noise_alt_stddev", str(0.0)) + bp.set_attribute("noise_lat_stddev", str(0.0)) + bp.set_attribute("noise_lon_stddev", str(0.0)) + bp.set_attribute("noise_alt_bias", str(0.0)) + bp.set_attribute("noise_lat_bias", str(0.0)) + bp.set_attribute("noise_lon_bias", str(0.0)) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.other.imu"): + bp.set_attribute("noise_accel_stddev_x", str(0.0)) + bp.set_attribute("noise_accel_stddev_y", str(0.0)) + bp.set_attribute("noise_accel_stddev_z", str(0.0)) + bp.set_attribute("noise_gyro_stddev_x", str(0.0)) + bp.set_attribute("noise_gyro_stddev_y", str(0.0)) + bp.set_attribute("noise_gyro_stddev_z", str(0.0)) + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"], + roll=sensor_spec["spawn_point"]["roll"], + yaw=sensor_spec["spawn_point"]["yaw"], + ) + + elif sensor_spec["type"].startswith("sensor.pseudo.*"): + sensor_location = carla.Location( + x=sensor_spec["spawn_point"]["x"], + y=sensor_spec["spawn_point"]["y"], + z=sensor_spec["spawn_point"]["z"], + ) + sensor_rotation = carla.Rotation( + pitch=sensor_spec["spawn_point"]["pitch"] + 0.001, + roll=sensor_spec["spawn_point"]["roll"] - 0.015, + yaw=sensor_spec["spawn_point"]["yaw"] + 0.0364, + ) + + # create sensor + sensor_transform = carla.Transform(sensor_location, sensor_rotation) + sensor = CarlaDataProvider.get_world().spawn_actor(bp, sensor_transform, vehicle) + # setup callback + sensor.listen(CallBack(sensor_spec["id"], sensor, self._agent.sensor_interface)) + self._sensors_list.append(sensor) + + # Tick once to spawn the sensors + CarlaDataProvider.get_world().tick() + + def cleanup(self): + """Cleanup sensors.""" + for i, _ in enumerate(self._sensors_list): + if self._sensors_list[i] is not None: + self._sensors_list[i].stop() + self._sensors_list[i].destroy() + self._sensors_list[i] = None + self._sensors_list = []