def main():
turtle = Turtlebot(pc=True)
cv2.namedWindow(WINDOW)
while not turtle.is_shutting_down():
# get point cloud
pc = turtle.get_point_cloud()
if pc is None:
continue
# mask out floor points
mask = pc[:, :, 1] > x_range[0]
# mask point too far and close
mask = np.logical_and(mask, pc[:, :, 2] > z_range[0])
mask = np.logical_and(mask, pc[:, :, 2] < z_range[1])
if np.count_nonzero(mask) <= 0:
continue
# empty image
image = np.zeros(mask.shape)
# assign depth i.e. distance to image
image[mask] = np.int8(pc[:, :, 2][mask] / 3.0 * 255)
im_color = cv2.applyColorMap(255 - image.astype(np.uint8),
cv2.COLORMAP_JET)
# show image
cv2.imshow(WINDOW, im_color)
cv2.waitKey(1)
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():
turtle = Turtlebot(pc=True)
direction = None
cv2.namedWindow(WINDOW)
cv2.setMouseCallback(WINDOW, click)
while not turtle.is_shutting_down():
# get point cloud
pc = turtle.get_point_cloud()
if pc is None:
continue
# 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
# empty image
image = np.zeros(mask.shape)
# assign depth i.e. distance to image
image[mask] = np.int8(pc[:, :, 2][mask] / 3.0 * 255)
im_color = cv2.applyColorMap(255 - image.astype(np.uint8),
cv2.COLORMAP_JET)
# show image
cv2.imshow(WINDOW, im_color)
cv2.waitKey(1)
# check obstacle
mask = np.logical_and(mask, pc[:, :, 1] > -0.2)
data = np.sort(pc[:, :, 2][mask])
state = MOVE
if data.size > 50:
dist = np.percentile(data, 10)
if dist < 0.6:
state = ROTATE
# command velocity
if active and state == MOVE:
turtle.cmd_velocity(linear=linear_vel)
direction = None
elif active and state == ROTATE:
if direction is None:
direction = np.sign(np.random.rand() - 0.5)
turtle.cmd_velocity(angular=direction * angular_vel)
def main():
# Initialize turtlebot class
turtle = Turtlebot()
# Register bumper callback
turtle.register_bumper_event_cb(bumper_cb)
# Do something, the program would end otherwise
rate = Rate(1)
while not turtle.is_shutting_down():
rate.sleep()
def main():
# Initialize turtlebot class
turtle = Turtlebot()
# Register bumper callback
turtle.register_bumper_event_cb(bumper_cb)
# Do something, the program would end otherwise
rate = Rate(1)
while not turtle.is_shutting_down():
if isPressed == 1:
turtle.cmd_velocity(linear=0)
break
elif isPressed == 0:
turtle.cmd_velocity(linear=0.1)
def main():
turtle = Turtlebot(rgb=True)
cv2.namedWindow(WINDOW)
while not turtle.is_shutting_down():
# get point cloud
image = turtle.get_rgb_image()
# wait for image to be ready
if image is None:
continue
# detect markers in the image
markers = detector.detect_markers(image)
# draw markers in the image
detector.draw_markers(image, markers)
# show image
cv2.imshow(WINDOW, image)
cv2.waitKey(1)
from turtlebot import Turtlebot
import numpy as np
turtle = Turtlebot(pc=True, rgb=True, depth=True)
while not turtle.is_shutting_down():
depth = turtle.get_depth_image()
rgb = turtle.get_rgb_image()
point_cloud = turtle.get_point_cloud()
rgb_K = turtle.get_rgb_K()
depth_K = turtle.get_depth_K()
if rgb is None:
continue
if depth is None:
continue
if point_cloud is None:
continue
if rgb_K is None:
continue
if depth_K is None:
continue
np.save('exports/depth.npy', depth)
np.save('exports/rgb.npy', rgb)
np.save('exports/point_cloud.npy', point_cloud)
np.save('exports/rgb_K.npy', rgb_K)
np.save('exports/depth_K.npy', depth_K)
exit(0)
def main():
maximum = 10
count = 0
speed = 30
s = 0.0001
prev_start = [[0] * 2 for i in range(maximum)]
prev_end = [[0] * 2 for i in range(maximum)]
circle = [[0] * 2 for i in range(maximum)]
turtle = Turtlebot(rgb=True)
cv2.namedWindow("OKNO")
while not turtle.is_shutting_down():
# get point cloud
image = turtle.get_rgb_image()
# wait for image to be ready
if image is None:
continue
cv2.rectangle(image, (0, 0), (image.shape[1], int(image.shape[0] / 1.6)), (0, 0, 0), -1)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
kernel_size = 5
blur_gray = cv2.GaussianBlur(gray, (kernel_size, kernel_size), 0)
ret, black = cv2.threshold(blur_gray, 215, 255, cv2.THRESH_BINARY)
edges = cv2.Canny(black, 200, 255)
rho = 2 # distance resolution in pixels of the Hough grid
theta = np.pi / 60 # angular resolution in radians of the Hough grid
threshold = 55 # minimum number of votes (intersections in Hough grid cell)
min_line_length = 5 # minimum number of pixels making up a line
max_line_gap = 40 # maximum gap in pixels between connectable line segments
line_image = np.copy(image) * 0 # creating a blank to draw lines on
lines = cv2.HoughLinesP(edges, rho, theta, threshold, np.array([]), min_line_length, max_line_gap)
left_line_x = []
left_line_y = []
right_line_x = []
right_line_y = []
Rmotor = 0
Lmotor = 0
if lines is not None:
for line in lines:
for x1, y1, x2, y2 in line:
cv2.line(line_image, (x1, y1), (x2, y2), (0, 0, 255), 1)
if (x2 - x1) == 0:
continue
slope = (y2 - y1) / (x2 - x1) # <-- Calculating the slope.
if math.fabs(slope) < 0.5: # <-- Only consider extreme slope
continue
if slope <= 0: # <-- If the slope is negative, left group.
left_line_x.extend([x1, x2])
left_line_y.extend([y1, y2])
else: # <-- Otherwise, right group.
right_line_x.extend([x1, x2])
right_line_y.extend([y1, y2])
min_y = image.shape[0] / 1.3 # <-- Just below the horizon
max_y = image.shape[0] # <-- The bottom of the image
left_x_start = 0
left_x_end = 0
right_x_start= image.shape[1]
right_x_end = image.shape[1]
if len(left_line_x) != 0:
poly_left = np.poly1d(np.polyfit(
left_line_y,
left_line_x,
deg=1
))
left_x_start = int(poly_left(max_y))
left_x_end = int(poly_left(min_y))
cv2.line(line_image, (int(left_x_start), int(max_y)), (int(left_x_end), int(min_y)), (0, 255, 0), 2)
if len(right_line_x) != 0:
poly_right = np.poly1d(np.polyfit(
right_line_y,
right_line_x,
deg=1
))
right_x_start = int(poly_right(max_y))
right_x_end = int(poly_right(min_y))
cv2.line(line_image, (int(right_x_start), int(max_y)), (int(right_x_end), int(min_y)), (0, 255, 0), 2)
prev_start[count][0] = right_x_start
prev_start[count][1] = left_x_start
prev_end[count][0] = right_x_end
prev_end[count][1] = left_x_end
# print(prev_start)
# print(prev_end)
avg_start = [0] * 2
avg_end = [0] * 2
for i in range(maximum):
avg_start[0] += prev_start[i][0]
avg_start[1] += prev_start[i][1]
avg_end[0] += prev_end[i][0]
avg_end[1] += prev_end[i][1]
avg_start[0] /= maximum
avg_start[1] /= maximum
avg_end[0] /= maximum
avg_end[1] /= maximum
center_x_start = int((avg_start[0] + avg_start[1]) / 2)
center_x_end = int((avg_end[0] + avg_end[1]) / 2)
Rmotor = speed + int(
np.arctan2((center_x_start - center_x_end), (max_y - min_y)) * (180.0 / 3.141592653589793238463))
Lmotor = speed + int(
np.arctan2((center_x_end - center_x_start), (max_y - min_y)) * (180.0 / 3.141592653589793238463))
Rmotor = 100 if Rmotor > 100 else Rmotor
Lmotor = 100 if Lmotor > 100 else Lmotor
cv2.line(line_image, (center_x_start, int(max_y)), (center_x_end, int(min_y)), (255, 0, 0), 3)
cv2.line(line_image, (int(image.shape[1] / 2), int(max_y)),
(int((image.shape[1] / 2 - center_x_start) + center_x_end), int(min_y)), (255, 255, 0), 4)
lines_edges = cv2.addWeighted(image, 0.8, line_image, 1, 0)
cv2.putText(lines_edges, str(Lmotor), (int(image.shape[1] / 6), 200), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255),
4);
cv2.putText(lines_edges, str(Rmotor), (int(image.shape[1] - image.shape[1] / 6), 200), cv2.FONT_HERSHEY_SIMPLEX,
1, (0, 0, 255), 4);
circle[count][0], circle[count][1] = line_intersection(
([int(left_x_start), int(max_y)], [int(left_x_end), int(min_y)]),
([int(right_x_start), int(max_y)], [int(right_x_end), int(min_y)]))
circleX = 0
circleY = 0
n = 0
for i in range(maximum):
if circle[i][0] is None:
n += 1
continue
circleX += circle[i][0]
circleY += circle[i][1]
if maximum-n != 0:
circleX /= (maximum - n)
circleY /= (maximum - n)
if circleX != 0 and circleY != 0:
uhel = int(np.arctan2((image.shape[1] / 2 - circleX), (max_y - circleY)) * (180.0 / 3.141592653589793238463))
if circleX > image.shape[1] / 2 - 40 and circleX < image.shape[1] / 2 + 40:
cv2.circle(lines_edges, (int(circleX), int(circleY)), 8, (0, 255, 0), -1)
turtle.cmd_velocity(linear=0.05+s)
s += 0.001
else:
cv2.circle(lines_edges, (int(circleX), int(circleY)), 8, (0, 0, 255), -1)
turtle.cmd_velocity(angular=np.sign(uhel)*0.5)
s = 0.0001
cv2.line(lines_edges, (int(image.shape[1] / 2 - (circleX - center_x_start)), int(image.shape[0] - 2)),
(int(image.shape[1] / 2), int(image.shape[0]) - 2), (0, 0, 255), 3)
cv2.putText(lines_edges, str(uhel), (int(image.shape[1]/2), 200),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 4);
count += 1
if count == maximum:
count = 0
cv2.line(lines_edges, (int(image.shape[1] / 2 - 40), 0), (int(image.shape[1] / 2 - 40), int(image.shape[0])),
(0, 211, 255), 1)
cv2.line(lines_edges, (int(image.shape[1] / 2 + 40), 0), (int(image.shape[1] / 2 + 40), int(image.shape[0])),
(0, 211, 255), 1)
cv2.imshow('lines', line_image)
cv2.imshow('linesedge', lines_edges)
# show image
cv2.imshow("OKNO", image)
cv2.imshow("OKNO1", edges)
cv2.waitKey(1)
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])
