diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/leaderboard_evaluator.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/leaderboard_evaluator.py index 280c8caf..423a9936 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/leaderboard_evaluator.py +++ b/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() diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/route_scenario.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/route_scenario.py index 827727cd..41dfc363 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/route_scenario.py +++ b/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 diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/scenario_manager.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/scenario_manager.py index 9993fa44..0295e8ae 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/scenario_manager.py +++ b/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/scenarios/scenario_manager.py @@ -104,7 +104,8 @@ 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 @@ -112,7 +113,7 @@ def load_scenario(self, scenario, agent, 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) diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/utils/statistics_manager.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/utils/statistics_manager.py index 7ea26d58..6514f11d 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/leaderboard/utils/statistics_manager.py +++ b/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()) diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/scripts/run_evaluation.sh b/behavior_metrics/brains/CARLA/TCP/leaderboard/scripts/run_evaluation.sh index 1d8a7435..f026baf1 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/scripts/run_evaluation.sh +++ b/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} \ diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/planner.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/planner.py index 6aff90a2..d38ed8fa 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/planner.py +++ b/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] diff --git a/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/tcp_agent.py b/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/tcp_agent.py index 8c11cda6..f035acf5 100644 --- a/behavior_metrics/brains/CARLA/TCP/leaderboard/team_code/tcp_agent.py +++ b/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) diff --git a/behavior_metrics/brains/CARLA/TCP/model.py b/behavior_metrics/brains/CARLA/TCP/model.py index 6881c851..2a58aa3e 100644 --- a/behavior_metrics/brains/CARLA/TCP/model.py +++ b/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) diff --git a/behavior_metrics/brains/CARLA/brain_carla_TCP_2.py b/behavior_metrics/brains/CARLA/brain_carla_TCP_2.py index 8031d06a..67263bda 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_TCP_2.py +++ b/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)), + } ''' \ No newline at end of file diff --git a/behavior_metrics/robot/interfaces/imu.py b/behavior_metrics/robot/interfaces/imu.py index 5afe9a88..e41bc3f4 100644 --- a/behavior_metrics/robot/interfaces/imu.py +++ b/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 diff --git a/behavior_metrics/robot/sensors.py b/behavior_metrics/robot/sensors.py index 1527a3f4..c5106514 100644 --- a/behavior_metrics/robot/sensors.py +++ b/behavior_metrics/robot/sensors.py @@ -198,6 +198,17 @@ def get_gnss(self, gnss_name): robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance """ return self.__get_sensor(gnss_name, 'gnss') + + def get_speedometer(self, speedometer_name): + """Retrieve an specific existing bird eye view + + Arguments: + speedometer_name {str} -- Name of the birdeyeview to be retrieved + + Returns: + robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance + """ + return self.__get_sensor(speedometer_name, 'speedometer') def kill(self): """Destroy all the running sensors"""