def main():
turtle = Turtlebot(pc=True, depth=True)
K = turtle.get_depth_K()
#Init map
init_map()
while not turtle.is_shutting_down():
# Process depth image to pointcloud
depth = turtle.get_depth_image()
if depth is None:
print("Depth image is None")
continue
# Pointcloud
pcl = image2cloud(depth, K, True)
fit_pointcloud(pcl)
move = True
turtle.reset_odometry()
move.rotate(turtle, 3.14, verbose=True)
move.translate(turtle, 2, verbose=True)
print("Back in main")
break
def main():
Found_first_beacon = False
Found_second_beacon = False
Found_third_beacon = False
Start_caprure_2D = False
see_map_with_rotation = False
full_rotarion_1 = False
full_rotarion_2 = False
movment1 = False
data = np.empty((2, 1500, 100))
data[:] = np.nan
odom = np.empty((1, 3, 100))
odom[:] = np.nan
counter = 0
sample_T_2D = 5
C = np.array([[], []])
MOVE = 1
ROTATE = 2
running = True
active = True
linear_vel = 0.2
angular_vel = 0.4
turtle = Turtlebot(rgb=True, depth=False, pc=True)
direction = -1
scale = float(12)
grid_width = 150
grid = -1 * np.ones((grid_width, grid_width))
minx_occ = 70
miny_occ = 70
plt.ion()
plt.show()
turtle.reset_odometry()
# get odometry
odometry = turtle.get_odometry()
if np.array_equal(odometry, [0.0, 0.0, 0.0]):
print("Odom reset correctly")
else:
print("Odom is not zero")
return -1
rate = Rate(10)
t = get_time()
# plt.figure(3)
# plt.title('Grid')
# plt.imshow(grid.T, interpolation='none')
# plt.gca().invert_yaxis()
# plt.show()
while not turtle.is_shutting_down():
# get RGB
img_rgb = turtle.get_rgb_image()
# get odometry
odometry = turtle.get_odometry()
odometry = [odometry[1], odometry[0], odometry[2]]
odometry = np.dot(odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
# print("Odometry", odometry)
# get point cloud
pc = turtle.get_point_cloud()
if pc is None:
continue
if img_rgb is not None and pc is not None:
# plt.figure(3)
# plt.title('Grid')
# plt.imshow(grid.T, interpolation='none')
# plt.gca().invert_yaxis()
# plt.show()
# Print 2D and update
if get_time() - t > sample_T_2D:
A = pcl_to_2d(pc)
# get odometry
odometry = turtle.get_odometry()
odometry = [odometry[1], odometry[0], odometry[2]]
odometry = np.dot(odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
R = R_from_ang_single(odometry[2])
T = [[odometry[0]], [odometry[1]]]
map = np.dot(R, A) + T
occ(map * scale, odometry, grid, scale, minx_occ, miny_occ)
t = get_time()
# # C = np.hstack((C, map))
# # print("shape map= ", C.shape)
# # plt.scatter(C[0, :], C[1, :], marker='.')
# # plt.scatter(odometry[0], odometry[1])
# # plt.axis('equal')
# # plt.ylim(-5, 5)
# # plt.xlim(-5, 5)
# # plt.pause(0.001)
# # show image
# cv2.imshow("grid", grid)
# cv2.waitKey(1)
# Start_caprure_2D = True
# Find the 2nd beacon
if Found_first_beacon == True and Found_second_beacon == False:
turtle.cmd_velocity(angular=0.3)
if Found_first_beacon == True and Found_second_beacon == True and see_map_with_rotation == False:
turtle.cmd_velocity(angular=0.7)
if -1.5 <= odometry[2] <= 0:
see_map_with_rotation = True
# np.save("/home/ros/Desktop/aroprojectjim/grid.npy", grid)
# Make full rotation
if 0 < odometry[0] < 2.5 and 2.5 < odometry[1] < 3.4 and (
full_rotarion_1 == False):
print("inside if for rotation")
t = get_time()
print("Full rotation 1 before")
while (get_time() - t < 16):
turtle.cmd_velocity(angular=0.5)
# get RGB
img_rgb = turtle.get_rgb_image()
# get point cloud
pc = turtle.get_point_cloud()
markers = np.array(detect_markers(img_rgb))
# draw markers in the image
detector.draw_markers(img_rgb, markers)
# show image
cv2.imshow("markers", img_rgb)
cv2.waitKey(1)
# Odometry
# print(turtle.get_odometry())
if len(markers) != 0:
id = markers[0, 1]
if id not in id_array and int(id) in range(5, 17):
# Print depth
# plt.figure(2)
# plt.title('depth')
# plt.imshow(depth)
# plt.show(clear)
# find coordinates
# get odometry
odometry = turtle.get_odometry()
odometry = [odometry[1], odometry[0], odometry[2]]
odometry = np.dot(
odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
coordinates_beacon = find_pozition_beacon(
pc, markers, odometry)
if coordinates_beacon != -1:
id_array.append(id)
cord_array.append([
coordinates_beacon[0],
coordinates_beacon[1]
])
if len(id_array) == 1:
print("found first beacon ")
Found_first_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 2:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 3:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 4:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=4)
return 1
print("Full rotation 1 after")
full_rotarion_1 = True
# Make full rotation
if -3 < odometry[0] < 0 and 2.5 < odometry[1] < 3.4 and (
full_rotarion_2 == False):
print("inside if for rotation 2")
t = get_time()
while (get_time() - t < 16):
turtle.cmd_velocity(angular=0.5)
# get RGB
img_rgb = turtle.get_rgb_image()
# get point cloud
pc = turtle.get_point_cloud()
markers = np.array(detect_markers(img_rgb))
# draw markers in the image
detector.draw_markers(img_rgb, markers)
# show image
cv2.imshow("markers", img_rgb)
cv2.waitKey(1)
# Odometry
# print(turtle.get_odometry())
if len(markers) != 0:
id = markers[0, 1]
if id not in id_array and int(id) in range(5, 17):
# Print depth
# plt.figure(2)
# plt.title('depth')
# plt.imshow(depth)
# plt.show(clear)
# find coordinates
# get odometry
odometry = turtle.get_odometry()
odometry = [odometry[1], odometry[0], odometry[2]]
odometry = np.dot(
odometry, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
coordinates_beacon = find_pozition_beacon(
pc, markers, odometry)
if coordinates_beacon != -1:
id_array.append(id)
cord_array.append([
coordinates_beacon[0],
coordinates_beacon[1]
])
if len(id_array) == 1:
print("found first beacon ")
Found_first_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 2:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 3:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 4:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=4)
return 1
full_rotarion_2 = True
# Find the 3rd and 4rd beacon
if Found_first_beacon == True and Found_second_beacon == True and see_map_with_rotation == True:
# mask out floor points
mask = pc[:, :, 1] < 0.2
# mask point too far
mask = np.logical_and(mask, pc[:, :, 2] < 3.0)
if np.count_nonzero(mask) <= 0:
continue
# check obstacle
mask = np.logical_and(mask, pc[:, :, 1] > 0)
mask = np.logical_and(mask, pc[:, :, 1] < 0.1)
data = np.sort(pc[:, :, 2][mask])
state = MOVE
if data.size > 50:
dist = np.percentile(data, 10)
# get odometry
odometry = turtle.get_odometry()
odometry = [odometry[1], odometry[0], odometry[2]]
odometry = np.dot(odometry,
[[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
# print ("Odom before collision function :", odometry)
# col = collision(grid, odometry[0]*scale + minx_occ, odometry[1]*scale + miny_occ)
# print("Colission free ", col )
# print("dist ", dist)
# print("Odom after collision function: ", odometry)
if dist < 0.8:
state = ROTATE
# command velocity
if active and state == MOVE:
turtle.cmd_velocity(linear=linear_vel)
direction = 1
elif active and state == ROTATE:
if direction is None:
direction = np.sign(np.random.rand() - 0.5)
turtle.cmd_velocity(angular=direction * angular_vel)
# End of the random walking
# get RGB
img_rgb = turtle.get_rgb_image()
# get point cloud
pc = turtle.get_point_cloud()
# get odometry
b = turtle.get_odometry()
a = [b[1], b[0], b[2]]
odometry = np.dot(a, [[-1, 0, 0], [0, 1, 0], [0, 0, 1]])
markers = np.array(detect_markers(img_rgb))
# draw markers in the image
detector.draw_markers(img_rgb, markers)
# show image
cv2.imshow("markers", img_rgb)
cv2.waitKey(1)
# Odometry
# print(turtle.get_odometry())
if len(markers) != 0:
id = markers[0, 1]
if id not in id_array and int(id) in range(5, 17):
# Print depth
# plt.figure(2)
# plt.title('depth')
# plt.imshow(depth)
# plt.show(clear)
# find coordinates
# # get odometry
# odometry = turtle.get_odometry()
# odometry = [odometry[1], odometry[0], odometry[2]]
# odometry = np.dot(odometry, [[-1, 0, 0],
# [0, 1, 0],
# [0, 0, 1]])
coordinates_beacon = find_pozition_beacon(
pc, markers, odometry)
if coordinates_beacon != -1:
id_array.append(id)
cord_array.append(
[coordinates_beacon[0], coordinates_beacon[1]])
if len(id_array) == 1:
print("found first beacon ")
Found_first_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 2:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 3:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=3)
if len(id_array) == 4:
Found_second_beacon = True
# play sound
turtle.play_sound(sound_id=4)
return 1
for i in range(len(id_array)):
print("Beacon ", id_array[i], cord_array[i][0], -cord_array[i][1])
