Skip to content

Commit

Permalink
[WIP] TCP brain replicated
Browse files Browse the repository at this point in the history
  • Loading branch information
@sergiopaniego
sergiopaniego committed Jan 31, 2024
1 parent 42e3338 commit 70bb0f2
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 44 deletions.
82 changes: 45 additions & 37 deletions behavior_metrics/brains/CARLA/brain_carla_TCP.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
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.speedometer = sensors.get_speedometer('speedometer_0') # gnss
self.handler = handler
self.inference_times = []
self.gpu_inference = config['GPU']
Expand Down Expand Up @@ -125,7 +126,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
config.trajectory[0].x = 55.3
config.trajectory[0].y = -105.6

config.trajectory[1].x = 2.0
config.trajectory[1].x = -30.0
config.trajectory[1].y = -105.6
print(config.trajectory[0].x, config.trajectory[0].y)
print()
Expand All @@ -139,6 +140,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
self.set_global_plan(gps_route, self.route)
self._init_route_planner()

self.steer_step = 0
self.last_steers = deque()
self.status = 0

Expand All @@ -150,8 +152,8 @@ 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)
#print('-----GLOBAL PLAN -----')
#print(self._global_plan)


def _init_route_planner(self):
Expand All @@ -160,8 +162,8 @@ def _init_route_planner(self):

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)
#print('-----GPS----')
#print(gps)

self.initialized = True

Expand Down Expand Up @@ -193,9 +195,9 @@ def _get_position(self, tick_data):
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-----')
#print('-----GPS-----')
#print(gps)
#print('-----GPS-----')
return gps

@torch.no_grad()
Expand Down Expand Up @@ -224,18 +226,24 @@ def execute(self):
self.update_frame('frame_0', rgb)
self.update_frame('frame_1', seg_image)

velocity = self.vehicle.get_velocity()
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('----------getSpeedometer--------------')
print(self.speedometer.getSpeedometer().data)

speed = self.speedometer.getSpeedometer().data
#velocity = self.vehicle.get_velocity()
#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
gt_velocity = torch.FloatTensor([speed]).to('cuda', dtype=torch.float32)
speed = torch.tensor(speed).view(-1, 1).to('cuda', dtype=torch.float32)
speed = speed / 12

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)
#print('----------compass--------------')
#print(compass)
compass = compass[-1]

if (math.isnan(compass) == True): #It can happen that the compass sends nan for a few frames
Expand Down Expand Up @@ -286,21 +294,21 @@ def execute(self):
'''

print('----------compass--------------')
#print('----------compass--------------')
'''
-0.999806824289457
'''
print(compass)
#print(compass)
#print('--------- POS ---------------')
'''
[ 43.17920366 -105.57289901]
'''
#print(pos)
print('-----------LOCAL COMMAND POINT ---------------')
#print('-----------LOCAL COMMAND POINT ---------------')
'''
[-3.83868739e+01 1.80376380e-02]
'''
print(local_command_point)
#print(local_command_point)
#print('-------NEXT WAYPOINT-----------')
'''
[4.7923297947398655, -105.55486136806194]
Expand All @@ -312,51 +320,51 @@ def execute(self):
-------Target point-----------
tensor([[-3.8387e+01, 1.8038e-02]], device='cuda:0')
'''
print('--------RESULT----------')
#print('--------RESULT----------')
#print(result)
print('------------------')
#print('------------------')

state = torch.cat([speed, self.target_point, cmd_one_hot], 1)

print('------STATE-----')
print(state)
#print('------STATE-----')
#print(state)

rgb = self._im_transform(rgb).unsqueeze(0).to('cuda', dtype=torch.float32)

#print(rgb.shape)

pred = self.net(rgb, state, self.target_point)

print('-----PRED------')
print(pred.keys())
print(pred['pred_wp'])
print('------COMMAND----')
print(command)
#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)
#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)

if brake_traj < 0.05: brake_traj = 0.0
if throttle_traj > brake_traj: brake_traj = 0.0


print('------steer_traj, throttle_traj, brake_traj, metadata_traj-----')
print(steer_traj, throttle_traj, brake_traj, metadata_traj)
#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***********************************************************************************************')
#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***********************************************************************************************')
#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)
Expand All @@ -366,12 +374,12 @@ def execute(self):
if brake_ctrl > 0.5:
throttle_ctrl = float(0)

print('-------------steer_ctrl, throttle_ctrl, brake_ctrl----------')
print(steer_ctrl, throttle_ctrl, brake_ctrl)
#print('-------------steer_ctrl, throttle_ctrl, brake_ctrl----------')
#print(steer_ctrl, throttle_ctrl, brake_ctrl)

print()
print()
print()
#print()
#print()
#print()

self.motors.sendThrottle(throttle_ctrl)
self.motors.sendSteer(steer_ctrl)
Expand Down
65 changes: 58 additions & 7 deletions behavior_metrics/brains/CARLA/brain_carla_TCP_2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,14 +130,55 @@ def set_global_plan(self, global_plan_gps, global_plan_world_coord, wp=None):
for x in ds_ids:
global_plan_gps[x][0]['lat'], global_plan_gps[x][0]['lon'] = global_plan_gps[x][0]['lon'], global_plan_gps[x][0]['lat']

def update_frame(self, frame_id, data):
"""Update the information to be shown in one of the GUI's frames.
Arguments:
frame_id {str} -- Id of the frame that will represent the data
data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
"""
if data.shape[0] != data.shape[1]:
if data.shape[0] > data.shape[1]:
difference = data.shape[0] - data.shape[1]
extra_left, extra_right = int(difference/2), int(difference/2)
extra_top, extra_bottom = 0, 0
else:
difference = data.shape[1] - data.shape[0]
extra_left, extra_right = 0, 0
extra_top, extra_bottom = int(difference/2), int(difference/2)


data = np.pad(data, ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), mode='constant', constant_values=0)

self.handler.update_frame(frame_id, data)

def tick(self):
rgb = self.camera_rgb.getImage().data
self.update_frame('frame_0', rgb)
speed = self.speedometer.getSpeedometer().data
#velocity = self.vehicle.get_velocity()
#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

imu_data = self.imu.getIMU()
compass = np.array([imu_data.compass.x, imu_data.compass.y, imu_data.compass.z])
compass = compass[-1]

compass = np.array([imu_data.compass.x, imu_data.compass.y, imu_data.compass.z, imu_data.compass.w])

def convert_vector_to_compass_orientation(orientation_vector):
_, _, orientation_x, orientation_y = orientation_vector

compass_orientation = math.atan2(round(orientation_y, 2), round(orientation_x, 2))

if compass_orientation < 0:
compass_orientation += 2 * math.pi

compass_orientation -= math.pi / 2.0

return compass_orientation

compass = convert_vector_to_compass_orientation(compass)


result = {
'rgb': rgb,
#'gps': gps,
Expand All @@ -150,17 +191,16 @@ def tick(self):
next_wp, next_cmd = self._route_planner.run_step(pos)

result['next_command'] = next_cmd.value
# Custom compass update
#compass = ((compass + 1) / 2) * 0.02 - 0.01

theta = compass + np.pi/2
R = np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]
])

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[0], local_command_point[1] = local_command_point[1], -local_command_point[0]t
local_command_point = R.T.dot(local_command_point)
local_command_point[0], local_command_point[1] = local_command_point[1], -local_command_point[0]

result['target_point'] = tuple(local_command_point)

Expand Down Expand Up @@ -190,6 +230,10 @@ 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(tick_data['rgb'].shape)
print(type(tick_data['rgb']))

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]]),
Expand Down Expand Up @@ -252,3 +296,10 @@ def execute(self):
self.motors.sendSteer(steer)
self.motors.sendBrake(brake)


'''
TODO: Draw the waypoints on the map
TODO: Draw the trajectory on the images.
TODO: Figure out what's the transformation for the compass.
'''

0 comments on commit 70bb0f2

Please sign in to comment.