(self, input_data)
| 130 | ] |
| 131 | |
| 132 | def tick(self, input_data): |
| 133 | self.step += 1 |
| 134 | |
| 135 | rgb = cv2.cvtColor(input_data['rgb'][1][:, :, :3], cv2.COLOR_BGR2RGB) |
| 136 | bev = cv2.cvtColor(input_data['bev'][1][:, :, :3], cv2.COLOR_BGR2RGB) |
| 137 | gps = input_data['gps'][1][:2] |
| 138 | speed = input_data['speed'][1]['speed'] |
| 139 | compass = input_data['imu'][1][-1] |
| 140 | |
| 141 | if (math.isnan(compass) == True): #It can happen that the compass sends nan for a few frames |
| 142 | compass = 0.0 |
| 143 | |
| 144 | result = { |
| 145 | 'rgb': rgb, |
| 146 | 'gps': gps, |
| 147 | 'speed': speed, |
| 148 | 'compass': compass, |
| 149 | 'bev': bev |
| 150 | } |
| 151 | |
| 152 | pos = self._get_position(result) |
| 153 | result['gps'] = pos |
| 154 | next_wp, next_cmd = self._route_planner.run_step(pos) |
| 155 | result['next_command'] = next_cmd.value |
| 156 | |
| 157 | |
| 158 | theta = compass + np.pi/2 |
| 159 | R = np.array([ |
| 160 | [np.cos(theta), -np.sin(theta)], |
| 161 | [np.sin(theta), np.cos(theta)] |
| 162 | ]) |
| 163 | |
| 164 | local_command_point = np.array([next_wp[0]-pos[0], next_wp[1]-pos[1]]) |
| 165 | local_command_point = R.T.dot(local_command_point) |
| 166 | result['target_point'] = tuple(local_command_point) |
| 167 | |
| 168 | return result |
| 169 | @torch.no_grad() |
| 170 | def run_step(self, input_data, timestamp): |
| 171 | if not self.initialized: |
no test coverage detected