def check_orientation(self, current_pose2D, destination2D):
desired_angle = destination_to_angle(current_pose2D[0], destination2D[0])
current_angle = destination_to_angle(
current_pose2D[0], current_pose2D[1], flip=True
)
# arguments are reversed (y, x)
direction_error_marigin = 40
if abs(desired_angle - current_angle) < np.deg2rad(direction_error_marigin):
return True
else:
return False
def assign_state(self, current_pose2D, destination2D):
self.reset_states()
logger.debug("a_t {}".format(self.a_t))
if self.a_t == 0:
if self.now_resting:
logger.debug("Now rest")
self.now_resting = False
self.states["resting"] = True
return
else:
self.now_resting = True
if current_pose2D is None:
logger.debug("No pose!!!")
self.states["orienting"] = True
self.states["resting"] = False
if not self.navigator_queue.empty():
message = self.navigator_queue.get()
logger.debug("Got navigator message: {}".format(message))
# @TODO turn off after one tick
self.states["at_checkpoint"] = True
if current_pose2D is not None:
oriented = self.check_orientation(current_pose2D, destination2D)
if oriented:
self.states["approaching"] = True
logger.debug("Approaching: {}".format(self.a_t))
else:
desired_angle = destination_to_angle(
current_pose2D[0], destination2D[0], flip=True
)
current_angle = destination_to_angle(
current_pose2D[0], current_pose2D[1]
)
diff = desired_angle - current_angle
n = abs(diff) // np.pi
new_sign = np.sign(diff) * ((-1) ** (n))
diff2 = new_sign * (abs(diff) % np.pi)
turning_type = "turning_left" if diff2 >= 0 else "turning_right"
self.states[turning_type] = True
logger.debug(
"Desired {}, Current {}, Diff {}, Diff2{} Turning {}: {}".format(
np.rad2deg(desired_angle),
np.rad2deg(current_angle),
np.rad2deg(diff),
np.rad2deg(diff2),
turning_type,
self.a_t,
)
)
def calculate_steering(self, current_pose2D, destination2D):
desired_angle = destination_to_angle(current_pose2D[0], destination2D[0])
(x1, y1), phi1 = current_pose2D
(x2, y2), phi2 = destination2D
phi1_d = phi1 + np.pi
output = self.pid(desired_angle - phi1_d)
return output
def visualize(self):
self._buffer = self.make_background()
checkpoints = [
self.transformator(checkpoint)[0]
for checkpoint in self.navigator.checkpoints
]
checkpoints.append(self.destination2d[0])
all_objects = [ckp for ckp in checkpoints]
all_objects.append(self.current2d[0])
if self.to_screen_transform is None:
self.to_screen_transform = self.calculate_transformation(
all_objects)
checkpoints_on_screen = [
self.to_screen_transform(checkpoint) for checkpoint in checkpoints
]
self.draw_checkpoints(checkpoints_on_screen)
destination_on_screen = self.to_screen_transform(self.destination2d[0])
self.draw_destination(destination_on_screen)
x, y = self.current2d[0]
l = 0.05 * self.window_size
angle = destination_to_angle(self.current2d[0], self.destination2d[0])
u = x + l * np.cos(angle)
v = y + l * np.sin(angle)
curr_ang = destination_to_angle(self.current2d[0],
self.current2d[1],
flip=True)
k = x + l * np.cos(curr_ang)
m = y + l * np.sin(curr_ang)
current_on_screen = self.to_screen_transform(self.current2d[0])
uv = self.to_screen_transform((u, v))
km = self.to_screen_transform((k, m))
self.draw_car(current_on_screen, uv, km)
# print("=======================")
# print(self.current3d)
# print(self.current3d.dot(self.transformator.pose_transformer.transform))
# print(self.transformator.pose_transformer.transform.dot(self.current3d))
# print(self.current2d)
# print("=======================")
# print(
# "On screen: Current [{}] Destination [{}] Angle[{}]".format(
# current_on_screen, destination_on_screen, angle
# )
# )
# print(
# "In 2D: Current [{}] Destination [{}] Angle[{}]".format(
# current, destination, angle
# )
# )
# print()
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.4
color = (0, 0, 255)
thickness = 1
org = (30, 20)
text = "screen: c [{}] d[{}] a [{}]".format(current_on_screen,
destination_on_screen,
np.rad2deg(angle))
text = "world: c [{cx:.3f},{cy:.3f}] d [{dx:.3f},{dy:.3f}] a [{a:.3f}]".format(
cx=self.current2d[0][0],
cy=self.current2d[0][1],
dx=self.destination2d[0][0],
dy=self.destination2d[0][1],
a=np.rad2deg(angle),
)
self._buffer = cv2.putText(self._buffer, text, org, font, fontScale,
color, thickness, cv2.LINE_AA)
cv2.imshow("vis", self._buffer)
cv2.waitKey(30)
[
(a, b),
(perspective.canvas_shape[0] - a, b),
(perspective.destination[2][0] - 8, mask.shape[1] - 8),
(perspective.destination[3][0] + 8, mask.shape[1] - 8),
]
]
).astype(np.int32)
print(polygon)
perspective.calculate_roi_mask(polygon)
for kf_id, pose, ts, filename, img in data_iterator(
id_and_pose, args.images_path, args.assoc_file
):
if True:
undi = calibration.undistort(img, use_roi=True)
undi[:100, :, :] = 0
cv2.imshow("preinv", undi)
img = perspective.inverse(undi)
img = cv2.bitwise_and(img, img, mask=perspective.roi_mask)
cv2.imshow("persp", img)
cv2.waitKey(0)
pose_world = to_world(pose)
position, normal_vec = transformator(pose_world)
position = position[:2]
angle = destination_to_angle(position, normal_vec)
offset_position = np.array((position[0], position[1]))
mapper.update(img, angle=180 + np.rad2deg(angle), position=offset_position)
cv2.imshow("map", mapper.map)
cv2.waitKey(0)