updated docker cp to host

Author: david-gasinski

algorithms/hill_climbing.py algorithms/hill_climb.pyalgorithms/hill_climbing.py renamed to 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

@@ -34,6 +34,15 @@
 
 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
@@ -50,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:
         """
@@ -178,12 +188,14 @@ def run_scenario(
         # allow the agent to localise and set the route
         budget = int(self._scenario_config["initialisation_budget"])
         status = False  # completion status
-        for tick in range(0, budget):
+        for tick in range(1, budget + 1):
             self.carla_world.tick()
             status = route_config.agent.run_step_init()  # type: ignore
 
-            if status:
-                logger.info(f"Successfully initialised agent in {tick} ticks")
+        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...")
@@ -194,7 +206,7 @@ def run_scenario(
             tm.set_random_device_seed(int(self._tm_config["seed"]))  # ADD TO CONFIG
             tm.set_synchronous_mode(self._tm_config["sync"])
 
-        try:  # the route gets sent to the agent here
+        try:
             scenario = RouteScenario(
                 world=self.carla_world,
                 config=route_config,
@@ -208,14 +220,12 @@ def run_scenario(
 
         logger.info("Starting scenario...")
         try:
-            # recorder_name = f"{self.results_manager.last_scenario}/recording.log"
-            # self.carla_client.start_recorder(recorder_name, True)
+            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()
@@ -269,7 +279,7 @@ def run(self) -> None:
         for iteration in range(self.iterations):
             logger.info("Starting CARLA container....")
             CARLAManager.restart_carla()
-            time.sleep(5)
+            time.sleep(5)  # allow CARLA to load
 
             self.curr_iteration = iteration
             logger.info(f"Starting algorithm iteration number {self.curr_iteration}")
@@ -297,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,
@@ -316,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:
@@ -352,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"""

config.yaml

@@ -1,7 +1,7 @@
 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
   fidelity: 'Low'
@@ -10,20 +10,21 @@ carla:
   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: false # camera to follow ego vehicle
+  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/scenarios/route_scenario.py

@@ -46,6 +46,7 @@
 )
 
 from srunner.scenarios.basic_scenario import BasicScenario
+from srunner.scenarios.background_activity import BackgroundBehavior
 from srunner.scenariomanager.weather_sim import RouteWeatherBehavior
 from srunner.scenariomanager.lights_sim import RouteLightsBehavior
 from srunner.scenariomanager.timer import RouteTimeoutBehavior
@@ -63,10 +64,6 @@ class RouteScenario(BasicScenario):
     along which several smaller scenarios are triggered
     """
 
-    # fix route scenario
-    # take in a pre-spawned ego vehicle
-    # position it at the first waypoint in the route
-
     def __init__(
         self,
         world,
@@ -373,11 +370,11 @@ def _create_behavior(self):
         )  # Tick the ScenarioTriggerer before the scenarios
 
         # Add the Background Activity
-        # behavior.add_child(
-        #    BackgroundBehavior(
-        #        self.ego_vehicles[0], self.route, name="BackgroundActivity"
-        #    )
-        # )
+        behavior.add_child(
+            BackgroundBehavior(
+                self.ego_vehicles[0], self.route, name="BackgroundActivity"
+            )
+        )
 
         behavior.add_children(scenario_behaviors)
         return behavior

srunner/tools/CARLA_manager.py

@@ -57,6 +57,25 @@ def stop_carla():
             logger.info(f"Kill stdout: {result.stdout.strip()}")
             CARLAManager.container_id = None
 
+    @staticmethod
+    def fetch_file(path: str, dest: str):
+        if CARLAManager.container_id is not None:
+            env = os.environ.copy()
+            result = subprocess.run(
+                f"docker cp {CARLAManager.container_id}:{path} {dest}",
+                shell=True,
+                text=True,
+                capture_output=True,
+                env=env,
+            )
+            logger.info(
+                f"Copying... {path} from container {CARLAManager.container_id} to {dest}"
+            )
+            logger.info(f"docker cp {CARLAManager.container_id}:{path} {dest}")
+
+            if not result.returncode == 0:
+                logger.info(f"Failed to copy {path} to {dest}")
+
     @staticmethod
     def restart_carla():
         if CARLAManager.container_id is not None: