(self, pos, theta, target)
| 143 | self.initialized = True |
| 144 | |
| 145 | def _get_angle_to(self, pos, theta, target): |
| 146 | R = np.array([ |
| 147 | [np.cos(theta), -np.sin(theta)], |
| 148 | [np.sin(theta), np.cos(theta)], |
| 149 | ]) |
| 150 | |
| 151 | aim = R.T.dot(target - pos) |
| 152 | angle = -np.degrees(np.arctan2(-aim[1], aim[0])) |
| 153 | angle = 0.0 if np.isnan(angle) else angle |
| 154 | |
| 155 | return angle |
| 156 | |
| 157 | |
| 158 | def _truncate_global_route_till_local_target(self, windows_size=5): |