added ROS service responsible for ticking Autoware

algorithms/hill_climbing.py → algorithms/hill_climb.py

@@ -1,11 +1,12 @@
 from basic_algorithm import BasicAlgorithm
 import lanelet2
 import random
+import json
 
 from math import sqrt, pow
 
 
-class Hill_Climb(BasicAlgorithm):
+class HillClimb(BasicAlgorithm):
     def __init__(self, args: dict) -> None:
         super(BasicAlgorithm).__init__()
         self.radius = args["radius"]
@@ -20,7 +21,7 @@ def _scenario_callback(
         self, scenario_definition: dict, driving_score: float
     ) -> dict:
         # pass in route id
-        waypoints = scenario_definition["routes"][0]["waypoints"]
+        waypoints = scenario_definition["routes"][0]["route"]["waypoints"]
 
         if self.prev_ds is not None:
             if not driving_score >= self.prev_ds:
@@ -78,9 +79,24 @@ def __get_all_lanelet_points(self) -> set[tuple[int, int]]:
         centerline_points = []
         for lanelet in list(lanelets):
             for points in lanelet.centerline:
-                centerline_points += (points.x, points.y)
+                centerline_points.append((points.x, points.y))
 
         return set(centerline_points)
 
     def __euclidian_distance(self, point1, point2) -> float:
-        return sqrt(pow(point1.x - point2.x, 2) + pow(point2.y - point1.x, 2))
+        return sqrt(pow(point1[0] - point2[0], 2) + pow(point2[1] - point1[1], 2))
+
+
+def main():  # debug
+    scenario = "/autoware_scenario_runner/example_scenario_default.json"
+
+    json_scenario = None
+    with open(scenario, "r") as f:
+        json_scenario = json.load(f)
+
+    radius = 10
+    lanelet2_path = "/autoware_scenario_runner/algorithms/resources/Town01.osm"
+    args = {"radius": radius, "lanelet_path": lanelet2_path}
+
+    climb = HillClimb(args)
+    climb._scenario_callback(json_scenario, 0.0)

aw_scenario_runner.py

@@ -14,6 +14,8 @@
 
 import carla
 
+import time
+
 from srunner.scenariomanager.scenario_manager import ScenarioManager
 from srunner.tools.results_manager import ScenarioDefinitionManager
 from srunner.scenarios.route_scenario import RouteScenario
@@ -24,11 +26,23 @@
 from srunner.scenarioconfigs.route_scenario_configuration import (
     RouteScenarioConfiguration,
 )
+from srunner.tools import route_manipulation
 from srunner.objects.ego_vehicle import EgoVehicle
 from srunner.tools.log import LogUtil
 
+from srunner.tools.CARLA_manager import CARLAManager
+
 logger = logging.getLogger("scenario-runner")
 
+infractions_dict = {
+    "OutsideRouteLanesTest": 0.3,
+    "CollisionTest": 1.0,
+    "RunningRedLightTest": 0.4,
+    "RunningStopTest": 0.25,
+}
+
+terminations_dict = {"AgentBlockedTest": 0.0}
+
 
 class AWScenarioRunner(object):
     # flags
@@ -45,6 +59,7 @@ class AWScenarioRunner(object):
     definition_manager = None
 
     aw_agent = None
+    host_volume = os.environ["SR_HOST_VOLUME"]
 
     def __init__(self, config: dict) -> None:
         """
@@ -131,15 +146,6 @@ def run_scenario(
 
         logger.info(f"{settings.__str__()}")
 
-        if self._tm_config["active"]:
-            logger.info("Loading Traffic Manager...")
-            tm_port = int(self._tm_config["port"])  # type: ignore
-            CarlaDataProvider.set_traffic_manager_port(tm_port)
-            tm = self.carla_client.get_trafficmanager(tm_port)
-
-            tm.set_random_device_seed(int(self._tm_config["seed"]))  # ADD TO CONFIG
-            tm.set_synchronous_mode(self._tm_config["sync"])
-
         # update the world
         CarlaDataProvider.set_world(self.carla_world)
 
@@ -150,6 +156,8 @@ def run_scenario(
 
         self.carla_world.tick()  # client must tick to spawn actors
 
+        logger.info("Initialising Autoware...")
+
         if not self.DEV_MODE:
             logger.info("Loading Autoware agent")
             agent_class_name = self.module_aw_agent.__name__.title().replace("_", "")
@@ -165,38 +173,59 @@ def run_scenario(
                 result = False
                 return
 
-        ego.prepare_ego()
-
         logger.info("Loading route...")
 
-        try:  # the route gets sent to the agent here
+        gps_route, route = route_manipulation.interpolate_trajectory(
+            route_config.keypoints
+        )
+        route_config.agent.set_global_plan(gps_route, route)  # set agent route
+
+        ego.prepare_ego(route[0][0])  # set location to first waypoint
+
+        self.carla_world.tick()
+        logger.info("Initialising agent route...")
+
+        # allow the agent to localise and set the route
+        budget = int(self._scenario_config["initialisation_budget"])
+        status = False  # completion status
+        for tick in range(1, budget + 1):
+            self.carla_world.tick()
+            status = route_config.agent.run_step_init()  # type: ignore
+
+        if not status:
+            logger.info("Agent failed to initialise route.")
+        else:
+            logger.info("Successfully initialised agent; route set.")
+
+        if self._tm_config["active"]:
+            logger.info("Loading Traffic Manager...")
+            tm_port = int(self._tm_config["port"])  # type: ignore
+            CarlaDataProvider.set_traffic_manager_port(tm_port)
+            tm = self.carla_client.get_trafficmanager(tm_port)
+
+            tm.set_random_device_seed(int(self._tm_config["seed"]))  # ADD TO CONFIG
+            tm.set_synchronous_mode(self._tm_config["sync"])
+
+        try:
             scenario = RouteScenario(
                 world=self.carla_world,
                 config=route_config,
                 debug_mode=self.DEBUG,
                 ego_vehicle=ego._actor,
+                route=route,
             )
         except Exception:
             logger.info("Could not load Route Scenario")
             traceback.print_exc()
 
-        # need to tick autoware and CARLA
-        # a determined number of times
-        # to allow it to plan the route
-        # assign a 'tick' budget
-        # exceeding budget = failure
-        # call agent init function or something...
-        # no need to tick scenario, just CARLA
-
         logger.info("Starting scenario...")
         try:
-            # recorder_name = f"{self.results_manager.last_scenario}/recording.log"
-            # self.carla_client.start_recorder(recorder_name, True)
-
-            self.scenario_manager.load_scenario(scenario, self.aw_agent)
+            self.carla_client.start_recorder("/home/carla/recording.log", True)
+            self.scenario_manager.load_scenario(
+                scenario, self.aw_agent, follow_ego=self._scenario_config["follow_ego"]
+            )
             self.scenario_manager.run_scenario()
-
-            # self.carla_client.stop_recorder()
+            self.carla_client.stop_recorder()
             result = True
         except Exception:
             traceback.print_exc()
@@ -249,7 +278,8 @@ def run(self) -> None:
 
         for iteration in range(self.iterations):
             logger.info("Starting CARLA container....")
-            # CARLAManager.start_carla()
+            CARLAManager.restart_carla()
+            time.sleep(5)  # allow CARLA to load
 
             self.curr_iteration = iteration
             logger.info(f"Starting algorithm iteration number {self.curr_iteration}")
@@ -277,7 +307,7 @@ def run(self) -> None:
             scenario_result.put(result_dict)
 
             scenario_process = multiprocessing.Process(
-                target=self.run_scenario,
+                target=self.run_scenario,  # need to catch connection exception
                 args=(
                     route_config,
                     env_config,
@@ -296,7 +326,18 @@ def run(self) -> None:
             if scenario_process.is_alive():
                 scenario_process.kill()
 
+            # copy over the recording from CARLA container if env variable is setup
+            if self.host_volume is not None:
+                CARLAManager.fetch_file(
+                    "/home/carla/recording.log",
+                    f"{self.host_volume}/{self.results_manager.last_scenario}/recording.log",
+                )
+
+            logger.info("Calculating driving score...")
             driving_score = self._calculate_driving_score(result["criteria"])
+            logger.info(
+                f"Scenario iteration {iteration} achieved a score of {driving_score}"
+            )
 
             # read the scenario definition
             if not self.DEV_MODE:
@@ -332,8 +373,31 @@ def _output_criteria(
         return criteria_dict
 
     def _calculate_driving_score(self, criteria: dict) -> float:
-        # to be implemented
-        return 0.0
+        driving_score = 0.0
+
+        for key in terminations_dict.keys():
+            if not criteria[key]["success_value"] == criteria[key]["actual_value"]:
+                logger.info(f"Found terminal condition {key}.")
+                return 0.0  # hit a termination condition, driving score of 0.0
+
+        completed_route = float(criteria["RouteCompletionTest"]["actual_value"]) / 100
+        logger.info(f"Agent route completion: {completed_route * 100}%")
+
+        logger.info("Checking penality conditions...")
+        penalties = 1
+        for infraction, penalty in infractions_dict.items():
+            delta_penalty = float(criteria[infraction]["actual_value"] * penalty)
+
+            if delta_penalty:
+                logger.info(
+                    f"Condition {infraction}: Breached {criteria[infraction]['actual_value']} times"
+                )
+                logger.info(f"Applying penalty of {delta_penalty}")
+            else:
+                logger.info(f"Condition {infraction}: Found zero breaches")
+
+        driving_score = completed_route * (1 / penalties)
+        return driving_score
 
     def destroy(self) -> None:
         """Deletes instances of all classes related to CARLA"""
@@ -378,9 +442,6 @@ def _cleanup(self) -> None:
 
 
 def main():
-    # single argument of configuration file
-
-    # configure logger
     config = None
     with open("config.yaml", "r") as stream:
         config = yaml.safe_load(stream)
@@ -401,6 +462,8 @@ def main():
     logger.addHandler(fh)
     logger.addHandler(sh)
 
+    CARLAManager._load_config(config["carla"])
+
     # reload world and sync must be present when running agent-based route scenarios
     scenario_runner = None
     try:

config.yaml

@@ -1,26 +1,30 @@
 log:
   path: 'logs/'
   clear_old_logs: true
-  log_format: '[%(levelname)s] [%(asctime)s] [%(message)s] [%(filename)s]:[%(lineno)d]'
+  log_format: '[%(levelname)s] [%(created)f] [%(filename)s] %(message)s'
 carla:
   host: 127.0.0.1
-  port: 3000
+  fidelity: 'Low'
+  port: 2000
   timeout: 20
   sync: true
   fixed_delta_seconds: 0.05 # update rate 1 / FPS
 traffic_manager:
-  active: false
+  active: true
   sync: true # must be the same as carla sync
   seed: 0
   port: 8001
 scenario_runner:
   debug: false
   dev_mode: false
+  follow_ego: true
   json: ./example_scenario.json
   route_id: 0
   agent: srunner/autoagents/autoware_agent
+  initialisation_budget: 200 # ticks
   algorithm:
     iterations: 10
-    path: algorithms/test_alg # relative path
+    path: algorithms/test_alg
     args: # will be passed into the algorithm class as a dict
-      test_arg: 10
+      lanelet_path: /autoware_scenario_runner/algorithms/resources/Town01.osm
+      radius: 10

requirements.txt

@@ -13,3 +13,4 @@ six
 simple-watchdog-timer
 antlr4-python3-runtime==4.10
 graphviz
+lanelet2

srunner/autoagents/autonomous_agent.py

@@ -57,6 +57,15 @@ def sensors(self):  # pylint: disable=no-self-use
 
         return sensors
 
+    def run_step_init(self):
+        """
+        Override
+        Initialisation loop / parameters
+
+        :return: status (ready or not)
+        """
+        return
+
     def run_step(self):
         """
         Override