Fixed a bug in Waypoint controller and optimized a parameter. Changed...

Fixed a bug in Waypoint controller and optimized a parameter. Changed transfuser lidar repeat to waypoint repeat. Added some more debug code.

leaderboard/team_code/transfuser_agent.py

@@ -56,6 +56,8 @@ class TransFuserAgent(autonomous_agent.AutonomousAgent):
 			self.save_path.mkdir(parents=True, exist_ok=False)
 
 			(self.save_path / 'rgb').mkdir(parents=True, exist_ok=False)
+			(self.save_path / 'lidar_0').mkdir(parents=True, exist_ok=False)
+			(self.save_path / 'lidar_1').mkdir(parents=True, exist_ok=False)
 			(self.save_path / 'meta').mkdir(parents=True, exist_ok=False)
 
 	def _init(self):
@@ -238,6 +240,7 @@ class TransFuserAgent(autonomous_agent.AutonomousAgent):
 		ego_theta = self.input_buffer['thetas'][-1]
 		ego_x, ego_y = self.input_buffer['gps'][-1]
 
+		#Only predict every second step because we only get a LiDAR every second frame.
 		if(self.step  % 2 == 0 or self.step <= 4):
 			for i, lidar_point_cloud in enumerate(self.input_buffer['lidar']):
 				curr_theta = self.input_buffer['thetas'][i]
@@ -248,22 +251,23 @@ class TransFuserAgent(autonomous_agent.AutonomousAgent):
 				lidar_transformed = torch.from_numpy(lidar_to_histogram_features(lidar_transformed, crop=self.config.input_resolution)).unsqueeze(0)
 				self.lidar_processed = list()
 				self.lidar_processed.append(lidar_transformed.to('cuda', dtype=torch.float32))
-			#from matplotlib import pyplot as plt
-			#x = self.lidar_processed[0].cpu().numpy()[0, 0]
-			#plt.imshow(x, interpolation='nearest')
-			#plt.show()
-			#plt.imshow(self.lidar_processed[0].cpu().numpy()[0, 1], interpolation='nearest')
-			#plt.show()
-			#plt.imshow(np.transpose(self.input_buffer['rgb'][0].cpu().numpy()[0], (1, 2, 0)).astype(np.int32), interpolation='nearest')
-			#plt.show()
-
-		pred_wp = self.net(self.input_buffer['rgb'] + self.input_buffer['rgb_left'] + \
-						   self.input_buffer['rgb_right']+self.input_buffer['rgb_rear'], \
-						   self.lidar_processed, target_point, gt_velocity)
-		steer, throttle, brake, metadata = self.net.control_pid(pred_wp, gt_velocity)
+
+
+			self.pred_wp = self.net(self.input_buffer['rgb'] + self.input_buffer['rgb_left'] + \
+							   self.input_buffer['rgb_right']+self.input_buffer['rgb_rear'], \
+							   self.lidar_processed, target_point, gt_velocity)
+
+		steer, throttle, brake, metadata = self.net.control_pid(self.pred_wp, gt_velocity)
 		self.pid_metadata = metadata
 
-		
+		# from matplotlib import pyplot as plt
+		# x = self.lidar_processed[0].cpu().numpy()[0, 0]
+		# plt.imshow(x, interpolation='nearest')
+		# plt.show()
+		# plt.imshow(self.lidar_processed[0].cpu().numpy()[0, 1], interpolation='nearest')
+		# plt.show()
+		# plt.imshow(np.transpose(self.input_buffer['rgb'][0].cpu().numpy()[0], (1, 2, 0)).astype(np.int32), interpolation='nearest')
+		# plt.show()
 
 		if brake < 0.05: brake = 0.0
 		if throttle > brake: brake = 0.0
@@ -273,16 +277,22 @@ class TransFuserAgent(autonomous_agent.AutonomousAgent):
 		control.throttle = float(throttle)
 		control.brake = float(brake)
 
-		#if SAVE_PATH is not None and self.step % 10 == 0:
+		#if SAVE_PATH is not None and self.step % 1 == 0: #TODO revert
 		#	self.save(tick_data)
 
 		return control
 
 	def save(self, tick_data):
-		frame = self.step // 10
+		frame = self.step // 1 #TODO revert
+
+		from matplotlib import cm
 
 		Image.fromarray(tick_data['rgb']).save(self.save_path / 'rgb' / ('%04d.png' % frame))
 
+		Image.fromarray(cm.gist_earth(self.lidar_processed[0].cpu().numpy()[0, 0], bytes=True)).save(self.save_path / 'lidar_0' / ('%04d.png' % frame))
+		Image.fromarray(cm.gist_earth(self.lidar_processed[0].cpu().numpy()[0, 1], bytes=True)).save(self.save_path / 'lidar_1' / ('%04d.png' % frame))
+
+
 		outfile = open(self.save_path / 'meta' / ('%04d.json' % frame), 'w')
 		json.dump(self.pid_metadata, outfile, indent=4)
 		outfile.close()

transfuser/config.py

@@ -54,7 +54,7 @@ class GlobalConfig:
     speed_n = 40 # buffer size
 
     max_throttle = 0.75 # upper limit on throttle signal value in dataset
-    brake_speed = 0.4 # desired speed below which brake is triggered
+    brake_speed = 0.1 # desired speed below which brake is triggered
     brake_ratio = 1.1 # ratio of speed to desired speed at which brake is triggered
     clip_delta = 0.25 # maximum change in speed input to logitudinal controller
 

transfuser/model.py

@@ -481,14 +481,16 @@ class TransFuser(nn.Module):
         waypoints[:,1] *= -1
         speed = velocity[0].data.cpu().numpy()
 
+        desired_speed = np.linalg.norm(waypoints[0] - waypoints[1]) * 2.0
+        brake = desired_speed < self.config.brake_speed or (speed / desired_speed) > self.config.brake_ratio
+
         aim = (waypoints[1] + waypoints[0]) / 2.0
         angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90
+        if((speed < 0.01) and (brake == True)):
+            angle = np.array(0.0) # When we don't move we don't want the angle error to accumulate in the integral
         steer = self.turn_controller.step(angle)
         steer = np.clip(steer, -1.0, 1.0)
 
-        desired_speed = np.linalg.norm(waypoints[0] - waypoints[1]) * 2.0
-        brake = desired_speed < self.config.brake_speed or (speed / desired_speed) > self.config.brake_ratio
-
         delta = np.clip(desired_speed - speed, 0.0, self.config.clip_delta)
         throttle = self.speed_controller.step(delta)
         throttle = np.clip(throttle, 0.0, self.config.max_throttle)