Merge pull request #109 from Intelligent-Testing-Lab/108-migrate-autoware_carla_interface-into-cawsr

108 migrate autoware carla interface into cawsr

Author: david-gasinski

cawsr.py

@@ -62,9 +62,7 @@
 
 
 class AWScenarioRunner(object):
-    # global class instances
-
-    aw_agent = None
+    aw_agent = None  # autoware agent
 
     def __init__(self, cawsr_config: dict, carla_conf: CARLA) -> None:
         """
@@ -147,6 +145,7 @@ def run_scenario(
         self.carla_client.load_world(env_config.town)
 
         logger.info("Updating world settings:")
+
         # tick asynchronously until then
         settings = self.carla_world.get_settings()
         settings.synchronous_mode = True
@@ -160,8 +159,9 @@ def run_scenario(
 
         logger.info("Spawning ego...")
         ego = EgoVehicle(env_config)
-        self.ego_vehicles.append(ego.spawn())
-        logger.info("Spawned ego...")
+        actor = ego.spawn()
+        self.ego_vehicles.append(actor)
+        logger.info(f"Spawned ego with id: {actor.id}")
 
         self.carla_world.tick()  # client must tick to spawn actors
 

srunner/autoagents/autoware_agent.py

@@ -14,14 +14,17 @@
 from srunner.autoagents.autoware_nodes import tick_node
 
 from srunner.autoagents.agent_state import autoware_state
-
 from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig
+from srunner.autoagents.autoware_carla_interface.carla_autoware import (
+    InitializeInterface,
+)
 
 import threading
 import rclpy
 import time
 import logging
 
+
 logger = logging.getLogger("scenario-runner")
 logger.propagate = False
 
@@ -32,7 +35,7 @@ class AutowareAgent(AutonomousAgent):
     counter = 0
     last_tick = time.perf_counter_ns()
 
-    def setup(self, config: EnvironmentConfig | None = None) -> None:
+    def setup(self, config: EnvironmentConfig) -> None:
         """Setup the Autoware Agent.
             - Initialise the state
             - Setup nodes
@@ -46,6 +49,8 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         self.autoware_state = autoware_state.AutowareState("ego_vehicle", None)
 
+        self.carla_interface = InitializeInterface(self.config)
+
         self.state_node = state_node.StateNode(self.autoware_state)
         self.state_node.reset_autoware(self.config.town, self.config.ego_name)
 
@@ -57,6 +62,7 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
         self._multi_thread_executor.add_node(self.route_node)
         self._multi_thread_executor.add_node(self.state_node)
         self._multi_thread_executor.add_node(self.autoware_node)
+        self._multi_thread_executor.add_node(self.carla_interface.interface.ros2_node)  # type:ignore
 
         self._executor_thread = threading.Thread(
             target=self._multi_thread_executor.spin, daemon=True
@@ -65,9 +71,12 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         self.sent_route = False
 
-        self.setup_tick_service()
+        self.carla_interface.load_world()
+        self.carla_interface.run_bridge()
+        # self.setup_tick_service()
 
     def setup_tick_service(self):
+        # no need to setup tick service anymore
         self.tick_node = tick_node.TickNode()
         self._tick_executor = rclpy.executors.SingleThreadedExecutor()
 
@@ -157,7 +166,7 @@ def run_step_init(self) -> bool:
         if self.autoware_state.route_set() and not self.autoware_state.sent_engage:
             return True
 
-        self.tick_node.autoware_tick()
+        self.carla_interface.tick_bridge()
         return False
 
     def run_step(self) -> None:
@@ -173,5 +182,4 @@ def run_step(self) -> None:
         if self.autoware_state.route_set() and not self.autoware_state.sent_engage:
             self.autoware_node.publish_engage(True)
 
-        # tick autoware
-        self.tick_node.autoware_tick()
+        self.carla_interface.tick_bridge()

srunner/autoagents/autoware_carla_interface/_init__.py

@@ -0,0 +1,117 @@
+# 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 signal
+import time
+
+
+from srunner.autoagents.autoware_carla_interface.modules.carla_wrapper import (
+    SensorReceivedNoData,
+)
+from srunner.autoagents.autoware_carla_interface.modules.carla_wrapper import (
+    SensorWrapper,
+)
+from srunner.autoagents.autoware_carla_interface.carla_ros import carla_ros2_interface
+from srunner.scenariomanager.carla_data_provider import CarlaDataProvider
+from srunner.scenariomanager.timer import GameTime
+
+from srunner.scenarioconfigs.environment_configuration import EnvironmentConfig
+
+
+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
+            try:
+                ego_action = self.sensor()
+            except SensorReceivedNoData as e:
+                raise RuntimeError(e)
+            self.ego_actor.apply_control(ego_action)
+
+
+class InitializeInterface(object):
+    def __init__(self, config: EnvironmentConfig):
+        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
+
+        self.config = config
+
+    def load_world(self):
+        self.ego_actor = CarlaDataProvider.get_actor_by_name(self.config.ego_name)
+        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)
+
+    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
+
+    def tick_bridge(self):
+        timestamp = None
+        world = CarlaDataProvider.get_world()
+        if world:
+            snapshot = world.get_snapshot()
+            if snapshot:
+                timestamp = snapshot.timestamp
+        if timestamp:
+            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()