From 9c95fdb8f58eedc9033837ccc3691eb704c661f0 Mon Sep 17 00:00:00 2001 From: jmcmah10 Date: Sun, 28 May 2023 09:25:10 -0700 Subject: [PATCH 1/9] aaaaaaaaa --- lib/cameras/obstacle_avoidance.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/cameras/obstacle_avoidance.py b/lib/cameras/obstacle_avoidance.py index 128893c..0772527 100644 --- a/lib/cameras/obstacle_avoidance.py +++ b/lib/cameras/obstacle_avoidance.py @@ -29,7 +29,7 @@ def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False): self.block_height = 15 # height of sections the screen is separated into #self.a = 0.055 # "a" value used in function to calculate maximum slope between blocks in frame, I found this to be about 0.05 through some tests self.a = 0.01 - self.fixed_min_slope = -0.00001 # idk maybe try this I feel like shit but the thing above has a bug :skull: + self.fixed_min_slope = -0.02 # idk maybe try this I feel like shit but the thing above has a bug :skull: # self.dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) # self.parameters = cv2.aruco.DetectorParameters_create() # Create a context object. This object owns the handles to all connected realsense devices From d88d1a1a49ec236972f6f7b81f19fb5f89e9aee0 Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 10:27:27 -0600 Subject: [PATCH 2/9] updates --- bin/autonomy_main.py | 227 ------------------------------ lib/autonomy_main.py | 214 ++++++++++++++++++++++++++++ lib/cameras/obstacle_avoidance.py | 6 +- lib/network/data_server.py | 22 ++- lib/network/drive.py | 2 +- lib/pathfinding.py | 29 +++- lib/thread.py | 14 +- requirements.txt | 1 - 8 files changed, 271 insertions(+), 244 deletions(-) delete mode 100644 bin/autonomy_main.py create mode 100644 lib/autonomy_main.py diff --git a/bin/autonomy_main.py b/bin/autonomy_main.py deleted file mode 100644 index f9706a6..0000000 --- a/bin/autonomy_main.py +++ /dev/null @@ -1,227 +0,0 @@ -from lib.pathfinding import Pathfinding -from lib.imu.imu import Imu -from lib.gps_reader import GPSReader -from lib.hardware.temp_tank_drive import Drive -#from lib.drive import Drive -from lib.obstacle_avoidance import ObstacleDetectionCamera -from lib.ultrasonic import Ultrasonic -#from network import ProtoSocket, Device -import time -import math -import sys - -def main2(drive, gps, imu, camera, ultrasonic_left, ultrasonic_right, marker_radius): - # setup - drive.set_speeds(0.0, 0.0) - speed1 = 0.9 - print("starting") - time.sleep(1.0) - while gps.read_gps()[0] == 0: - pass - print("gps ready") - while imu.get_orientation()[2] == 0: - pass - print("imu ready") - pathfinding = Pathfinding(gps, imu, camera, gps_goal) - # the camera gets mad if it doesn't get to think ahead of time >:( - camera.get_distances(20) - time.sleep(3.0) - - pathfind_to_find_marker(drive, gps, imu, camera, ultrasonic, pathfinding, marker_radius) - - targets = get_targets(gps_goal, marker_radius) - - while len(camera.read_markers()) == 0: - pathfinding = Pathfinding(gps, imu, camera, targets[0]) - del targets[0] - if len(targets) == 0: - targets = get_targets(gps_goal, marker_radius) - pathfind_to_find_marker(drive, gps, imu, camera, ultrasonic, pathfinding, 2.0) - - done = False - while not done: - info = get_second_marker_info(camera.read_markers(), -1) - if len(info) != 0: - if info[1] > 330: - drive.set_speeds(-0.7, 0.7) - elif info[1] < 310: - drive.set_speeds(0.7, -0.7) - elif info[3] > 1.5: - drive.set_speeds(0.8, 0.8) - else: - done = True - else: - drive.set_speeds(0.0, 0.0) - drive.set_speeds(0.0, 0.0) - print("done") - -def get_targets(center, radius): - radius *= 1.414 - targets = [] - for i in range(math.ceil(radius), 1, -1): - targets.append((center[0] + i * (0.00001 / 1.11), center[1])) - targets.append((center[0], center[1] + i * (0.00001 / 1.11))) - targets.append((center[0] - i * (0.00001 / 1.11), center[1])) - targets.append((center[0], center[1] + i * (0.00001 / 1.11))) - targets.append(center) - -def pathfind_to_find_marker(drive, gps, imu, camera, ultrasonic, pathfinding, marker_radius): - speed1 = 0.9 - # go to gps location - while (not pathfinding.is_at_goal(marker_radius) and len(camera.read_markers()) == 0): - print("running pathfinding") - target_cords = pathfinding.pathfinding() - current_direction = imu.get_orientation()[2] % 360.0 - start_point = gps.read_gps() - target_direction = pathfinding.get_direction(start_point, target_cords) - while not pathfinding.rotational_equality(target_direction, current_direction) and len(camera.read_markers()) == 0: - adjust_to_face_target_direction(drive, target_direction, current_direction) - current_direction = imu.get_orientation()[2] % 360.0 - drive.set_speeds(speed1, speed1) - gps_pos = gps.read_gps() - while not reached_point(start_point, target_cords, gps_pos) and (ultrasonic_left.is_drivable() and ultrasonic_right.is_drivable()) and (not camera.is_blocked()) and len(camera.read_markers()) == 0: - gps_pos = gps.read_gps() - target_direction = pathfinding.get_direction(gps_pos, target_cords) - current_direction = imu.get_orientation()[2] % 360.0 - adjust_while_moving_to_target(drive, target_direction, current_direction, 0.9, 0.4) - if not (ultrasonic_left.is_drivable() and ultrasonic_right.is_drivable()): - drive.set_speeds(-0.9, -0.9) - time.sleep(1.0) - drive.set_speeds(0.0, 0.0) - drive.set_speeds(0.0, 0.0) - print("done!") - -def main(drive, gps, imu, camera, ultrasonic_left, ultrasonic_right, gps_goal): - # setup - drive.set_speeds(0.0, 0.0) - speed1 = 0.9 - print("starting") - time.sleep(1.0) - while gps.read_gps()[0] == 0: - pass - print("gps ready") - while imu.get_orientation()[2] == 0: - pass - print("imu ready") - pathfinding = Pathfinding(gps, imu, camera, gps_goal) - # the camera gets mad if it doesn't get to think ahead of time >:( - camera.get_distances(20) - time.sleep(3.0) - - # go to gps location - while (not pathfinding.is_at_goal(1.0)): - print("running pathfinding") - #print(imu.get_orientation()[2] % 360.0) - #print(gps.read_gps()) - target_cords = pathfinding.pathfinding() - current_direction = imu.get_orientation()[2] % 360.0 - start_point = gps.read_gps() - target_direction = pathfinding.get_direction(start_point, target_cords) - #print(target_direction) - while not pathfinding.rotational_equality(target_direction, current_direction): - adjust_to_face_target_direction(drive, target_direction, current_direction) - current_direction = imu.get_orientation()[2] % 360.0 - drive.set_speeds(speed1, speed1) - gps_pos = gps.read_gps() - while not reached_point(start_point, target_cords, gps_pos) and (ultrasonic_left.is_drivable() and ultrasonic_right.is_drivable()) and (not camera.is_blocked()): - gps_pos = gps.read_gps() - target_direction = pathfinding.get_direction(gps_pos, target_cords) - current_direction = imu.get_orientation()[2] % 360.0 - adjust_while_moving_to_target(drive, target_direction, current_direction, 0.9, 0.4) - #drive.set_speeds(-0.9, -0.9) - #time.sleep(1.0) - drive.set_speeds(0.0, 0.0) - drive.set_speeds(0.0, 0.0) - print("done!") - -def get_second_marker_info(camera_info, first_id): - for i in markers: - if i[0] != first_id: - return i - return [] - -def contains_second_marker(markers, first_id): - for i in markers: - if i[0] != first_id: - return True - return False - -def drive_spiral(drive, current_rotation, current_position, origin, radius): - rotation_vector = (-(current_position[1] - origin[1]), current_position[0] - origin[0]) - current_radius = math.sqrt(math.pow(rotation_vector[0], 2) + math.pow(rotation_vector[1], 2)) - target_direction = (math.atan2(-rotation_vector[1], rotation_vector[0]) * (180.0 / math.pi)) % 360.0 - error = 0.1 - if current_radius >= radius - error and current_radius <= radius + error: - adjust_while_moving_to_target(drive, target_direction, current_direction, 0.7, 0.3) - elif current_radius < radius - error: - drive.set_speeds(0.7, 0.3) - else: - drive.set_speeds(0.3, 0.7) - -def adjust_to_face_target_direction(drive, target_direction, current_direction): - speed1 = 0.75 - if (current_direction - target_direction) % 360.0 <= 180.0: - drive.set_speeds(-speed1, speed1) - else: - drive.set_speeds(speed1, -speed1) - -def adjust_while_moving_to_target(drive, target_direction, current_direction, speed1, speed2): - relative_angle = (current_direction - target_direction) % 360.0 - angle_threshold = 3.0 - if relative_angle <= angle_threshold or relative_angle >= 360.0 - angle_threshold: - drive.set_speeds(speed1, speed1) - elif relative_angle <= 180.0: - drive.set_speeds(speed2, speed1) - else: - drive.set_speeds(speed1, speed2) - -def reached_point(start_point, target_point, current_point): - # handle the case where the slope would be undefined - if start_point[1] == target_point[1]: - if start_point[0] >= target_point[0]: - return (current_point[0] <= target_point[0]) - else: - return (current_point[0] >= target_point[0]) - - # calculate the slope of the boundary line - slope = -(target_point[0] - start_point[0]) / (target_point[1] - start_point[1]) - y = (slope * (current_point[0] - target_point[0])) + target_point[1] - if start_point[1] > target_point[1]: - return (y >= current_point[1]) - else: - return (y <= current_point[1]) - -if __name__ == "__main__": - print("Setting up drive") - #socket = ProtoSocket(port = 8003, device = Device.AUTONOMY, destination = ("127.0.0.1", 8001)) - drive = Drive() #drive = Drive(socket) # AutonomyRover() - - print("Setting up gps...") - gps = GPSReader() - - print("Setting up imu...") - imu = Imu() - - print("Setting up camera...") - camera = ObstacleDetectionCamera(1.8, 240, -0.3) #camera = DepthCamera() - - print("Setting up ultrasonic...") - # TODO create ultrasonic objects for left and right (it shouldn't matter if they are flipped btw) - ultrasonic_left = Ultrasonic() - ultrasonic_right = Ultrasonic() - - print("Starting main") - try: - main2(drive, gps, imu, camera, ultrasonic_left, ultrasonic_right) - finally: - # TODO on the rover we don't want to close all of this, we want it to be passed in - print("shit") - drive.set_speeds(0.0, 0.0) - gps.stop_reading() - imu.stop_reading() - ultrasonic_left.stop_reading() - ultrasonic_right.stop_reading() - time.sleep(1) - ultrasonic_left.clean_up() - ultrasonic_right.clean_up() - time.sleep(2) diff --git a/lib/autonomy_main.py b/lib/autonomy_main.py new file mode 100644 index 0000000..17468c6 --- /dev/null +++ b/lib/autonomy_main.py @@ -0,0 +1,214 @@ +from lib.pathfinding import Pathfinding +from lib.hardware.imu import Imu +from lib.gps_reader import GPSReader +from lib.hardware.temp_tank_drive import Drive +#from lib.drive import Drive +from lib.obstacle_avoidance import ObstacleDetectionCamera +from lib.ultrasonic import Ultrasonic +#from network import ProtoSocket, Device +import time +import math +import sys + +class Autonomy: + def __init__(self): + self.previous_left_ultrasonic = -1.0 + self.left_ultrasonic = -1.0 + self.previous_right_distance = -1.0 + self.right_ultrasonic = -1.0 + + def autonomy_to_cords(self, collection): + self._main(collection.drive, collection.gps, collection.imu, collection.camera, collection.subsystems, collection.dashboard) + + def autonomy_to_marker(self, collection, radius): + self._main2(collection.drive, collection.gps, collection.imu, collection.camera, collection.subsystems, collection.dashboard, 20.0) + + def _main2(self, drive, gps, imu, camera, subsystems, dashboard, marker_radius): + pathfinding = Pathfinding(gps, imu, camera, targets[0], subsystems, dashboard) + + self._pathfind_to_find_marker(drive, gps, imu, camera, subsystems, pathfinding, dashboard, marker_radius) + + targets = self._get_targets(gps_goal, marker_radius) + + at_marker = False + while not at_marker: + self.dashboard.send_message(AutonomyData(state = AutonomyState.SEARCHING)) + while len(camera.read_markers()) == 0: + pathfinding = Pathfinding(gps, imu, camera, targets[0]) + del targets[0] + if len(targets) == 0: + targets = self._get_targets(gps_goal, marker_radius) + self._pathfind_to_find_marker(drive, gps, imu, camera, subsystems, pathfinding, 2.0) + + self.dashboard.send_message(AutonomyData(state = AutonomyState.APPROACHING)) + done = False + while True: + info = self._get_second_marker_info(camera.read_markers(), -1) + if len(info) != 0: + if info[1] > 330: + drive.set_speeds(-0.7, 0.7) + elif info[1] < 310: + drive.set_speeds(0.7, -0.7) + elif info[3] > 1.5: + drive.set_speeds(0.8, 0.8) + else: + at_marker = True + break + else: + break + + self.dashboard.send_message(AutonomyData(state = AutonomyState.AT_DESTINATION)) + drive.set_speeds(0.0, 0.0) + print("done") + + def _get_targets(center, radius): + radius *= 1.414 + targets = [] + for i in range(math.ceil(radius), 1, -1): + targets.append((center[0] + i * (0.00001 / 1.11), center[1])) + targets.append((center[0], center[1] + i * (0.00001 / 1.11))) + targets.append((center[0] - i * (0.00001 / 1.11), center[1])) + targets.append((center[0], center[1] + i * (0.00001 / 1.11))) + targets.append(center) + + def _pathfind_to_find_marker(drive, gps, imu, camera, subsystems, pathfinding, dashboard, marker_radius): + speed1 = 0.9 + # go to gps location + while (not pathfinding.is_at_goal(marker_radius) and len(camera.read_markers()) == 0): + print("running pathfinding") + self.dashboard.send_message(AutonomyData(state = AutonomyState.PATHING)) + target_cords = pathfinding.pathfinding() + current_direction = imu.get_orientation()[2] % 360.0 + start_point = gps.read_gps() + target_direction = pathfinding.get_direction(start_point, target_cords) + self.dashboard.send_message(AutonomyData(state = AutonomyState.DRIVING)) + while not pathfinding.rotational_equality(target_direction, current_direction) and len(camera.read_markers()) == 0: + self._adjust_to_face_target_direction(drive, target_direction, current_direction) + current_direction = imu.get_orientation()[2] % 360.0 + drive.set_speeds(speed1, speed1) + gps_pos = gps.read_gps() + while not self._reached_point(start_point, target_cords, gps_pos) and subsystems.is_drivable() and (not camera.is_blocked()) and len(camera.read_markers()) == 0: + gps_pos = gps.read_gps() + target_direction = pathfinding.get_direction(gps_pos, target_cords) + current_direction = imu.get_orientation()[2] % 360.0 + self._adjust_while_moving_to_target(drive, target_direction, current_direction, 0.9, 0.4) + if not subsystems.is_drivable(): + drive.set_speeds(-0.9, -0.9) + time.sleep(0.5) + drive.set_speeds(0.0, 0.0) + drive.set_speeds(0.0, 0.0) + print("done!") + + def _main(drive, gps, imu, camera, subsystems, dashboard, gps_goal): + # go to gps location + self.dashboard.send_message(AutonomyData(state = AutonomyState.PATHING)) + while (not pathfinding.is_at_goal(1.0)): + print("running pathfinding") + target_cords = pathfinding.pathfinding() + current_direction = imu.get_orientation()[2] % 360.0 + start_point = gps.read_gps() + target_direction = pathfinding.get_direction(start_point, target_cords) + while not pathfinding.rotational_equality(target_direction, current_direction): + self._adjust_to_face_target_direction(drive, target_direction, current_direction) + current_direction = imu.get_orientation()[2] % 360.0 + drive.set_speeds(speed1, speed1) + gps_pos = gps.read_gps() + while not self._reached_point(start_point, target_cords, gps_pos) and subsystems.is_drivable() and (not camera.is_blocked()): + gps_pos = gps.read_gps() + target_direction = pathfinding.get_direction(gps_pos, target_cords) + current_direction = imu.get_orientation()[2] % 360.0 + self._adjust_while_moving_to_target(drive, target_direction, current_direction, 0.9, 0.4) + drive.set_speeds(0.0, 0.0) + self.dashboard.send_message(AutonomyData(state = AutonomyState.AT_DESTINATION)) + drive.set_speeds(0.0, 0.0) + print("done!") + + def _get_second_marker_info(camera_info, first_id): + for i in markers: + if i[0] != first_id: + return i + return [] + +""" + def _drive_spiral(drive, current_rotation, current_position, origin, radius): + rotation_vector = (-(current_position[1] - origin[1]), current_position[0] - origin[0]) + current_radius = math.sqrt(math.pow(rotation_vector[0], 2) + math.pow(rotation_vector[1], 2)) + target_direction = (math.atan2(-rotation_vector[1], rotation_vector[0]) * (180.0 / math.pi)) % 360.0 + error = 0.1 + if current_radius >= radius - error and current_radius <= radius + error: + _adjust_while_moving_to_target(drive, target_direction, current_direction, 0.7, 0.3) + elif current_radius < radius - error: + drive.set_speeds(0.7, 0.3) + else: + drive.set_speeds(0.3, 0.7) +""" + + def _adjust_to_face_target_direction(drive, target_direction, current_direction): + speed1 = 0.75 + if (current_direction - target_direction) % 360.0 <= 180.0: + drive.set_speeds(-speed1, speed1) + else: + drive.set_speeds(speed1, -speed1) + + def _adjust_while_moving_to_target(drive, target_direction, current_direction, speed1, speed2): + relative_angle = (current_direction - target_direction) % 360.0 + angle_threshold = 3.0 + if relative_angle <= angle_threshold or relative_angle >= 360.0 - angle_threshold: + drive.set_speeds(speed1, speed1) + elif relative_angle <= 180.0: + drive.set_speeds(speed2, speed1) + else: + drive.set_speeds(speed1, speed2) + + def _reached_point(start_point, target_point, current_point): + # handle the case where the slope would be undefined + if start_point[1] == target_point[1]: + if start_point[0] >= target_point[0]: + return (current_point[0] <= target_point[0]) + else: + return (current_point[0] >= target_point[0]) + + # calculate the slope of the boundary line + slope = -(target_point[0] - start_point[0]) / (target_point[1] - start_point[1]) + y = (slope * (current_point[0] - target_point[0])) + target_point[1] + if start_point[1] > target_point[1]: + return (y >= current_point[1]) + else: + return (y <= current_point[1]) + + """ + if __name__ == "__main__": + print("Setting up drive") + #socket = ProtoSocket(port = 8003, device = Device.AUTONOMY, destination = ("127.0.0.1", 8001)) + drive = Drive() #drive = Drive(socket) # AutonomyRover() + + print("Setting up gps...") + gps = GPSReader() + + print("Setting up imu...") + imu = Imu() + + print("Setting up camera...") + camera = ObstacleDetectionCamera(1.8, 240, -0.3) #camera = DepthCamera() + + print("Setting up ultrasonic...") + # TODO create ultrasonic objects for left and right (it shouldn't matter if they are flipped btw) + ultrasonic_left = Ultrasonic() + ultrasonic_right = Ultrasonic() + + print("Starting main") + try: + main2(drive, gps, imu, camera, ultrasonic_left, ultrasonic_right) + finally: + # TODO on the rover we don't want to close all of this, we want it to be passed in + print("shit") + drive.set_speeds(0.0, 0.0) + gps.stop_reading() + imu.stop_reading() + ultrasonic_left.stop_reading() + ultrasonic_right.stop_reading() + time.sleep(1) + ultrasonic_left.clean_up() + ultrasonic_right.clean_up() + time.sleep(2) + """ diff --git a/lib/cameras/obstacle_avoidance.py b/lib/cameras/obstacle_avoidance.py index e1fe6f5..128893c 100644 --- a/lib/cameras/obstacle_avoidance.py +++ b/lib/cameras/obstacle_avoidance.py @@ -30,12 +30,12 @@ def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False): #self.a = 0.055 # "a" value used in function to calculate maximum slope between blocks in frame, I found this to be about 0.05 through some tests self.a = 0.01 self.fixed_min_slope = -0.00001 # idk maybe try this I feel like shit but the thing above has a bug :skull: - self.dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) - self.parameters = cv2.aruco.DetectorParameters_create() + # self.dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) + # self.parameters = cv2.aruco.DetectorParameters_create() # Create a context object. This object owns the handles to all connected realsense devices if test: self.pipeline = MockRealSense() - else: + else: self.pipeline = rs.pipeline() self.init_realsense() diff --git a/lib/network/data_server.py b/lib/network/data_server.py index 3f4ef96..675add8 100644 --- a/lib/network/data_server.py +++ b/lib/network/data_server.py @@ -4,11 +4,25 @@ class AutonomyDataServer(ProtoSocket): def __init__(self, port): super().__init__(port, device=Device.AUTONOMY, destination = ("127.0.0.1", 8001), send_heartbeats=False) - self.leftSensorValue = None - self.rightSensorValue = None + self.leftSensorValue = 9999.0 + self.rightSensorValue = 9999.0 + self.previousLeftSensorValue = 9999.0 + self.previousRightSensorValue = 9999.0 + self.drivableDistance = 90.0 def on_message(self, wrapper): if wrapper.name == "DriveData": data = DriveData.FromString(wrapper.data) - if data.leftSensorValue != 0: self.leftSensorValue = data.leftSensorValue - if data.rightSensorValue!= 0: self.rightSensorValue = data.rightSensorValue + if data.leftSensorValue != 0: + if data.leftSensorValue == -1.0: + data.leftSensorValue = 9999.0 + self.previousLeftSensorValue = self.leftSensorValue + self.leftSensorValue = data.leftSensorValue + if data.rightSensorValue!= 0: + if data.rightSensorValue == -1.0: + data.rightSensorValue = 9999.0 + self.previousRightSensorValue = self.rightSensorValue + self.rightSensorValue = data.rightSensorValue + + def is_drivable(self): + return (self.previousRightSensorValue <= self.drivableDistance and self.rightSensorValue <= self.drivableDistance) and (self.previousLeftSensorValue <= self.drivableDistance and self.leftSensorValue <= self.drivableDistance) \ No newline at end of file diff --git a/lib/network/drive.py b/lib/network/drive.py index 3b2733e..42ae12c 100644 --- a/lib/network/drive.py +++ b/lib/network/drive.py @@ -25,5 +25,5 @@ def set_throttle(self, throttle): def set_speeds(self, left, right): #print(f"Setting left={left}, right={right}") - command = DriveCommand(set_left=True, left=left, set_right=True, right=right) + command = DriveCommand(set_left=True, left=left, set_right=True, right=-right) self.socket.send_message(command) diff --git a/lib/pathfinding.py b/lib/pathfinding.py index 3871a99..78f7f9c 100644 --- a/lib/pathfinding.py +++ b/lib/pathfinding.py @@ -9,7 +9,7 @@ from lib.imu.imu import Imu class Pathfinding: - def __init__(self, gps_reader, imu_reader, camera, target_gps_coords, ultrasonic_left, ultrasonic_right): + def __init__(self, gps_reader, imu_reader, camera, target_gps_coords, subsystems, dashboard): # create readers self.gps_reader = gps_reader self.imu_reader = imu_reader @@ -38,8 +38,11 @@ def __init__(self, gps_reader, imu_reader, camera, target_gps_coords, ultrasonic self.camera_horizontal_fov = 87 # ultrasonics (for blind spot of camera) - self.ultrasonic_left = ultrasonic_left - self.ultrasonic_right = ultrasonic_right + self.subsystems = susbsystems + + # sending information + self.dashboard = dashboard + self.send_distance = 6.0 def _put_behind(self): if self.compass_direction < 45.0 or self.compass_direction > 315.0: @@ -59,6 +62,13 @@ def pathfinding(self): # calculate the path and target direction path = self._a_star(lambda a, b : (abs(b[0] - a[0]) + abs(b[1] - a[1]))) + + # send the calculated path + data = AutonomyData( + path = map(lambda a : GpsCoordinates(latitude=a[0], longitude=a[1])), + ) + self.dashboard.send_message(data) + target = self.get_target_node(path) # output target direction and target location @@ -69,7 +79,6 @@ def is_at_goal(self, radius): dist_square = math.pow(self.target_gps_coords[0] - current_gps_position[0], 2) + math.pow(self.target_gps_coords[1] - current_gps_position[1], 2) return dist_square <= math.pow(radius * (0.00001 / 1.11), 2) - def _read_data(self): # read gps, compass, and camera data self.current_position = self._gps_to_grid(self.read_gps()) @@ -77,6 +86,12 @@ def _read_data(self): self.camera_data = self._read_camera() self._update_blocked_areas() + # send new blocked areas (within a given radius to cap data size) + data = AutonomyData( + obstacles = filter(lambda a : (math.pow(a[0] - self.current_position[0], 2) + math.pow(a[1] - self.current_position[1], 2) <= math.pow(self.send_distance, 2)), self.is_blocked), + ) + self.dashboard.send_message(data) + # read gps coordinates def read_gps(self): #return self.gps_reader.read_gps() @@ -132,7 +147,7 @@ def _update_blocked_areas(self): for p in to_be_blocked: self.is_blocked.add(p) index += 1.0 - if (not self.ultrasonic_left.is_drivable()) or (not self.ultrasonic_right.is_drivable()): + if (not self.subsystems.is_drivable()): self.is_blocked.add(self._get_in_front()) def _get_in_front(self): @@ -235,8 +250,8 @@ def _get_adjacent_nodes_with_corners(self, node): (node[0] - 1, node[1] + 1), (node[0] - 1, node[1] - 1)] - def _set_contains(self, val, set): - return val in set + def _set_contains(self, val, set_list): + return val in set_list # given two points, create a continuous line of discrete points (integer pairs) from start to end def _bresenhams_line_floating_point(self, start, end): diff --git a/lib/thread.py b/lib/thread.py index df1d1d3..cb529dc 100644 --- a/lib/thread.py +++ b/lib/thread.py @@ -1,6 +1,7 @@ import time import cv2 import threading +from lib.autonomy_main import Autonomy from network.generated import * @@ -11,9 +12,13 @@ def __init__(self, collection): self.camera = cv2.VideoCapture(0) self.keep_alive = True self.command = None + self.autonomy = Autonomy() def run(self): - while self.keep_alive: + while self.keep_alive: + time.sleep(1.0) + """ + while self.keep_alive: # Read camera and send it to the dashboard video = self.collection.video if not self.camera.isOpened(): @@ -43,10 +48,17 @@ def run(self): destination=self.command.destination, ) self.collection.dashboard.send_message(data) + """ def startTask(self, command): self.command = command print(f'"Navigating" to {command.destination}') + if self.command.task == AutonomyTask.GPS_ONLY: + self.autonomy.autonomy_to_cords(self.collection) + self.command = None + elif self.command.task == AutonomyTask.VISUAL_MARKER: + self.autonomy.autonomy_to_marker(self.collection) + self.command = None def stopTask(self): self.command = None diff --git a/requirements.txt b/requirements.txt index ff0e94e..6742a4b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,4 @@ imutils==0.5.4 -opencv_contrib_python==4.5.3.56 pigpio==1.78 numpy==1.24.3 gps==3.19 From dd1fb9834164b12e17ec64d22664dbac6fa4b6c8 Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 10:53:55 -0600 Subject: [PATCH 3/9] fixed imports --- bin/depth_camera_test.py | 3 +-- bin/subsystems_test.py | 2 +- lib/autonomy_main.py | 8 +++----- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/bin/depth_camera_test.py b/bin/depth_camera_test.py index 1bb2540..d331e73 100644 --- a/bin/depth_camera_test.py +++ b/bin/depth_camera_test.py @@ -1,5 +1,4 @@ -from lib.depth_camera import DepthCamera -from lib.obstacle_avoidance import ObstacleDetectionCamera +from lib.cameras.obstacle_avoidance import ObstacleDetectionCamera import time import math import os diff --git a/bin/subsystems_test.py b/bin/subsystems_test.py index f391b28..774b666 100644 --- a/bin/subsystems_test.py +++ b/bin/subsystems_test.py @@ -2,7 +2,7 @@ from network import ProtoSocket, Device from network.src.generated.Protobuf.drive_pb2 import DriveCommand -from lib.drive import Drive +from lib.network.drive import Drive socket = ProtoSocket(port=8000, device=Device.DASHBOARD, destination=("127.0.0.1", 8001)) drive = Drive(socket) diff --git a/lib/autonomy_main.py b/lib/autonomy_main.py index 17468c6..d2f2e55 100644 --- a/lib/autonomy_main.py +++ b/lib/autonomy_main.py @@ -1,10 +1,8 @@ from lib.pathfinding import Pathfinding from lib.hardware.imu import Imu -from lib.gps_reader import GPSReader -from lib.hardware.temp_tank_drive import Drive -#from lib.drive import Drive -from lib.obstacle_avoidance import ObstacleDetectionCamera -from lib.ultrasonic import Ultrasonic +from lib.hardware.gps_reader import GPSReader +from lib.network.drive import Drive +from lib.cameras.obstacle_avoidance import ObstacleDetectionCamera #from network import ProtoSocket, Device import time import math From 2e25e374b9b6791407006f04d51c7157a75d3b5f Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 12:07:09 -0600 Subject: [PATCH 4/9] changed acceptable driving slope (around 40 degrees now) --- bin/depth_camera_test.py | 2 +- lib/cameras/obstacle_avoidance.py | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/bin/depth_camera_test.py b/bin/depth_camera_test.py index d331e73..7f3b196 100644 --- a/bin/depth_camera_test.py +++ b/bin/depth_camera_test.py @@ -40,7 +40,7 @@ def main(): camera = ObstacleDetectionCamera(2.4, 50, -0.3) print("Initialized") fun = lambda a : int(a) - time.sleep(5) + # time.sleep(5) #while True: # print(camera.read_markers()) diff --git a/lib/cameras/obstacle_avoidance.py b/lib/cameras/obstacle_avoidance.py index 128893c..030d22b 100644 --- a/lib/cameras/obstacle_avoidance.py +++ b/lib/cameras/obstacle_avoidance.py @@ -29,7 +29,7 @@ def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False): self.block_height = 15 # height of sections the screen is separated into #self.a = 0.055 # "a" value used in function to calculate maximum slope between blocks in frame, I found this to be about 0.05 through some tests self.a = 0.01 - self.fixed_min_slope = -0.00001 # idk maybe try this I feel like shit but the thing above has a bug :skull: + self.fixed_min_slope = -0.017 # idk maybe try this I feel like shit but the thing above has a bug :skull: # self.dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) # self.parameters = cv2.aruco.DetectorParameters_create() # Create a context object. This object owns the handles to all connected realsense devices @@ -236,14 +236,17 @@ def _get_marker_position(self, ids, corners): # Calculates center center = (int((topLeft[0] + bottomLeft[0]) / 2), int((topLeft[1] + bottomLeft[1]) / 2)) return center - + def _find_device_that_supports_advanced_mode(self) : ctx = rs.context() ds5_dev = rs.device() - devices = ctx.query_devices(); + devices = ctx.query_devices() + print("devices:") + print(list(devices)) for dev in devices: if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_PRODUCT_IDS: - if dev.supports(rs.camera_info.name): + print(rs.camera_info.name) + if dev.supports(rs.camera_info.name): print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) return dev raise Exception("No device that supports advanced mode was found") From 7809bad00db05e6a5f462b6cbfdaf9ef4a8067ed Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 14:04:07 -0600 Subject: [PATCH 5/9] added depth camera thread --- bin/main.py | 3 +- lib/cameras/obstacle_avoidance.py | 67 ++++----------------- lib/hardware/depth_camera_thread.py | 90 +++++++++++++++++++++++++++++ 3 files changed, 102 insertions(+), 58 deletions(-) create mode 100644 lib/hardware/depth_camera_thread.py diff --git a/bin/main.py b/bin/main.py index c379dff..a8a7ad6 100644 --- a/bin/main.py +++ b/bin/main.py @@ -20,7 +20,8 @@ def __init__(self): self.drive = Drive(collection=self) self.gps = GpsThread(collection=self) self.imu = ImuThread(collection=self) - self.camera = ObstacleDetectionCamera(1.8, 240, -0.3, test=True) + self.depth_camera = DepthCameraThread(collection=self) + self.camera = ObstacleDetectionCamera(1.8, 240, -0.3, test=False, self.depth_camera) # Main process print("[Info] Initializing main thread...") diff --git a/lib/cameras/obstacle_avoidance.py b/lib/cameras/obstacle_avoidance.py index 030d22b..56c7032 100644 --- a/lib/cameras/obstacle_avoidance.py +++ b/lib/cameras/obstacle_avoidance.py @@ -16,7 +16,7 @@ def stop(self, *args): pass DS5_PRODUCT_IDS = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"] class ObstacleDetectionCamera: - def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False): + def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False, depth_camera_thread): print("[Info] Initializing Obstacle Detection Camera") self.test = test self.camera_width = 640 @@ -33,23 +33,13 @@ def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False): # self.dictionary = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) # self.parameters = cv2.aruco.DetectorParameters_create() # Create a context object. This object owns the handles to all connected realsense devices - if test: - self.pipeline = MockRealSense() - else: - self.pipeline = rs.pipeline() - self.init_realsense() - - def init_realsense(self): - # Load proper settings - self.load_high_density_settings() - - # Configure streams - self.config = rs.config() - self.config.enable_stream(rs.stream.depth, self.camera_width, self.camera_height, rs.format.z16, 30) - self.config.enable_stream(rs.stream.color, self.camera_width, self.camera_height, rs.format.bgr8, 30) - - # Start streaming - self.pipeline.start(self.config) + self.dashboard = dashboard + #if test: + # self.pipeline = MockRealSense() + #else: + # self.pipeline = rs.pipeline() + # self.init_realsense() + self.depth_camera_thread = depth_camera_thread def is_blocked(self): if self.test: return False @@ -94,8 +84,7 @@ def get_distances(self, sections): # [sections] is the amount of columns to split the screen into def _get_distances(self, sections:int) -> [int]: # read depth frame - frames = self.pipeline.wait_for_frames() - depth = frames.get_depth_frame() + depth = self.depth_camera_thread.get_depth_frame() if not depth: return [] @@ -203,8 +192,7 @@ def _read_section(self, depth, minY, minX, block_width): def read_markers(self): # Get frame from camera - realsense_frames = self.pipeline.wait_for_frames() - color = realsense_frames.get_color_frame() + color = self.depth_camera_thread.get_color_frame() frame = np.asanyarray(color.get_data()) frame = imutils.resize(frame, width=1000) return self._detect_markers(frame) @@ -237,40 +225,5 @@ def _get_marker_position(self, ids, corners): center = (int((topLeft[0] + bottomLeft[0]) / 2), int((topLeft[1] + bottomLeft[1]) / 2)) return center - def _find_device_that_supports_advanced_mode(self) : - ctx = rs.context() - ds5_dev = rs.device() - devices = ctx.query_devices() - print("devices:") - print(list(devices)) - for dev in devices: - if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_PRODUCT_IDS: - print(rs.camera_info.name) - if dev.supports(rs.camera_info.name): - print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) - return dev - raise Exception("No device that supports advanced mode was found") - - def load_high_density_settings(self): - dev = self._find_device_that_supports_advanced_mode() - advnc_mode = rs.rs400_advanced_mode(dev) - print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") - while not advnc_mode.is_enabled(): - print("Trying to enable advanced mode...") - advnc_mode.toggle_advanced_mode(True) - # At this point the device will disconnect and re-connect. - print("Sleeping for 5 seconds...") - time.sleep(5) - # The 'dev' object will become invalid and we need to initialize it again - dev = find_device_that_supports_advanced_mode() - advnc_mode = rs.rs400_advanced_mode(dev) - print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") - with open('high-density-camera.json') as j_file: - json_dict = json.load(j_file) - advnc_mode.load_json(json.dumps(json_dict)) - - def clean_up(self): - self.pipeline.stop() - def close(self): self.clean_up() diff --git a/lib/hardware/depth_camera_thread.py b/lib/hardware/depth_camera_thread.py new file mode 100644 index 0000000..42bf8bb --- /dev/null +++ b/lib/hardware/depth_camera_thread.py @@ -0,0 +1,90 @@ +import sys +import time +import threading +import cv2 + +from network import * +from network.generated import * + +class DepthCameraThread(threading.Thread): + def __init__(self, collection, test=False): + print("[Info] Initializing Depth Camera...") + super().__init__() + self.keep_alive = True + #if test or sys.platform != "linux": + # self.session = MockGps() + #else: + # self.session = gps.gps("localhost", "2947") + self.collection = collection + self.depth_frame = None + self.color_frame = None + self.init_realsense() + self.read_frames() + + def close(self): + self.keep_alive = False + + # TODO: Remove in favor of directly accessing self.coordinates + def read_depth_frame(self): + return self.depth_frame + + def read_color_frame(self): + return self.color_frame + + def read_frames(self): + frames = self.pipeline.wait_for_frames() + self.depth_frame = frames.get_depth_frame() + self.color_frame = frames.get_color_frame() + self.collection.video.send_frame(frame=self.color_frame, name=CameraName.ROVER_FRONT) + self.collection.video.send_frame(frame=cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET), name=CameraName.AUTONOMY_DEPTH) + + def run(self): + while self.keep_alive: + self.read_frames() + + def init_realsense(self): + # Load proper settings + self.load_high_density_settings() + + # Configure streams + self.config = rs.config() + self.config.enable_stream(rs.stream.depth, self.camera_width, self.camera_height, rs.format.z16, 30) + self.config.enable_stream(rs.stream.color, self.camera_width, self.camera_height, rs.format.bgr8, 30) + + # Start streaming + self.pipeline.start(self.config) + + def _find_device_that_supports_advanced_mode(self) : + ctx = rs.context() + ds5_dev = rs.device() + devices = ctx.query_devices() + print("devices:") + print(list(devices)) + for dev in devices: + if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_PRODUCT_IDS: + print(rs.camera_info.name) + if dev.supports(rs.camera_info.name): + print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) + return dev + raise Exception("No device that supports advanced mode was found") + + def load_high_density_settings(self): + dev = self._find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + while not advnc_mode.is_enabled(): + print("Trying to enable advanced mode...") + advnc_mode.toggle_advanced_mode(True) + # At this point the device will disconnect and re-connect. + print("Sleeping for 5 seconds...") + time.sleep(5) + # The 'dev' object will become invalid and we need to initialize it again + dev = find_device_that_supports_advanced_mode() + advnc_mode = rs.rs400_advanced_mode(dev) + print("Advanced mode is", "enabled" if advnc_mode.is_enabled() else "disabled") + with open('high-density-camera.json') as j_file: + json_dict = json.load(j_file) + advnc_mode.load_json(json.dumps(json_dict)) + + def clean_up(self): + self.pipeline.stop() \ No newline at end of file From 03bc6ae32cf6dfbe51bb4be0474bcbd65f99743a Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 18:42:38 -0600 Subject: [PATCH 6/9] adjust camera based on orientation --- lib/hardware/depth_camera_thread.py | 4 ++++ lib/network/drive.py | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/lib/hardware/depth_camera_thread.py b/lib/hardware/depth_camera_thread.py index 42bf8bb..ad538c8 100644 --- a/lib/hardware/depth_camera_thread.py +++ b/lib/hardware/depth_camera_thread.py @@ -31,6 +31,9 @@ def read_depth_frame(self): def read_color_frame(self): return self.color_frame + def set_orientation(self): + self.collection.drive.set_camera_tilt(self.collection.imu.get_orientation()[0] + 90.0) + def read_frames(self): frames = self.pipeline.wait_for_frames() self.depth_frame = frames.get_depth_frame() @@ -40,6 +43,7 @@ def read_frames(self): def run(self): while self.keep_alive: + self.set_orientation() self.read_frames() def init_realsense(self): diff --git a/lib/network/drive.py b/lib/network/drive.py index 42ae12c..d6b9afc 100644 --- a/lib/network/drive.py +++ b/lib/network/drive.py @@ -27,3 +27,7 @@ def set_speeds(self, left, right): #print(f"Setting left={left}, right={right}") command = DriveCommand(set_left=True, left=left, set_right=True, right=-right) self.socket.send_message(command) + + def set_camera_tilt(self, angle): + command = DriveCommand(front_tilt=angle) + self.socket.send_message(command) From e3a13daa349332e0b87ea9b43f1a780809dca4a4 Mon Sep 17 00:00:00 2001 From: Jack McMahon <60189032+jmcmah10@users.noreply.github.com> Date: Wed, 31 May 2023 18:59:29 -0600 Subject: [PATCH 7/9] temp remove orientation --- lib/hardware/depth_camera_thread.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/hardware/depth_camera_thread.py b/lib/hardware/depth_camera_thread.py index ad538c8..966199f 100644 --- a/lib/hardware/depth_camera_thread.py +++ b/lib/hardware/depth_camera_thread.py @@ -43,7 +43,7 @@ def read_frames(self): def run(self): while self.keep_alive: - self.set_orientation() + # self.set_orientation() self.read_frames() def init_realsense(self): From b84f6eb76a2342f4b98dea5a2b24311a6251ecab Mon Sep 17 00:00:00 2001 From: jmcmah10 Date: Sun, 28 May 2023 11:34:17 -0700 Subject: [PATCH 8/9] Temp1 --- bin/depth_camera_test.py | 4 +-- bin/main.py | 2 +- lib/autonomy_main.py | 18 ++---------- lib/cameras/obstacle_avoidance.py | 2 +- lib/hardware/imu_thread.py | 4 +-- lib/network/data_server.py | 10 +++---- lib/network/drive.py | 47 ++++++++++++++++--------------- lib/pathfinding.py | 6 ++-- 8 files changed, 40 insertions(+), 53 deletions(-) diff --git a/bin/depth_camera_test.py b/bin/depth_camera_test.py index 7f3b196..1056869 100644 --- a/bin/depth_camera_test.py +++ b/bin/depth_camera_test.py @@ -7,7 +7,7 @@ camera_horizontal_fov = 90.0 compass_direction = 0.0 current_position = (0, 0) - +""" def _update_blocked_areas(depth_lists): grid_ticker = {} max_ticks = 40 @@ -34,7 +34,7 @@ def _update_blocked_areas(depth_lists): out_list.append((math.floor(k[0] + math.cos(compass_direction)), math.floor(k[1] + math.sin(compass_direction)))) out_list.append(k) print(out_list) - +""" def main(): print("Started") camera = ObstacleDetectionCamera(2.4, 50, -0.3) diff --git a/bin/main.py b/bin/main.py index a8a7ad6..c0afd96 100644 --- a/bin/main.py +++ b/bin/main.py @@ -21,7 +21,7 @@ def __init__(self): self.gps = GpsThread(collection=self) self.imu = ImuThread(collection=self) self.depth_camera = DepthCameraThread(collection=self) - self.camera = ObstacleDetectionCamera(1.8, 240, -0.3, test=False, self.depth_camera) + self.camera = ObstacleDetectionCamera(1.8, 240, -0.3, self.depth_camera) # Main process print("[Info] Initializing main thread...") diff --git a/lib/autonomy_main.py b/lib/autonomy_main.py index d2f2e55..2d339e4 100644 --- a/lib/autonomy_main.py +++ b/lib/autonomy_main.py @@ -1,6 +1,6 @@ from lib.pathfinding import Pathfinding -from lib.hardware.imu import Imu -from lib.hardware.gps_reader import GPSReader +from lib.hardware.imu_thread import ImuThread +from lib.hardware.gps_thread import GpsThread from lib.network.drive import Drive from lib.cameras.obstacle_avoidance import ObstacleDetectionCamera #from network import ProtoSocket, Device @@ -127,20 +127,6 @@ def _get_second_marker_info(camera_info, first_id): return i return [] -""" - def _drive_spiral(drive, current_rotation, current_position, origin, radius): - rotation_vector = (-(current_position[1] - origin[1]), current_position[0] - origin[0]) - current_radius = math.sqrt(math.pow(rotation_vector[0], 2) + math.pow(rotation_vector[1], 2)) - target_direction = (math.atan2(-rotation_vector[1], rotation_vector[0]) * (180.0 / math.pi)) % 360.0 - error = 0.1 - if current_radius >= radius - error and current_radius <= radius + error: - _adjust_while_moving_to_target(drive, target_direction, current_direction, 0.7, 0.3) - elif current_radius < radius - error: - drive.set_speeds(0.7, 0.3) - else: - drive.set_speeds(0.3, 0.7) -""" - def _adjust_to_face_target_direction(drive, target_direction, current_direction): speed1 = 0.75 if (current_direction - target_direction) % 360.0 <= 180.0: diff --git a/lib/cameras/obstacle_avoidance.py b/lib/cameras/obstacle_avoidance.py index 2396417..bf1b44b 100644 --- a/lib/cameras/obstacle_avoidance.py +++ b/lib/cameras/obstacle_avoidance.py @@ -16,7 +16,7 @@ def stop(self, *args): pass DS5_PRODUCT_IDS = ["0AD1", "0AD2", "0AD3", "0AD4", "0AD5", "0AF6", "0AFE", "0AFF", "0B00", "0B01", "0B03", "0B07","0B3A"] class ObstacleDetectionCamera: - def __init__(self, cutoff_dist, max_zeroes, min_slope, test=False, depth_camera_thread): + def __init__(self, cutoff_dist, max_zeroes, min_slope, depth_camera_test, test=False): print("[Info] Initializing Obstacle Detection Camera") self.test = test self.camera_width = 640 diff --git a/lib/hardware/imu_thread.py b/lib/hardware/imu_thread.py index b55e58e..d6038ed 100644 --- a/lib/hardware/imu_thread.py +++ b/lib/hardware/imu_thread.py @@ -6,8 +6,8 @@ from .osc_decoder import decode from network.generated import Orientation, RoverPosition -IMU_SERIAL_PORT = "COM25" - +# IMU_SERIAL_PORT = "COM25" +IMU_SERIAL_PORT = "/dev/ttyACM1" class MockImu: def __init__(self): print("[Warning] Using mock IMU") diff --git a/lib/network/data_server.py b/lib/network/data_server.py index 675add8..c25f21e 100644 --- a/lib/network/data_server.py +++ b/lib/network/data_server.py @@ -4,10 +4,10 @@ class AutonomyDataServer(ProtoSocket): def __init__(self, port): super().__init__(port, device=Device.AUTONOMY, destination = ("127.0.0.1", 8001), send_heartbeats=False) - self.leftSensorValue = 9999.0 - self.rightSensorValue = 9999.0 - self.previousLeftSensorValue = 9999.0 - self.previousRightSensorValue = 9999.0 + self.leftSensorValue = 0.0 + self.rightSensorValue = 0.0 + self.previousLeftSensorValue = 0.0 + self.previousRightSensorValue = 0.0 self.drivableDistance = 90.0 def on_message(self, wrapper): @@ -25,4 +25,4 @@ def on_message(self, wrapper): self.rightSensorValue = data.rightSensorValue def is_drivable(self): - return (self.previousRightSensorValue <= self.drivableDistance and self.rightSensorValue <= self.drivableDistance) and (self.previousLeftSensorValue <= self.drivableDistance and self.leftSensorValue <= self.drivableDistance) \ No newline at end of file + return (self.previousRightSensorValue <= self.drivableDistance or self.rightSensorValue <= self.drivableDistance) and (self.previousLeftSensorValue <= self.drivableDistance or self.leftSensorValue <= self.drivableDistance) diff --git a/lib/network/drive.py b/lib/network/drive.py index d6b9afc..1271038 100644 --- a/lib/network/drive.py +++ b/lib/network/drive.py @@ -2,32 +2,33 @@ from network.generated import DriveCommand class Drive: - """A service to manage drive controls. + """A service to manage drive controls. - This class only sends messages to the subsystems program, whether it is running on the tank or the - rover. The only thing that changes is the IP address of the Subsystems program, which is why this - class takes a [socket] paramter as input. + This class only sends messages to the subsystems program, whether it is running on the tank or the + rover. The only thing that changes is the IP address of the Subsystems program, which is why this + class takes a [socket] paramter as input. - - To change the top speed of the tank, use [set_throttle] - - To manually control each wheel, use [set_speeds] - """ - def __init__(self, collection): - self.socket = collection.subsystems + - To change the top speed of the tank, use [set_throttle] + - To manually control each wheel, use [set_speeds] + """ + def __init__(self, collection): + self.socket = collection.subsystems + self.set_throttle(0.3) - def close(self): - self.set_speeds(0, 0) - print("Stopped driving") + def close(self): + self.set_speeds(0, 0) + print("Stopped driving") - def set_throttle(self, throttle): - #print(f"Setting throttle to {throttle}") - command = DriveCommand(set_throttle=True, throttle=throttle) - self.socket.send_message(command) + def set_throttle(self, throttle): + #print(f"Setting throttle to {throttle}") + command = DriveCommand(set_throttle=True, throttle=throttle) + self.socket.send_message(command) - def set_speeds(self, left, right): - #print(f"Setting left={left}, right={right}") - command = DriveCommand(set_left=True, left=left, set_right=True, right=-right) - self.socket.send_message(command) + def set_speeds(self, left, right): + #print(f"Setting left={left}, right={right}") + command = DriveCommand(set_left=True, left=left, set_right=True, right=-right) + self.socket.send_message(command) - def set_camera_tilt(self, angle): - command = DriveCommand(front_tilt=angle) - self.socket.send_message(command) + def set_camera_tilt(self, angle): + command = DriveCommand(front_tilt=angle) + self.socket.send_message(command) diff --git a/lib/pathfinding.py b/lib/pathfinding.py index 78f7f9c..145fb27 100644 --- a/lib/pathfinding.py +++ b/lib/pathfinding.py @@ -4,9 +4,9 @@ import time import threading import statistics -from lib.gps_reader import GPSReader -from lib.obstacle_avoidance import ObstacleDetectionCamera -from lib.imu.imu import Imu +from lib.hardware.gps_thread import GpsThread +from lib.cameras.obstacle_avoidance import ObstacleDetectionCamera +from lib.hardware.imu_thread import ImuThread class Pathfinding: def __init__(self, gps_reader, imu_reader, camera, target_gps_coords, subsystems, dashboard): From 683b533720e44d8e36044199d8f3a24f2309bae6 Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Wed, 31 May 2023 20:19:50 -0600 Subject: [PATCH 9/9] Spaces --- lib/hardware/depth_camera_thread.py | 60 ++++++++++++++--------------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/lib/hardware/depth_camera_thread.py b/lib/hardware/depth_camera_thread.py index 966199f..5811138 100644 --- a/lib/hardware/depth_camera_thread.py +++ b/lib/hardware/depth_camera_thread.py @@ -7,45 +7,45 @@ from network.generated import * class DepthCameraThread(threading.Thread): - def __init__(self, collection, test=False): - print("[Info] Initializing Depth Camera...") - super().__init__() - self.keep_alive = True - #if test or sys.platform != "linux": - # self.session = MockGps() - #else: - # self.session = gps.gps("localhost", "2947") - self.collection = collection - self.depth_frame = None - self.color_frame = None - self.init_realsense() - self.read_frames() + def __init__(self, collection, test=False): + print("[Info] Initializing Depth Camera...") + super().__init__() + self.keep_alive = True + #if test or sys.platform != "linux": + # self.session = MockGps() + #else: + # self.session = gps.gps("localhost", "2947") + self.collection = collection + self.depth_frame = None + self.color_frame = None + self.init_realsense() + self.read_frames() - def close(self): - self.keep_alive = False + def close(self): + self.keep_alive = False - # TODO: Remove in favor of directly accessing self.coordinates - def read_depth_frame(self): - return self.depth_frame - - def read_color_frame(self): - return self.color_frame + # TODO: Remove in favor of directly accessing self.coordinates + def read_depth_frame(self): + return self.depth_frame + + def read_color_frame(self): + return self.color_frame def set_orientation(self): self.collection.drive.set_camera_tilt(self.collection.imu.get_orientation()[0] + 90.0) - def read_frames(self): + def read_frames(self): frames = self.pipeline.wait_for_frames() self.depth_frame = frames.get_depth_frame() self.color_frame = frames.get_color_frame() - self.collection.video.send_frame(frame=self.color_frame, name=CameraName.ROVER_FRONT) - self.collection.video.send_frame(frame=cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET), name=CameraName.AUTONOMY_DEPTH) + self.collection.video.send_frame(frame=self.color_frame, name=CameraName.ROVER_FRONT) + self.collection.video.send_frame(frame=cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET), name=CameraName.AUTONOMY_DEPTH) - def run(self): - while self.keep_alive: + def run(self): + while self.keep_alive: # self.set_orientation() - self.read_frames() - + self.read_frames() + def init_realsense(self): # Load proper settings self.load_high_density_settings() @@ -58,7 +58,7 @@ def init_realsense(self): # Start streaming self.pipeline.start(self.config) - def _find_device_that_supports_advanced_mode(self) : + def _find_device_that_supports_advanced_mode(self) : ctx = rs.context() ds5_dev = rs.device() devices = ctx.query_devices() @@ -67,7 +67,7 @@ def _find_device_that_supports_advanced_mode(self) : for dev in devices: if dev.supports(rs.camera_info.product_id) and str(dev.get_info(rs.camera_info.product_id)) in DS5_PRODUCT_IDS: print(rs.camera_info.name) - if dev.supports(rs.camera_info.name): + if dev.supports(rs.camera_info.name): print("Found device that supports advanced mode:", dev.get_info(rs.camera_info.name)) return dev raise Exception("No device that supports advanced mode was found")