[WIP] Looking for the issue with TCP brain replication

behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/leaderboard_evaluator.py

@@ -111,6 +111,8 @@ def __init__(self, args, statistics_manager):
             self.traffic_manager = self.client.get_trafficmanager(int(args.trafficManagerPort))
             # self.traffic_manager = self.client.get_trafficmanager(8000)
         except Exception as e:
+            print('TRAFFIC MANAGER CREATION FAILED!!!')
+            print(int(args.trafficManagerPort))
             print(e)
         dist = pkg_resources.get_distribution("carla")
         # if dist.version != 'leaderboard':
@@ -263,7 +265,8 @@ def _load_and_wait_for_world(self, args, town, ego_vehicles=None):
         else:
             self.world.wait_for_tick()
 
-        if CarlaDataProvider.get_map().name != town:
+        if CarlaDataProvider.get_map().name != town and CarlaDataProvider.get_map().name.split('/')[2] != town:
+            print(CarlaDataProvider.get_map().name)
             raise Exception("The CARLA server uses the wrong map!"
                             "This scenario requires to use map {}".format(town))
 
@@ -350,7 +353,10 @@ def _load_and_run_scenario(self, args, config):
             self._load_and_wait_for_world(args, config.town, config.ego_vehicles)
             self._prepare_ego_vehicles(config.ego_vehicles, False)
             scenario = RouteScenario(world=self.world, config=config, debug_mode=args.debug)
-            self.statistics_manager.set_scenario(scenario.scenario)
+            print(scenario)
+            print('-----------------------------------------------------------------')
+            self.statistics_manager.set_scenario(scenario)
+            #self.statistics_manager.set_scenario(scenario.scenario)
 
             # self.agent_instance._init()
             # self.agent_instance.sensor_interface = SensorInterface()

behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/route_scenario.py

@@ -33,15 +33,15 @@
 from srunner.scenarios.object_crash_intersection import VehicleTurningRoute
 from srunner.scenarios.other_leading_vehicle import OtherLeadingVehicle
 from srunner.scenarios.maneuver_opposite_direction import ManeuverOppositeDirection
-from srunner.scenarios.junction_crossing_route import SignalJunctionCrossingRoute, NoSignalJunctionCrossingRoute
+#from srunner.scenarios.junction_crossing_route import SignalJunctionCrossingRoute, NoSignalJunctionCrossingRoute
 
 from srunner.scenariomanager.scenarioatomics.atomic_criteria import (CollisionTest,
 																	 InRouteTest,
 																	 RouteCompletionTest,
 																	 OutsideRouteLanesTest,
 																	 RunningRedLightTest,
-																	 RunningStopTest,
-																	 ActorSpeedAboveThresholdTest)
+																	 RunningStopTest)#,
+																	 #ActorSpeedAboveThresholdTest)
 
 from leaderboard.utils.route_parser import RouteParser, TRIGGER_THRESHOLD, TRIGGER_ANGLE_THRESHOLD
 from leaderboard.utils.route_manipulation import interpolate_trajectory
@@ -61,10 +61,10 @@
 	"Scenario4": VehicleTurningRoute,
 	"Scenario5": OtherLeadingVehicle,
 	"Scenario6": ManeuverOppositeDirection,
-	"Scenario7": SignalJunctionCrossingRoute,
-	"Scenario8": SignalJunctionCrossingRoute,
-	"Scenario9": SignalJunctionCrossingRoute,
-	"Scenario10": NoSignalJunctionCrossingRoute
+	#"Scenario7": SignalJunctionCrossingRoute,
+	#"Scenario8": SignalJunctionCrossingRoute,
+	#"Scenario9": SignalJunctionCrossingRoute,
+	#"Scenario10": NoSignalJunctionCrossingRoute
 }
 
 
@@ -222,16 +222,17 @@ def _update_route(self, world, config, debug_mode):
 		world_annotations = RouteParser.parse_annotations_file(config.scenario_file)
 
 		# prepare route's trajectory (interpolate and add the GPS route)
-		# gps_route, route = interpolate_trajectory(world, config.trajectory)
-		gps_route, route, wp_route = interpolate_trajectory(world, config.trajectory)
+		gps_route, route = interpolate_trajectory(world, config.trajectory)
+		#gps_route, route, wp_route = interpolate_trajectory(world, config.trajectory)
 
 		potential_scenarios_definitions, _ = RouteParser.scan_route_for_scenarios(
 			config.town, route, world_annotations)
 
 		self.route = route
-		CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route))
+		#CarlaDataProvider.set_ego_vehicle_route(convert_transform_to_location(self.route))
 
-		config.agent.set_global_plan(gps_route, self.route, wp_route)
+		#config.agent.set_global_plan(gps_route, self.route, wp_route)
+		config.agent.set_global_plan(gps_route, self.route)
 
 		# Sample the scenarios to be used for this route instance.
 		self.sampled_scenarios_definitions = self._scenario_sampling(potential_scenarios_definitions)
@@ -520,8 +521,14 @@ def _create_behavior(self):
 		scenario_behaviors = []
 		blackboard_list = []
 
+		print('---------------------')
+		print(self.list_scenarios)
+		print('--------RUTA!-------------')
+		print(self.route)
+		print('---------------------')
+
 		for i, scenario in enumerate(self.list_scenarios):
-			if scenario.scenario.behavior is not None:
+			if hasattr(scenario, 'scenario') and scenario.scenario.behavior is not None:
 				route_var_name = scenario.config.route_var_name
 
 				if route_var_name is not None:
@@ -542,7 +549,7 @@ def _create_behavior(self):
 			self.route,
 			blackboard_list,
 			scenario_trigger_distance,
-			repeat_scenarios=False
+			#repeat_scenarios=False
 		)
 
 		subbehavior.add_child(scenario_triggerer)  # make ScenarioTriggerer the first thing to be checked
@@ -554,8 +561,17 @@ def _create_behavior(self):
 	def _create_test_criteria(self):
 		"""
 		"""
+		print()
+		print('_create_test_criteria')
+		print()
 		criteria = []
-		route = convert_transform_to_location(self.route)
+		#route = convert_transform_to_location(self.route)
+		route = self.route
+		print()
+		print('convert_transform_to_location')
+		print('convert_transform_to_location')
+		print('convert_transform_to_location')
+		print()
 
 		# we terminate the route if collision or red light infraction happens during data collection
 
@@ -576,19 +592,20 @@ def _create_test_criteria(self):
 
 		stop_criterion = RunningStopTest(self.ego_vehicles[0])
 
+		'''
 		blocked_criterion = ActorSpeedAboveThresholdTest(self.ego_vehicles[0],
 														 speed_threshold=0.1,
 														 below_threshold_max_time=180.0,
 														 terminate_on_failure=True,
 														 name="AgentBlockedTest")
-
+		'''
 		criteria.append(completion_criterion)
 		criteria.append(outsidelane_criterion)
 		criteria.append(collision_criterion)
 		criteria.append(red_light_criterion)
 		criteria.append(stop_criterion)
 		criteria.append(route_criterion)
-		criteria.append(blocked_criterion)
+		#criteria.append(blocked_criterion)
 
 		return criteria
 

behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/scenario_manager.py

@@ -104,15 +104,16 @@ def load_scenario(self, scenario, agent, rep_number):
         GameTime.restart()
         self._agent = AgentWrapper(agent)
         self.scenario_class = scenario
-        self.scenario = scenario.scenario
+        #self.scenario = scenario.scenario
+        self.scenario = scenario
         self.scenario_tree = self.scenario.scenario_tree
         self.ego_vehicles = scenario.ego_vehicles
         self.other_actors = scenario.other_actors
         self.repetition_number = rep_number
 
         # To print the scenario tree uncomment the next line
         # py_trees.display.render_dot_tree(self.scenario_tree)
-        CarlaDataProvider.set_ego(self.ego_vehicles[0])
+        #CarlaDataProvider.set_ego(self.ego_vehicles[0])
 
         self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode)
 

behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/utils/statistics_manager.py

@@ -143,9 +143,11 @@ def compute_route_statistics(self, config, duration_time_system=-1, duration_tim
                 failure = "Agent timed out"
 
             for node in self._master_scenario.get_criteria():
-                if node.list_traffic_events:
+                if node.events:
+                #if node.list_traffic_events:
                     # analyze all traffic events
-                    for event in node.list_traffic_events:
+                    for event in node.events:
+                    #for event in node.list_traffic_events:
                         if event.get_type() == TrafficEventType.COLLISION_STATIC:
                             score_penalty *= PENALTY_COLLISION_STATIC
                             route_record.infractions['collisions_layout'].append(event.get_message())

behavior_metrics/brains/CARLA/TCP/leaderboard/scripts/run_evaluation.sh

@@ -1,12 +1,17 @@
 #!/bin/bash
 export CARLA_ROOT= PATH_TO_CARLA
+export CARLA_ROOT="/home/SergioPaniego/carla/Dist/CARLA_Shipping_0.9.15-dirty/LinuxNoEditor"
 export CARLA_SERVER=${CARLA_ROOT}/CarlaUE4.sh
+export PYTHONPATH="/home/SergioPaniego/carla-ros-bridge/catkin_ws/devel/lib/python3/dist-packages:/opt/ros/noetic/lib/python3/dist-packages"
 export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI
 export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla
-export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.10-py3.7-linux-x86_64.egg
+export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla/agents
+export PYTHONPATH=$PYTHONPATH:${CARLA_ROOT}/PythonAPI/carla/agents/navigation
+export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.15-py3.8-linux-x86_64.egg
 export PYTHONPATH=$PYTHONPATH:leaderboard
 export PYTHONPATH=$PYTHONPATH:leaderboard/team_code
-export PYTHONPATH=$PYTHONPATH:scenario_runner
+export PYTHONPATH=$PYTHONPATH:leaderboard/scenario_runner
+export PYTHONPATH=$PYTHONPATH:/home/SergioPaniego/Documentos/BehaviorMetrics/behavior_metrics/brains/CARLA
 
 export LEADERBOARD_ROOT=leaderboard
 export CHALLENGE_TRACK_CODENAME=SENSORS
@@ -20,11 +25,18 @@ export RESUME=True
 # TCP evaluation
 export ROUTES=leaderboard/data/evaluation_routes/routes_lav_valid.xml
 export TEAM_AGENT=team_code/tcp_agent.py
-export TEAM_CONFIG= PATH_TO_MODEL_CKPT
+export TEAM_CONFIG=/home/SergioPaniego/Documentos/BehaviorMetrics/behavior_metrics/models/CARLA/tcp_best_model.ckpt
 export CHECKPOINT_ENDPOINT=results_TCP.json
 export SCENARIOS=leaderboard/data/scenarios/all_towns_traffic_scenarios.json
 export SAVE_PATH=data/results_TCP/
 
+echo "CARLA_ROOT"
+echo $CARLA_ROOTs
+echo "PATH_TO_CARLA"
+echo $PATH_TO_CARLA
+echo "PYTHONPATH"
+echo $PYTHONPATH
+echo "HOLA"
 
 python3 ${LEADERBOARD_ROOT}/leaderboard/leaderboard_evaluator.py \
 --scenarios=${SCENARIOS}  \

behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/planner.py

@@ -80,6 +80,9 @@ def run_step(self, gps):
 
         if len(self.route) == 1:
             return self.route[0]
+        #print()
+        #print()
+        #print('DENTRO', self.route)
 
         to_pop = 0
         farthest_in_range = -np.inf
@@ -110,4 +113,7 @@ def run_step(self, gps):
         self.debug.dot(gps, gps, (0, 0, 255))
         self.debug.show()
 
+        #print()
+        #print()
+        #print('DENTRO', self.route)
         return self.route[1]

behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/tcp_agent.py

@@ -138,6 +138,17 @@ def tick(self, input_data):
 		speed = input_data['speed'][1]['speed']
 		compass = input_data['imu'][1][-1]
 
+		#print('---- IMU DATA ----')
+		#print(input_data['imu'])
+		#print('---- COMPASS ----')
+		#print(compass)
+		'''
+		(1263, array([ 5.01221323e+00, -2.09726281e-02,  9.80859756e+00, -6.65326806e-05,
+       -1.39825954e-03,  1.25346202e-02,  3.65396426e-03]))
+		---- COMPASS ----
+		0.0036539642605930567
+		'''
+
 		if (math.isnan(compass) == True): #It can happen that the compass sends nan for a few frames
 			compass = 0.0
 
@@ -190,14 +201,28 @@ def run_step(self, input_data, timestamp):
 		cmd_one_hot = [0] * 6
 		cmd_one_hot[command] = 1
 		cmd_one_hot = torch.tensor(cmd_one_hot).view(1, 6).to('cuda', dtype=torch.float32)
+		print('---SPEED---')
+		print(tick_data['speed'])
 		speed = torch.FloatTensor([float(tick_data['speed'])]).view(1,1).to('cuda', dtype=torch.float32)
 		speed = speed / 12
+		#print('---SPEED---')
+		print(speed)
+		print(tick_data['rgb'].shape)
+		print(type(tick_data['rgb']))
+		from PIL import Image
+		imagen_pil = Image.fromarray(tick_data['rgb'])
+		imagen_pil.save('imagen_de_tcp.png')
+
 		rgb = self._im_transform(tick_data['rgb']).unsqueeze(0).to('cuda', dtype=torch.float32)
 
 		tick_data['target_point'] = [torch.FloatTensor([tick_data['target_point'][0]]),
 										torch.FloatTensor([tick_data['target_point'][1]])]
 		target_point = torch.stack(tick_data['target_point'], dim=1).to('cuda', dtype=torch.float32)
+
 		state = torch.cat([speed, target_point, cmd_one_hot], 1)
+		print(state)
+		print(state.dtype)
+
 
 		pred= self.net(rgb, state, target_point)
 

behavior_metrics/brains/CARLA/TCP/model.py

@@ -2,7 +2,8 @@
 import numpy as np
 import torch 
 from torch import nn
-from brains.CARLA.TCP.resnet import *
+#from brains.CARLA.TCP.resnet import *
+from TCP.resnet import *
 
 
 class PIDController(object):
@@ -16,6 +17,9 @@ def __init__(self, K_P=1.0, K_I=0.0, K_D=0.0, n=20):
 		self._min = 0.0
 
 	def step(self, error):
+		print('llega!')
+		print(self._window)
+		print('llega!')
 		self._window.append(error)
 		self._max = max(self._max, abs(error))
 		self._min = -abs(self._max)

behavior_metrics/brains/CARLA/brain_carla_TCP_2.py

@@ -160,6 +160,9 @@ def tick(self):
         #speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
         #speed = 3.6 * speed_m_s #m/s to km/h 
 
+        print('---SPEED 1---')
+        print(speed)
+
         imu_data = self.imu.getIMU()
 
         compass = np.array([imu_data.compass.x, imu_data.compass.y, imu_data.compass.z, imu_data.compass.w])
@@ -230,6 +233,9 @@ def execute(self):
         cmd_one_hot = torch.tensor(cmd_one_hot).view(1, 6).to('cuda', dtype=torch.float32)
         speed = torch.FloatTensor([float(tick_data['speed'])]).view(1,1).to('cuda', dtype=torch.float32)
         speed = speed / 12
+        print('---SPEED 2---')
+        print(speed)
+
 
         print(tick_data['rgb'].shape)
         print(type(tick_data['rgb']))
@@ -291,15 +297,38 @@ def execute(self):
         self.pid_metadata['status'] = self.status
 
 
-        #print(throttle, steer, brake)
+        print(throttle, steer, brake)
         self.motors.sendThrottle(throttle)
         self.motors.sendSteer(steer)
         self.motors.sendBrake(brake)
 
+        #self.motors.sendThrottle(1.0)
+        #self.motors.sendSteer(0.0)
+        #self.motors.sendBrake(0.0)
+
+
 
         '''
+            TODO: REVIEW PID CONTROLLERS SINCE THEY USE A WINDOW
             TODO: Draw the waypoints on the map
             TODO: Draw the trajectory on the images.
             TODO: Figure out what's the transformation for the compass.
-
+            TODO: Review metadata from model!!! metadata = {
+			'speed': float(speed.astype(np.float64)),
+			'steer': float(steer),
+			'throttle': float(throttle),
+			'brake': float(brake),
+			'wp_4': tuple(waypoints[3].astype(np.float64)),
+			'wp_3': tuple(waypoints[2].astype(np.float64)),
+			'wp_2': tuple(waypoints[1].astype(np.float64)),
+			'wp_1': tuple(waypoints[0].astype(np.float64)),
+			'aim': tuple(aim.astype(np.float64)),
+			'target': tuple(target.astype(np.float64)),
+			'desired_speed': float(desired_speed.astype(np.float64)),
+			'angle': float(angle.astype(np.float64)),
+			'angle_last': float(angle_last.astype(np.float64)),
+			'angle_target': float(angle_target.astype(np.float64)),
+			'angle_final': float(angle_final.astype(np.float64)),
+			'delta': float(delta.astype(np.float64)),
+		}
         '''

behavior_metrics/robot/interfaces/imu.py

@@ -7,6 +7,10 @@ def imuMsg2IMU(imuMsg):
 
     imu = IMU()
 
+    #print('-----')
+    #print(imuMsg.orientation)
+    #print('-----')
+
     imu.compass = imuMsg.orientation
     imu.accelerometer = imuMsg.linear_acceleration
     imu.gyroscope = imuMsg.angular_velocity