diff --git a/behavior_metrics/brains/CARLA/brain_carla_TCP.py b/behavior_metrics/brains/CARLA/brain_carla_TCP.py index c780d978..45b7bd12 100644 --- a/behavior_metrics/brains/CARLA/brain_carla_TCP.py +++ b/behavior_metrics/brains/CARLA/brain_carla_TCP.py @@ -22,10 +22,10 @@ from brains.CARLA.TCP.leaderboard.leaderboard.utils.route_manipulation import interpolate_trajectory from brains.CARLA.TCP.leaderboard.leaderboard.utils.route_indexer import RouteIndexer +from collections import deque -PRETRAINED_MODELS = '/home/SergioPaniego/Descargas/' -#MODEL_DIR = 'best_model.ckpt' # Inside download folder. +PRETRAINED_MODELS = ROOT_PATH + '/' + PRETRAINED_MODELS_DIR + 'CARLA/' class Brain: @@ -34,6 +34,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None): self.camera_rgb = sensors.get_camera('camera_0') # rgb front view camera self.camera_seg = sensors.get_camera('camera_2') # segmentation camera self.imu = sensors.get_imu('imu_0') # imu + self.gnss = sensors.get_gnss('gnss_0') # gnss self.handler = handler self.inference_times = [] self.gpu_inference = config['GPU'] @@ -82,6 +83,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None): time.sleep(1) # sleep for 1 second before checking again #app_configuration = Config(config_data['config'][0]) + ''' route_counter = 0 TEST_ROUTES = [{ "map": "Town02", @@ -103,17 +105,32 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None): self.target_point = torch.stack(self.target_point, dim=1).to('cuda', dtype=torch.float32) #self.target_point = torch.tensor(self.target_point).view(-1, 1).to('cuda', dtype=torch.float32) - + ''' self._im_transform = T.Compose([T.ToTensor(), T.Normalize(mean=[0.485,0.456,0.406], std=[0.229,0.224,0.225])]) repetitions = 1 - routes = '/home/SergioPaniego/Documentos/BehaviorMetrics/behavior_metrics/brains/CARLA/TCP/leaderboard/data/TCP_training_routes/routes_town01.xml' - scenarios = '/home/SergioPaniego/Documentos/BehaviorMetrics/behavior_metrics/brains/CARLA/TCP/leaderboard/data/scenarios/town01_all_scenarios.json' + routes = 'brains/CARLA/TCP/leaderboard/data/TCP_training_routes/routes_town02.xml' + scenarios = 'brains/CARLA/TCP/leaderboard/data/scenarios/town02_all_scenarios.json' route_indexer = RouteIndexer(routes, scenarios, repetitions) - #route_indexer = RouteIndexer(args.routes, args.scenarios, args.repetitions) # setup config = route_indexer.next() + ''' + We currently hard-code the initial and target points + ''' + print('----- TRAJECTORY ------') + print(config) + print(config.trajectory) + print(config.trajectory[0].x, config.trajectory[0].y) + config.trajectory[0].x = 55.3 + config.trajectory[0].y = -105.6 + + config.trajectory[1].x = 2.0 + config.trajectory[1].y = -105.6 + print(config.trajectory[0].x, config.trajectory[0].y) + print() + print(config.trajectory[1].x, config.trajectory[1].y) + print('----- TRAJECTORY ------') # prepare route's trajectory (interpolate and add the GPS route) gps_route, route = interpolate_trajectory(world, config.trajectory) @@ -122,6 +139,9 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None): self.set_global_plan(gps_route, self.route) self._init_route_planner() + self.last_steers = deque() + self.status = 0 + def set_global_plan(self, global_plan_gps, global_plan_world_coord, wp=None): """ @@ -130,12 +150,19 @@ def set_global_plan(self, global_plan_gps, global_plan_world_coord, wp=None): ds_ids = downsample_route(global_plan_world_coord, 50) self._global_plan_world_coord = [(global_plan_world_coord[x][0], global_plan_world_coord[x][1]) for x in ds_ids] self._global_plan = [global_plan_gps[x] for x in ds_ids] + print('-----GLOBAL PLAN -----') + print(self._global_plan) def _init_route_planner(self): self._route_planner = RoutePlanner(4.0, 50.0) self._route_planner.set_route(self._global_plan, True) + gps = np.array([self._global_plan[0][0]['lon'], self._global_plan[0][0]['lat']]) + gps = (gps - self._route_planner.mean) * self._route_planner.scale + print('-----GPS----') + print(gps) + self.initialized = True @@ -163,16 +190,18 @@ def update_frame(self, frame_id, data): def _get_position(self, tick_data): - gps = tick_data['gps'] + gps = self.gnss.getGNSS() + gps = np.array([gps.longitude, gps.latitude]) gps = (gps - self._route_planner.mean) * self._route_planner.scale - + print('-----GPS-----') + print(gps) + print('-----GPS-----') return gps @torch.no_grad() def execute(self): """Main loop of the brain. This will be called iteratively each TIME_CYCLE (see pilot.py)""" - # VOID = -1 # LEFT = 1 # RIGHT = 2 @@ -190,8 +219,6 @@ def execute(self): cmd_one_hot = torch.tensor(cmd_one_hot).view(1, 6).to('cuda', dtype=torch.float32) rgb = self.camera_rgb.getImage().data - #import cv2 - #rgb = cv2.cvtColor(rgb[:, :, :3], cv2.COLOR_BGR2RGB) seg_image = self.camera_seg.getImage().data self.update_frame('frame_0', rgb) @@ -204,22 +231,12 @@ def execute(self): speed = torch.tensor(speed).view(-1, 1).to('cuda', dtype=torch.float32) speed = speed / 12 - print(speed) - #print(type(speed)) - #print(speed.dtype) - # expected Tensor as element 1 in argument 0, but got tuple - print(self.target_point) - #print(type(self.target_point)) - print(cmd_one_hot) - #print(type(cmd_one_hot)) - - imu_data = self.imu.getIMU().data - print(imu_data) - compass = imu_data - - - #gps = input_data['gps'][1][:2] - #compass = input_data['imu'][1][-1] + imu_data = self.imu.getIMU() + #compass = np.array([imu_data.compass.x, imu_data.compass.y, imu_data.compass.z, imu_data.compass.w]) + compass = np.array([imu_data.compass.x, imu_data.compass.y, imu_data.compass.z]) + print('----------compass--------------') + print(compass) + compass = compass[-1] if (math.isnan(compass) == True): #It can happen that the compass sends nan for a few frames compass = 0.0 @@ -235,9 +252,9 @@ def execute(self): pos = self._get_position(result) #result['gps'] = pos next_wp, next_cmd = self._route_planner.run_step(pos) + next_wp = [next_wp[1], next_wp[0]] result['next_command'] = next_cmd.value - theta = compass + np.pi/2 R = np.array([ [np.cos(theta), -np.sin(theta)], @@ -245,49 +262,133 @@ def execute(self): ]) local_command_point = np.array([next_wp[0]-pos[0], next_wp[1]-pos[1]]) - local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + + #local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + #local_command_point = R.T.dot(local_command_point) + + result['target_point'] = tuple(local_command_point) + result['target_point'] = [torch.FloatTensor([result['target_point'][0]]), + torch.FloatTensor([result['target_point'][1]])] + self.target_point = torch.stack(result['target_point'], dim=1).to('cuda', dtype=torch.float32) + + + + ''' + + Local point seems to be incorrect and R.T.dot is commented while in the original code it's used. + ''' + + print('----------compass--------------') + ''' + -0.999806824289457 + ''' + print(compass) + #print('--------- POS ---------------') + ''' + [ 43.17920366 -105.57289901] + ''' + #print(pos) + print('-----------LOCAL COMMAND POINT ---------------') + ''' + [-3.83868739e+01 1.80376380e-02] + ''' + print(local_command_point) + #print('-------NEXT WAYPOINT-----------') + ''' + [4.7923297947398655, -105.55486136806194] + ''' + #print(next_wp) + print('-------Target point-----------') + print(self.target_point) + ''' + -------Target point----------- + tensor([[-3.8387e+01, 1.8038e-02]], device='cuda:0') + ''' + print('--------RESULT----------') + #print(result) + print('------------------') state = torch.cat([speed, self.target_point, cmd_one_hot], 1) + print('------STATE-----') print(state) rgb = self._im_transform(rgb).unsqueeze(0).to('cuda', dtype=torch.float32) - print(rgb.shape) + #print(rgb.shape) pred = self.net(rgb, state, self.target_point) - #print(pred) - #print(type(pred)) + print('-----PRED------') + print(pred.keys()) + print(pred['pred_wp']) + print('------COMMAND----') + print(command) steer_ctrl, throttle_ctrl, brake_ctrl, metadata = self.net.process_action(pred, command, gt_velocity, self.target_point) + print('------ steer_ctrl, throttle_ctrl, brake_ctrl, metadata-------') print(steer_ctrl, throttle_ctrl, brake_ctrl, metadata) steer_traj, throttle_traj, brake_traj, metadata_traj = self.net.control_pid(pred['pred_wp'], gt_velocity, self.target_point) - print(steer_traj, throttle_traj, brake_traj, metadata_traj) + if brake_traj < 0.05: brake_traj = 0.0 + if throttle_traj > brake_traj: brake_traj = 0.0 - print() - print() - print() - self.alpha = 0.3 - steer_ctrl = np.clip(self.alpha*steer_ctrl + (1-self.alpha)*steer_traj, -1, 1) - throttle_ctrl = np.clip(self.alpha*throttle_ctrl + (1-self.alpha)*throttle_traj, 0, 0.75) - brake_ctrl = np.clip(self.alpha*brake_ctrl + (1-self.alpha)*brake_traj, 0, 1) + print('------steer_traj, throttle_traj, brake_traj, metadata_traj-----') + print(steer_traj, throttle_traj, brake_traj, metadata_traj) + + + if self.status == 0: + print('LOG 1***********************************************************************************************') + self.alpha = 0.3 + steer_ctrl = np.clip(self.alpha*steer_ctrl + (1-self.alpha)*steer_traj, -1, 1) + throttle_ctrl = np.clip(self.alpha*throttle_ctrl + (1-self.alpha)*throttle_traj, 0, 0.75) + brake_ctrl = np.clip(self.alpha*brake_ctrl + (1-self.alpha)*brake_traj, 0, 1) + else: + print('LOG 2***********************************************************************************************') + self.alpha = 0.3 + #self.pid_metadata['agent'] = 'ctrl' + steer_ctrl = np.clip(self.alpha*steer_traj + (1-self.alpha)*steer_ctrl, -1, 1) + throttle_ctrl = np.clip(self.alpha*throttle_traj + (1-self.alpha)*throttle_ctrl, 0, 0.75) + brake_ctrl = np.clip(self.alpha*brake_traj + (1-self.alpha)*brake_ctrl, 0, 1) if brake_ctrl > 0.5: throttle_ctrl = float(0) + print('-------------steer_ctrl, throttle_ctrl, brake_ctrl----------') print(steer_ctrl, throttle_ctrl, brake_ctrl) + print() + print() + print() self.motors.sendThrottle(throttle_ctrl) self.motors.sendSteer(steer_ctrl) self.motors.sendBrake(brake_ctrl) + if len(self.last_steers) >= 20: + self.last_steers.popleft() + self.last_steers.append(abs(float(steer_ctrl))) + + num = 0 + for s in self.last_steers: + if s > 0.10: + num += 1 + if num > 10: + self.status = 1 + self.steer_step += 1 + + else: + self.status = 0 + diff --git a/behavior_metrics/configs/CARLA/default_carla_torch.yml b/behavior_metrics/configs/CARLA/default_carla_torch.yml index 995949a9..f6e88293 100644 --- a/behavior_metrics/configs/CARLA/default_carla_torch.yml +++ b/behavior_metrics/configs/CARLA/default_carla_torch.yml @@ -30,6 +30,10 @@ Behaviors: IMU_0: Name: 'imu_0' Topic: '/carla/ego_vehicle/imu' + GNSS: + GNSS_0: + Name: 'gnss_0' + Topic: '/carla/ego_vehicle/gnss' Actuators: CARLA_Motors: Motors_0: @@ -41,7 +45,7 @@ Behaviors: PilotTimeCycle: 50 AsyncMode: True Parameters: - Model: 'best_model.ckpt' + Model: 'tcp_best_model.ckpt' ImageCropped: True ImageSize: [ 100,50 ] ImageNormalized: True diff --git a/behavior_metrics/robot/interfaces/gnss.py b/behavior_metrics/robot/interfaces/gnss.py new file mode 100644 index 00000000..3361f66d --- /dev/null +++ b/behavior_metrics/robot/interfaces/gnss.py @@ -0,0 +1,98 @@ +import rospy +from sensor_msgs.msg import NavSatFix +import threading + + +def gnssMsg2GNSS(gnssMsg): + + gnss = GNSS() + + #print(gnssMsg) + + gnss.latitude = gnssMsg.latitude + gnss.longitude = gnssMsg.longitude + gnss.altitude = gnssMsg.altitude + + #msg.latitude = data[0] + # msg.longitude = data[1] + # msg.altitude = data[2] + + now = rospy.get_rostime() + gnss.timeStamp = now.secs + (now.nsecs * 1e-9) + + return gnss + + +class GNSS(): + + def __init__(self): + + self.latitude = 0 + self.longitude = 0 + self.altitude = 0 + self.timeStamp = 0 # Time stamp [s] + + def __str__(self): + s = "GNSS: {\n latitude: " + str(self.latitude) + "\n }\n longitude: " + str(self.longitude) + "\n }\n altitude: " + str(self.altitude) + "\n }\n timeStamp: " + str(self.timeStamp) + "\n}" + return s + + +class ListenerGNSS: + ''' + ROS GNSS Subscriber. GNSS Client to Receive gnss from ROS nodes. + ''' + def __init__(self, topic): + ''' + ListenerGNSS Constructor. + + @param topic: ROS topic to subscribe + @type topic: String + + ''' + self.topic = topic + self.data = GNSS() + self.sub = None + self.lock = threading.Lock() + self.start() + + def __callback(self, gnss): + ''' + Callback function to receive and save GNSS. + + @param odom: ROS Odometry received + + @type odom: Odometry + + ''' + gnss = gnssMsg2GNSS(gnss) + + self.lock.acquire() + self.data = gnss + self.lock.release() + + def stop(self): + ''' + Stops (Unregisters) the client. + + ''' + self.sub.unregister() + + def start(self): + ''' + Starts (Subscribes) the client. + + ''' + self.sub = rospy.Subscriber(self.topic, NavSatFix, self.__callback) + + def getGNSS(self): + ''' + Returns last GNSS. + + @return last GNSS saved + + ''' + self.lock.acquire() + gnss = self.data + self.lock.release() + + return gnss diff --git a/behavior_metrics/robot/interfaces/imu.py b/behavior_metrics/robot/interfaces/imu.py index b94e16a1..5afe9a88 100644 --- a/behavior_metrics/robot/interfaces/imu.py +++ b/behavior_metrics/robot/interfaces/imu.py @@ -1,5 +1,5 @@ import rospy -from std_msgs.msg import Float32 +from sensor_msgs.msg import Imu import threading @@ -7,7 +7,10 @@ def imuMsg2IMU(imuMsg): imu = IMU() - imu.data = imuMsg.data + imu.compass = imuMsg.orientation + imu.accelerometer = imuMsg.linear_acceleration + imu.gyroscope = imuMsg.angular_velocity + now = rospy.get_rostime() imu.timeStamp = now.secs + (now.nsecs * 1e-9) @@ -18,11 +21,26 @@ class IMU(): def __init__(self): - self.data = 0 # X coord [meters] + self.compass = { + 'x': 0, + 'y': 0, + 'z': 0, + 'w': 0 + } + self.gyroscope = { + 'x': 0, + 'y': 0, + 'z': 0 + } + self.accelerometer = { + 'x': 0, + 'y': 0, + 'z': 0 + } self.timeStamp = 0 # Time stamp [s] def __str__(self): - s = "IMU: {\n x: " + str(self.x) + "\n timeStamp: " + str(self.timeStamp) + "\n}" + s = "IMU: {\n compass: " + str(self.compass) + "\n }\n accelerometer: " + str(self.accelerometer) + "\n }\n gyroscope: " + str(self.gyroscope) + "\n }\n timeStamp: " + str(self.timeStamp) + "\n}" return s @@ -71,7 +89,7 @@ def start(self): Starts (Subscribes) the client. ''' - self.sub = rospy.Subscriber(self.topic, Float32, self.__callback) + self.sub = rospy.Subscriber(self.topic, Imu, self.__callback) def getIMU(self): ''' diff --git a/behavior_metrics/robot/interfaces/pose3d.py b/behavior_metrics/robot/interfaces/pose3d.py index 9fc84a3c..dcd1b72a 100644 --- a/behavior_metrics/robot/interfaces/pose3d.py +++ b/behavior_metrics/robot/interfaces/pose3d.py @@ -109,7 +109,7 @@ def __init__(self): self.timeStamp = 0 # Time stamp [s] def __str__(self): - s = "Pose3D: {\n x: " + str(self.x) + "\n Y: " + str(self.y) + s = "Pose3D: {\n X: " + str(self.x) + "\n Y: " + str(self.y) s = s + "\n Z: " + str(self.z) + "\n H: " + str(self.h) s = s + "\n Yaw: " + str(self.yaw) + "\n Pitch: " + str(self.pitch) + "\n Roll: " + str(self.roll) s = s + "\n quaternion: " + str(self.q) + "\n timeStamp: " + str(self.timeStamp) + "\n}" diff --git a/behavior_metrics/robot/sensors.py b/behavior_metrics/robot/sensors.py index cd15f82e..1527a3f4 100644 --- a/behavior_metrics/robot/sensors.py +++ b/behavior_metrics/robot/sensors.py @@ -22,6 +22,7 @@ logger.error('CARLA is not supported') from robot.interfaces.speedometer import ListenerSpeedometer from robot.interfaces.imu import ListenerIMU +from robot.interfaces.gnss import ListenerGNSS __author__ = 'fqez' __contributors__ = [] @@ -79,6 +80,12 @@ def __init__(self, sensors_config): if imu_conf: self.imu = self.__create_sensor(imu_conf, 'imu') + # Load gnss + gnss_conf = sensors_config.get('GNSS', None) + self.gnss = None + if gnss_conf: + self.gnss = self.__create_sensor(gnss_conf, 'gnss') + def __create_sensor(self, sensor_config, sensor_type): """Fill the sensor dictionary with instances of the sensor_type and sensor_config""" sensor_dict = {} @@ -97,6 +104,8 @@ def __create_sensor(self, sensor_config, sensor_type): sensor_dict[name] = ListenerSpeedometer(topic) elif sensor_type == 'imu': sensor_dict[name] = ListenerIMU(topic) + elif sensor_type == 'gnss': + sensor_dict[name] = ListenerGNSS(topic) return sensor_dict @@ -117,6 +126,8 @@ def __get_sensor(self, sensor_name, sensor_type): sensor = self.speedometer[sensor_name] elif sensor_type == 'imu': sensor = self.imu[sensor_name] + elif sensor_type == 'gnss': + sensor = self.gnss[sensor_name] except KeyError: return "[ERROR] No existing camera with {} name.".format(sensor_name) @@ -176,6 +187,17 @@ def get_imu(self, imu_name): robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance """ return self.__get_sensor(imu_name, 'imu') + + def get_gnss(self, gnss_name): + """Retrieve an specific existing bird eye view + + Arguments: + gnss_name {str} -- Name of the birdeyeview to be retrieved + + Returns: + robot.interfaces.birdeyeview.BirdEyeView instance -- birdeyeview instance + """ + return self.__get_sensor(gnss_name, 'gnss') def kill(self): """Destroy all the running sensors"""