Add traffic manager option and Improve README explanation

Author: mraditya01

simulator/carla_autoware/README.md

@@ -24,13 +24,12 @@ This ros package enables communication between Autoware and CARLA for autonomous
 
 1. Download maps (y-axis inverted version) to arbitaly 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` and add `projector_type: local` on the first line.
+3. Create `map_projector_info.yaml` on the folder and add `projector_type: local` on the first line.
 
 ### Build
 
 ```bash
-cd colcon_ws
-colcon build --symlink-install
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
 ### Run
@@ -53,15 +52,89 @@ colcon build --symlink-install
 5. Wait for planning
 6. Engage
 
-### Tips
+## Inner-workings / Algorithms
+
+The main class responsible for setting up the CARLA world and the ego vehicle is InitializeInterface. The configuration parameters are fetched using the carla_interface.
+
+### Configureable Parameters for World Loading
+
+All the key parameters can be configured in `carla_autoware.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 carla_autoware)/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                                                                                                                        |
+
+### 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. |
+| `self.max_steering_angle` | int  | 45                                                                                     | (Line 74) Maximum steering angle of a vehicle in degrees, used as parameter to compute the mapping from normalized steering input values.                                                                                         |
+
+- 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 CALRA 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)
+   ```
+
+## Tips
 
-- If you want to edit the sensors configuration used in CARLA, edit `objects.json` located in `carla_autoware/config`. Make sure the sensor configuration is the same as the `sensor_kit` description used in Autoware.
 - Misalignment might occurs during initialization, pressing `init by gnss` button should fix it.
-- Sensor frequency can be changed on `carla_ros.py` Line 67.
-- Changing the `fixed_delta_seconds` can increase the simulation tick (clock), some sensors params in `objects.json` need to be adjusted when it is changed (example: LIDAR rotation frequency have to match the FPS).
+- 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
+## 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 configurated 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 (`carla_autoware.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.
+- Ego Vehicle Control.
+  - Currently during sharp turning the ego-vehicle have large lateral error.

simulator/carla_autoware/launch/carla_autoware.launch.xml

@@ -4,14 +4,12 @@
   <arg name="timeout" default="20"/>
   <arg name="ego_vehicle_role_name" default="ego_vehicle"/>
   <arg name="vehicle_type" default="vehicle.toyota.prius"/>
-  <arg name="spawn_point" default="None"/>
+  <arg name="spawn_point" default="None" description="location of ego vehicle spawn (default is random), example = [0.2 , 4.1, 0.4, 0., 0., 0.]"/>
   <arg name="carla_map" default="Town01"/>
-  <arg name="passive" default="False"/>
   <arg name="sync_mode" default="True"/>
   <arg name="fixed_delta_seconds" default="0.05" description="Timestep for CARLA, FPS=1/value"/>
-  <arg name="role_name" default="ego_vehicle"/>
-  <arg name="spawn_point" default="None" description="location of ego vehicle spawn (default is random), example = [0.2 , 4.1, 0.4, 0., 0., 0.]"/>
   <arg name="objects_definition_file" default="$(find-pkg-share carla_autoware)/objects.json"/>
+  <arg name="use_traffic_manager" default="False"/>
 
   <!-- CARLA Interface -->
   <node pkg="carla_autoware" exec="carla_autoware" name="carla_autoware" output="screen">
@@ -25,6 +23,7 @@
     <param name="spawn_point" value="$(var spawn_point)"/>
     <param name="vehicle_type" value="$(var vehicle_type)"/>
     <param name="objects_definition_file" value="$(var objects_definition_file)"/>
+    <param name="use_traffic_manager" value="$(var use_traffic_manager)"/>
   </node>
 
   <!-- Awsim configuration frame to CARLA frame -->

simulator/carla_autoware/src/carla_autoware/carla_autoware.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.sr/bin/env python
 
+import random
 import signal
 import time
 
@@ -66,15 +67,16 @@ def __init__(self):
         self.timeout = self.param_["timeout"]
         self.sync_mode = self.param_["sync_mode"]
         self.fixed_delta_seconds = self.param_["fixed_delta_seconds"]
-        self.map_name = self.param_["carla_map"]
+        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"]
 
     def load_world(self):
         client = carla.Client(self.local_host, self.port)
         client.set_timeout(self.timeout)
-        client.load_world(self.map_name)
+        client.load_world(self.carla_map)
         self.world = client.get_world()
         if self.world is not None:
             settings = self.world.get_settings()
@@ -90,7 +92,9 @@ def load_world(self):
             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.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])
@@ -103,6 +107,43 @@ def load_world(self):
             self.sensor_wrapper = SensorWrapper(self.interface)
             self.ego_vehicle = CarlaDataProvider.get_actor_by_name(self.agent_role_name)
             self.sensor_wrapper.setup_sensors(self.ego_vehicle, False)
+            ##########################################################################################################################################################
+            # TRAFFIC MANAGER
+            ##########################################################################################################################################################
+            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)
 
         else:
             print("Carla Interface Couldn't find the world, Carla is not Running")

simulator/carla_autoware/src/carla_autoware/carla_ros.py

@@ -93,6 +93,7 @@ def setup(self):
             "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,
         }
         self.param_values = {}
         for param_name, param_type in self.parameters.items():