def __init__(self, skip=False):
self.encoder = Encoder()
if skip is False:
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
self.rotation_Kp = 0.75
self.wall_distance_change = 0.2
self.dist_threshold = 1
self.front_adjustment_Kp = 1.5
self.dist_from_front_wall = 7
self.rotation_mult = 1.3
self.forward_mult = 1.1
self.wall_following_Kp = 0.5
self.orientation = 'n'
self.cell_size = 18
self.max_front_distance = self.cell_size / 1.5
self.max_side_distance = self.cell_size / 1.5
def __init__(self):
self.encoder = Encoder()
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
def __init__(self, skip=False):
self.encoder = Encoder()
if skip is False:
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
self.orientation = 'n'
self.cell_size = 18
self.max_front_distance = 13
class Robot():
def __init__(self, skip=False):
self.encoder = Encoder()
if skip is False:
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
self.rotation_Kp = 0.75
self.wall_distance_change = 0.2
self.dist_threshold = 1
self.front_adjustment_Kp = 1.5
self.dist_from_front_wall = 7
self.rotation_mult = 1.3
self.forward_mult = 1.1
self.wall_following_Kp = 0.5
self.orientation = 'n'
self.cell_size = 18
self.max_front_distance = self.cell_size / 1.5
self.max_side_distance = self.cell_size / 1.5
def stop(self):
print("Exiting Robot")
self.encoder.ctrlC()
self.camera.ctrlC()
self.distance_sensor.ctrlC()
exit()
def ctrlC(self, signum, frame):
self.stop()
#getters and setters
def goal_in_front(self, val=None):
#goal is in front of the robot
if val is not None:
self.GIF = val
# else:
return self.GIF
def no_wall_detected(self, val=None):
#No wall is in front of the robot within 10 cm
if val is not None:
self.no_wall = val
return self.no_wall
def less_than_10cm(self, val=None):
#something is in front of the robot within 10 cm that is not the goal
if val is not None:
self.less_than_10 = val
return self.less_than_10
def stop_range(self, val=None):
#robot front is within proper distance +- error of goal
if val is not None:
self.stop_r = val
return self.stop_r
#from faceGoal.py
def check_goal_in_front(self):
print("Checking GIF()")
#checks if goal is in front within error range
#sets robot variable accordingly and returns True or False
blobs = self.camera.get_blobs()
ERROR = 0.5
Kp = 0.009
t_set = []
#Find largest blob
largest = -1
size = 0.0
for i in range(len(blobs)):
if blobs[i].size > size:
size = blobs[i].size
largest = i
for x in range(0, 20):
if len(blobs) > 0:
self.blob_x = blobs[largest].pt[0]
if ((Kp * (blobs[largest].pt[0] - 320)) > -ERROR) and (
(Kp * (blobs[largest].pt[0] - 320)) < ERROR):
if ((Kp * (blobs[largest].pt[1] - 240) > -2 * ERROR) and
((Kp * (blobs[largest].pt[1] - 240)) < 2 * ERROR)):
## self.goal_in_front(True)
print("GIF True")
t_set.append(True)
else:
## self.goal_in_front(False)
print("GIF False because too high")
t_set.append(False)
else:
## self.goal_in_front(False)
print("GIF False goal center not in error range")
t_set.append(False)
else:
self.blob_x = 1000
## self.goal_in_front(False)
print("GIF False no blobs")
t_set.append(False)
if any(t_set):
self.goal_in_front(True)
return True
else:
self.goal_in_front(False)
return False
def check_no_wall_in_front(self):
f_distance = self.distance_sensor.get_front_inches()
GIF = self.check_goal_in_front()
if (GIF):
if (f_distance > 3 * 5):
print("GIF but also a wall")
self.no_wall_detected(True)
return True
else:
self.no_wall_detected(False)
return False
def rotate(self, direction='r'):
rotations = (self.encoder.WSEPARATION * math.pi /
4) / (self.encoder.WDIAMETER * math.pi)
ticks = int(rotations * 32 * self.rotation_mult)
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
max_forward = self.encoder.get_max_forward_speed()
max_backward = self.encoder.get_max_backward_speed()
speed = min(self.encoder.get_max_forward_speed(),
-self.encoder.get_max_backward_speed())
if direction.lower() == 'r':
self.encoder.setSpeedsIPS(speed, -speed)
if self.orientation == 'n':
self.orientation = 'e'
elif self.orientation == 'e':
self.orientation = 's'
elif self.orientation == 's':
self.orientation = 'w'
else:
self.orientation = 'n'
else:
self.encoder.setSpeedsIPS(-speed, speed)
if self.orientation == 'n':
self.orientation = 'w'
elif self.orientation == 'e':
self.orientation = 'n'
elif self.orientation == 's':
self.orientation = 'e'
else:
self.orientation = 's'
while (self.encoder.steps_to_move[0] !=
-1) or (self.encoder.steps_to_move[1] != -1):
proportional_speed = self.rotation_Kp * (
self.encoder.steps_to_move[0] - self.encoder.step_count[0])
proportional_control = self.saturation_function(
proportional_speed, speed, -speed)
if direction.lower() == 'r':
proportional_control = -proportional_control
self.encoder.setSpeedsIPS(proportional_control,
-proportional_control)
time.sleep(0.01)
self.encoder.stop()
time.sleep(0.05)
def get_right_dir(self):
if self.orientation == 'n':
return 'e'
elif self.orientation == 'e':
return 's'
elif self.orientation == 's':
return 'w'
else:
return 'n'
def get_left_dir(self):
if self.orientation == 'n':
return 'w'
elif self.orientation == 'w':
return 's'
elif self.orientation == 's':
return 'e'
else:
return 'n'
def get_back_dir(self):
if self.orientation == 'n':
return 's'
elif self.orientation == 'w':
return 'e'
elif self.orientation == 's':
return 'n'
else:
return 'w'
def change_orientation(self, direction):
if direction.lower() == 'n':
if self.orientation == 'n':
return
elif self.orientation == 'e':
self.rotate('l')
elif self.orientation == 's':
self.rotate('l')
time.sleep(0.1)
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 'e':
if self.orientation == 'e':
return
elif self.orientation == 's':
self.rotate('l')
elif self.orientation == 'w':
self.rotate('l')
time.sleep(0.1)
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 's':
if self.orientation == 's':
return
elif self.orientation == 'w':
self.rotate('l')
elif self.orientation == 'n':
self.rotate('l')
time.sleep(0.1)
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 'w':
if self.orientation == 'w':
return
elif self.orientation == 'n':
self.rotate('l')
elif self.orientation == 'e':
self.rotate('l')
time.sleep(0.1)
self.rotate('l')
else:
self.rotate('r')
#moves robot forward
#returns whether or not it stopped early
def forward(self):
rotations = (self.cell_size / 2) / (self.encoder.WDIAMETER * math.pi)
ticks = int(rotations * 32 * self.forward_mult)
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
next_cell = NextCell(self)
time.sleep(0.05)
follow_both(self, next_cell, next_cell.move_to_cell,
self.wall_following_Kp, self.dist_from_front_wall)
self.encoder.stop()
if self.distance_sensor.get_front_inches() < self.max_front_distance:
return True
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
next_cell.initialize_walls()
time.sleep(0.05)
follow_both(self, next_cell, next_cell.center_in_cell,
self.wall_following_Kp, self.dist_from_front_wall)
self.encoder.stop()
time.sleep(0.05)
return False
def adjust_and_check_colors(self, mapper):
if (self.distance_sensor.get_front_inches() < self.max_front_distance) \
and (abs(self.distance_sensor.get_front_inches() - self.dist_from_front_wall) > self.dist_threshold):
self.adjust_front_distance()
color = self.camera.color_get()
# while color == False:
# print("color = false")
# print(color)
# color = self.mapper.rob.camera.color_get()
if color is not None:
if color == 0:
mapper.color_locations['g'] = [
mapper.current_y, mapper.current_x
]
elif color == 1:
mapper.color_locations['o'] = [
mapper.current_y, mapper.current_x
]
elif color == 2:
mapper.color_locations['p'] = [
mapper.current_y, mapper.current_x
]
elif color == 3:
mapper.color_locations['b'] = [
mapper.current_y, mapper.current_x
]
time.sleep(0.05)
def adjust_front_distance(self):
max_forward = self.encoder.get_max_forward_speed() / 2
max_backward = self.encoder.get_max_backward_speed() / 2
while abs(self.distance_sensor.get_front_inches() -
self.dist_from_front_wall) > self.dist_threshold:
proportional_speed = self.front_adjustment_Kp * (
self.dist_from_front_wall -
self.distance_sensor.get_front_inches())
proportional_control = self.saturation_function(
proportional_speed, max_forward, max_backward)
self.encoder.setSpeedsIPS(proportional_control,
proportional_control)
time.sleep(0.01)
self.encoder.stop()
def saturation_function(self, proportional_speed, max_forward_speed,
max_backward_speed):
if proportional_speed > 0.1:
if -proportional_speed < max_backward_speed:
return max_backward_speed
else:
return -proportional_speed
elif proportional_speed < -0.1:
if -proportional_speed > max_forward_speed:
return max_forward_speed
else:
return -proportional_speed
else:
return 0
data.pose.position.z)
def click_callback(event, x, y, flags, param):
global clicked_coords, clicked_R, clicked_T, clicked_sd_coords, R, T
if event == cv2.EVENT_RBUTTONUP:
clicked_coords = (x / display_scale, y / display_scale)
# clicked_coords = (160, 120)
print 'clicked coords', clicked_coords
clicked_R = deepcopy(R)
clicked_T = deepcopy(T)
clicked_sd_coords = deepcopy(sd_coords)
if __name__ == '__main__':
c = Camera()
rospy.init_node('pixels')
rospy.Subscriber("/pidrone/est_pos", PoseStamped, drone_callback)
rospy.Subscriber("/pidrone/sd_pos", PoseStamped, sd_callback)
with open("output.txt", "a") as output:
for img in c.getImage():
while R is None or T is None or sd_coords is None or d is None:
time.sleep(0.001)
print 'got new image'
global clicked_coords
cv2.namedWindow('preview')
cv2.setMouseCallback('preview', click_callback)
big_img = cv2.resize(img, (0, 0),
fx=display_scale,
fy=display_scale)
cv2.imshow('preview', big_img)
import cv2
import numpy as np
from geometry_msgs.msg import PoseStamped
import rospy
import tf
import copy
from pyquaternion import Quaternion
from camera_class import Camera
import time
camera = Camera(width=320, height=240)
class Homography:
""" Runs Homography AR"""
def __init__(self, min_match_count = 10, width = 320, height = 240,
camera_matrix = np.array([[ 253.70549591, 0., 162.39457585],
[ 0., 251.25243215, 126.5400089 ],
[ 0., 0., 1. ]]),
dist_coeffs = np.array([ 0.20462996, -0.41924085, 0.00484044, 0.00776978,
0.27998478]),
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_50),
marker_edge_length = 15.5, kp1=None, des1=None, flann_index_kdtree = 0,
index_params = dict(algorithm = 6, table_number = 6,
key_size = 12, multi_probe_level = 1),
search_params = dict(checks = 50)):
self.min_match_count = min_match_count
self.width = width
self.height = height
self.camera_matrix = camera_matrix
class Robot():
def __init__(self):
self.encoder = Encoder()
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
def stop(self):
print("Exiting Robot")
self.encoder.ctrlC()
self.camera.ctrlC()
self.distance_sensor.ctrlC()
exit()
def ctrlC(self, signum, frame):
self.stop()
#getters and setters
def goal_in_front(self, val=None):
#goal is in front of the robot
if val is not None:
self.GIF = val
# else:
return self.GIF
def no_wall_detected(self, val=None):
#No wall is in front of the robot within 10 cm
if val is not None:
self.no_wall = val
return self.no_wall
def less_than_10cm(self, val=None):
#something is in front of the robot within 10 cm that is not the goal
if val is not None:
self.less_than_10 = val
return self.less_than_10
def stop_range(self, val=None):
#robot front is within proper distance +- error of goal
if val is not None:
self.stop_r = val
return self.stop_r
#from faceGoal.py
def check_goal_in_front(self):
print("Checking GIF()")
#checks if goal is in front within error range
#sets robot variable accordingly and returns True or False
blobs = self.camera.get_blobs()
ERROR = 0.5
Kp = 0.009
t_set = []
#Find largest blob
largest = -1
size = 0.0
for i in range(len(blobs)):
if blobs[i].size > size:
size = blobs[i].size
largest = i
for x in range(0, 20):
if len(blobs) > 0:
self.blob_x = blobs[largest].pt[0]
if ((Kp * (blobs[largest].pt[0] - 320)) > -ERROR) and (
(Kp * (blobs[largest].pt[0] - 320)) < ERROR):
if ((Kp * (blobs[largest].pt[1] - 240) > -2 * ERROR) and
((Kp * (blobs[largest].pt[1] - 240)) < 2 * ERROR)):
## self.goal_in_front(True)
print("GIF True")
t_set.append(True)
else:
## self.goal_in_front(False)
print("GIF False because too high")
t_set.append(False)
else:
## self.goal_in_front(False)
print("GIF False goal center not in error range")
t_set.append(False)
else:
self.blob_x = 1000
## self.goal_in_front(False)
print("GIF False no blobs")
t_set.append(False)
if any(t_set):
self.goal_in_front(True)
return True
else:
self.goal_in_front(False)
return False
def check_no_wall_in_front(self):
f_distance = self.distance_sensor.get_front_inches()
GIF = self.check_goal_in_front()
if (GIF):
if (f_distance > 3 * 5):
print("GIF but also a wall")
self.no_wall_detected(True)
return True
else:
self.no_wall_detected(False)
return False
class Robot():
def __init__(self, skip=False):
self.encoder = Encoder()
if skip is False:
self.encoder.calibrateSpeeds()
self.distance_sensor = DistanceSensor()
self.camera = Camera()
self.blob_x = -1000
signal.signal(signal.SIGINT, self.ctrlC)
self.GIF = None
self.no_wall = None
self.less_than_10 = None
self.stop_r = None
self.orientation = 'n'
self.cell_size = 18
self.max_front_distance = 13
def stop(self):
print("Exiting Robot")
self.encoder.ctrlC()
self.camera.ctrlC()
self.distance_sensor.ctrlC()
exit()
def ctrlC(self, signum, frame):
self.stop()
#getters and setters
def goal_in_front(self, val=None):
#goal is in front of the robot
if val is not None:
self.GIF = val
# else:
return self.GIF
def no_wall_detected(self, val=None):
#No wall is in front of the robot within 10 cm
if val is not None:
self.no_wall = val
return self.no_wall
def less_than_10cm(self, val=None):
#something is in front of the robot within 10 cm that is not the goal
if val is not None:
self.less_than_10 = val
return self.less_than_10
def stop_range(self, val=None):
#robot front is within proper distance +- error of goal
if val is not None:
self.stop_r = val
return self.stop_r
#from faceGoal.py
def check_goal_in_front(self):
print("Checking GIF()")
#checks if goal is in front within error range
#sets robot variable accordingly and returns True or False
blobs = self.camera.get_blobs()
ERROR = 0.5
Kp = 0.009
t_set = []
#Find largest blob
largest = -1
size = 0.0
for i in range(len(blobs)):
if blobs[i].size > size:
size = blobs[i].size
largest = i
for x in range(0, 20):
if len(blobs) > 0:
self.blob_x = blobs[largest].pt[0]
if ((Kp * (blobs[largest].pt[0] - 320)) > -ERROR) and (
(Kp * (blobs[largest].pt[0] - 320)) < ERROR):
if ((Kp * (blobs[largest].pt[1] - 240) > -2 * ERROR) and
((Kp * (blobs[largest].pt[1] - 240)) < 2 * ERROR)):
## self.goal_in_front(True)
print("GIF True")
t_set.append(True)
else:
## self.goal_in_front(False)
print("GIF False because too high")
t_set.append(False)
else:
## self.goal_in_front(False)
print("GIF False goal center not in error range")
t_set.append(False)
else:
self.blob_x = 1000
## self.goal_in_front(False)
print("GIF False no blobs")
t_set.append(False)
if any(t_set):
self.goal_in_front(True)
return True
else:
self.goal_in_front(False)
return False
def check_no_wall_in_front(self):
f_distance = self.distance_sensor.get_front_inches()
GIF = self.check_goal_in_front()
if (GIF):
if (f_distance > 3 * 5):
print("GIF but also a wall")
self.no_wall_detected(True)
return True
else:
self.no_wall_detected(False)
return False
def rotate(self, direction='r'):
rotations = (self.encoder.WSEPARATION * math.pi /
4) / (self.encoder.WDIAMETER * math.pi)
ticks = int(rotations * 32 * 1.2)
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
speed = min(self.encoder.get_max_forward_speed(),
-self.encoder.get_max_backward_speed())
if direction.lower() == 'r':
self.encoder.setSpeedsIPS(speed, -speed)
if self.orientation == 'n':
self.orientation = 'e'
elif self.orientation == 'e':
self.orientation = 's'
elif self.orientation == 's':
self.orientation = 'w'
else:
self.orientation = 'n'
else:
self.encoder.setSpeedsIPS(-speed, speed)
if self.orientation == 'n':
self.orientation = 'w'
elif self.orientation == 'e':
self.orientation = 'n'
elif self.orientation == 's':
self.orientation = 'e'
else:
self.orientation = 's'
while self.encoder.steps_to_move[0] != -1:
time.sleep(0.1)
def get_right_dir(self):
if self.orientation == 'n':
return 'e'
elif self.orientation == 'e':
return 's'
elif self.orientation == 's':
return 'w'
else:
return 'n'
def get_left_dir(self):
if self.orientation == 'n':
return 'w'
elif self.orientation == 'w':
return 's'
elif self.orientation == 's':
return 'e'
else:
return 'n'
def change_orientation(self, direction):
if direction.lower() == 'n':
if self.orientation == 'n':
return
elif self.orientation == 'e':
self.rotate('l')
elif self.orientation == 's':
self.rotate('l')
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 'e':
if self.orientation == 'e':
return
elif self.orientation == 's':
self.rotate('l')
elif self.orientation == 'w':
self.rotate('l')
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 's':
if self.orientation == 's':
return
elif self.orientation == 'w':
self.rotate('l')
elif self.orientation == 'n':
self.rotate('l')
self.rotate('l')
else:
self.rotate('r')
elif direction.lower() == 'w':
if self.orientation == 'w':
return
elif self.orientation == 'n':
self.rotate('l')
elif self.orientation == 'e':
self.rotate('l')
self.rotate('l')
else:
self.rotate('r')
def forward(self):
rotations = (self.cell_size / 2) / (self.encoder.WDIAMETER * math.pi)
ticks = int(rotations * 32)
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
next_cell = NextCell(
self, (self.distance_sensor.get_left_inches() < self.cell_size),
(self.distance_sensor.get_right_inches() < self.cell_size))
#print(self.distance_sensor.get_front_inches())
# if self.distance_sensor.get_right_inches() < (self.mapper.rob.cell_size / 2):
# if self.distance_sensor.get_left_inches() < (self.mapper.rob.cell_size / 2):
# follow_both(self)
# else:
# follow_right(self)
# elif self.distance_sensor.get_left_inches() < (self.mapper.rob.cell_size / 2):
# follow_left(self)
# else:
if True:
input("Enter to WF")
self.encoder.setSpeedsIPS(self.encoder.get_max_forward_speed(),
self.encoder.get_max_forward_speed())
follow_both(self, next_cell)
# while next_cell.move_to_cell():
# print("moving to cell")
# time.sleep(0.1)
self.encoder.step_count = (0, 0)
self.encoder.steps_to_move = [ticks, ticks]
time.sleep(0.1)
follow_both(self, next_cell)
## self.encoder.setSpeedsIPS(self.encoder.get_max_forward_speed(), self.encoder.get_max_forward_speed())
input("Enter to Center")
while next_cell.center_in_cell():
print("Centering in cell")
distance = self.distance_sensor.get_front_inches()
proportional_control = saturation_function(
1.5 * ((self.cell_size / 2) - distance),
self.encoder.get_max_forward_speed(),
self.encoder.get_max_backward_speed())
self.encoder.setSpeedsIPS(proportional_control,
proportional_control)
time.sleep(0.01)
##
## time.sleep(0.1)
self.encoder.stop()
def genFunction():
#forever: uses camera class to get a camera frame and yields it
while False:
frame = Camera().get_frame()
yield (b'--frame\r\n'
b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n')