diff --git a/Control/Jetson/dronekit_server_client.py b/Control/Jetson/dronekit_server_client.py index d854f00..76c1790 100644 --- a/Control/Jetson/dronekit_server_client.py +++ b/Control/Jetson/dronekit_server_client.py @@ -160,16 +160,14 @@ class DroneKitComms: _ = self.make_request(self.nudge_fork_vert_endpoint, data) def zero_fork_horiz(self): - self.nudge_fork_horiz(go_left=True, amount=0.9) self.nudge_fork_horiz(go_left=True, amount=0.9) self.nudge_fork_horiz(go_left=True, amount=0.9) time.sleep(2) - self.nudge_fork_horiz(go_left=False, amount=0.9) - self.nudge_fork_horiz(go_left=False, amount=0.4) # TODO: tune this + self.nudge_fork_horiz(go_left=False, amount=1.0) def approach_pallet(self): - _ = self.make_request(self.gps_target_radius_km) + _ = self.make_request(self.approach_pallet_endpoint) # print(rsp) diff --git a/Control/Jetson/vision_cargo_approach.py b/Control/Jetson/vision_cargo_approach.py index 5977178..0c76d9a 100644 --- a/Control/Jetson/vision_cargo_approach.py +++ b/Control/Jetson/vision_cargo_approach.py @@ -17,9 +17,13 @@ class Resolution: width: int height: int -def remap(in_value, in_min, in_max, out_min, out_max): - remapped = (((in_value - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min - return remapped +def remap(value, maxInput, minInput, maxOutput, minOutput): + value = maxInput if value > maxInput else value + value = minInput if value < minInput else value + inputSpan = maxInput - minInput + outputSpan = maxOutput - minOutput + scaledThrust = float(value - minInput) / float(inputSpan) + return minOutput + (scaledThrust * outputSpan) class VisionSystem: @@ -96,8 +100,7 @@ class VisionSystem: cv2.putText(frame, str(botom_right), (int(corners[2][0]), int(corners[2][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA) cv2.putText(frame, str(bottom_left), (int(corners[3][0]), int(corners[3][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA) tag_area = self.get_apriltag_size(top_left, top_right, botom_right) - print(tag_area) - print('---') + pos_x = 2 * (center[0] - (self.resolution.width/2)) / self.resolution.width pos_y = 2 * (center[1] - (self.resolution.height/2)) / self.resolution.height clean_pos_y = float("{0:.3f}".format(pos_y)) @@ -122,7 +125,7 @@ class VisionSystem: class CargoApproachingSystem: def __init__(self) -> None: - self.ky = 2.2 + self.ky = 2 self.kt = -1 def calculate(self, ey, et): @@ -130,62 +133,6 @@ class CargoApproachingSystem: return delta -class Tester: - def __init__(self) -> None: - self.log = logging.getLogger("TESTER") - self.log.setLevel("DEBUG") - self.camera = VisionSystem() - self.steering_law = CargoApproachingSystem() - self.no_detects = 0 - self.max_approach_area_thresh = 40000 - self.centre_distance_thresh = (-0.04, 0.08) - self.centre_angle_thresh = (-0.2, 0.2) - - def run_pc(self): - while True: - pallet_pose = self.camera.get_pallet_pose_pc() - if pallet_pose: - position, rotation, area = pallet_pose - error_y = position[0] - error_theta = rotation[1] - # print(error_y, error_theta) - delta = self.steering_law.calculate(error_y, error_theta) - # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000) - delta_PWM = remap(delta, -0.7, 0.7, 500, 2500) - print(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}") - cv2.waitKey(1) - - def run_jetson(self): - self.log.info("Starting vision system") - self.is_running = True - while self.is_running: - time.sleep(0.05) - pallet_pose = self.camera.get_pallet_pose() # getting the position, rotation and size of the tracker - if pallet_pose: # If we found a tracker - self.no_detects = 0 - position, rotation, area = pallet_pose - error_y = position[0] # centre offset distance - error_theta = rotation[1] # centre offset angle - # print(error_y, error_theta) - delta = self.steering_law.calculate(error_y, error_theta) # getting the steering angle from the control law - delta_PWM = remap(delta, -0.7, 0.7, 500, 2500) # Converting steering law output into PWM values - self.log.info(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}") - if area < self.max_approach_area_thresh: # If we are far enough from the cargo, move towards it - self.log.info(f"Setting speed: 1950, steering: {delta_PWM}") - else: - self.log.info(f"We are close enough to cargo!, {area}") - is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_y, error_theta)) - if is_aligned: - self.log.info(f'Cargo aligned: {error_y} in {self.centre_distance_thresh} and {error_theta} in {self.centre_angle_thresh}') - else: - self.log.info(f'Cargo NOT aligned: {error_y} not in {self.centre_distance_thresh} or {error_theta} inot n {self.centre_angle_thresh}') - - else: - self.no_detects +=1 - if self.no_detects > 3: # if we do not detect anything 3 times in a row, stop the vehicle - self.log.info("No cargo detected, setting speed to 1500") - - class VisionCargoApproacher: def __init__(self, forklift_comms) -> None: self.log = logging.getLogger("VISION CARGO APPROACHER") @@ -201,6 +148,7 @@ class VisionCargoApproacher: self.centre_distance_thresh = (-0.2, 0.2) # horizontal tolerance for pallet alignment pre fork alignment self.centre_angle_thresh = (-0.2, 0.2) # angular tolerance for pallet alignment pre fork alignment self.horizontal_camera_offset = 0 # In case the camera isn't perfectly straight, we can compansate for that in code + self.steering_log_file = open("slf.file", 'w+') def navigate_to_cargo_vision(self): self.log.info("Starting vision system") @@ -217,13 +165,12 @@ class VisionCargoApproacher: error_x += self.horizontal_camera_offset # adding offset # print(error_x, error_theta) delta = self.steering_law.calculate(error_x, error_theta) # getting the steering angle from the control law - self.log.info("***") - self.log.info(f"Error y {delta} | error theta {error_x} | delta {error_theta}") - self.log.info("***") # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000) - delta_PWM = remap(delta, -0.7, 0.7, 3000, 1) # Converting steering law output into PWM values - self.log.info(f"PWM: {delta_PWM}, ey: {error_x}, et: {error_theta}") - self.log.warning(f"TAG AREA: {area}") + delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100) # Converting steering law output into PWM values + little_log = f"Error y: {delta} | error theta {error_x} | steering_raw {error_theta} | steering_conv: {delta_PWM}, tag_area: {area}" + self.log.info(little_log) + self.steering_log_file.writelines([little_log]) + if area < self.max_approach_area_thresh: # If we are far enough from the cargo, move towards it speed_pwm = remap(area, 40000, 1000, 1500, 2000) self.forklift.set_speed_steering(speed_pwm, delta_PWM) @@ -244,7 +191,7 @@ class VisionCargoApproacher: time.sleep(0.2) return True delta = self.steering_law.calculate(error_x, error_theta) # getting the steering angle from the control law - delta_PWM = remap(delta, 0.7, -0.7, 2000, 1000) + delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100) self.forklift.set_speed_steering(1000, delta_PWM) time.sleep(3) self.forklift.set_speed_steering(1500, 1500) @@ -255,6 +202,8 @@ class VisionCargoApproacher: self.log.warning("DID NOT DETECT CODE!") self.forklift.set_speed(1500) + self.steering_log_file.close() + # def stop(self): # self.is_running = False # self.forklift.set_disarm() @@ -281,5 +230,61 @@ class VisionCargoApproacher: # else: # logging.critical(f"Platform {platform.release()} not tested!") +class Tester: + def __init__(self) -> None: + self.log = logging.getLogger("TESTER") + self.log.setLevel("DEBUG") + self.camera = VisionSystem() + self.steering_law = CargoApproachingSystem() + self.no_detects = 0 + self.max_approach_area_thresh = 40000 + self.centre_distance_thresh = (-0.04, 0.08) + self.centre_angle_thresh = (-0.2, 0.2) + + def run_pc(self): + while True: + pallet_pose = self.camera.get_pallet_pose_pc() + if pallet_pose: + position, rotation, area = pallet_pose + error_y = position[0] + error_theta = rotation[1] + # print(error_y, error_theta) + delta = self.steering_law.calculate(error_y, error_theta) + # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000) + delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100) + print(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}") + cv2.waitKey(1) + + def run_jetson(self): + self.log.info("Starting vision system") + self.is_running = True + while self.is_running: + time.sleep(0.05) + pallet_pose = self.camera.get_pallet_pose() # getting the position, rotation and size of the tracker + if pallet_pose: # If we found a tracker + self.no_detects = 0 + position, rotation, area = pallet_pose + error_y = position[0] # centre offset distance + error_theta = rotation[1] # centre offset angle + # print(error_y, error_theta) + delta = self.steering_law.calculate(error_y, error_theta) # getting the steering angle from the control law + delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100) # Converting steering law output into PWM values + self.log.info(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}") + if area < self.max_approach_area_thresh: # If we are far enough from the cargo, move towards it + self.log.info(f"Setting speed: 1950, steering: {delta_PWM}") + else: + self.log.info(f"We are close enough to cargo!, {area}") + is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_y, error_theta)) + if is_aligned: + self.log.info(f'Cargo aligned: {error_y} in {self.centre_distance_thresh} and {error_theta} in {self.centre_angle_thresh}') + else: + self.log.info(f'Cargo NOT aligned: {error_y} not in {self.centre_distance_thresh} or {error_theta} inot n {self.centre_angle_thresh}') + + else: + self.no_detects +=1 + if self.no_detects > 3: # if we do not detect anything 3 times in a row, stop the vehicle + self.log.info("No cargo detected, setting speed to 1500") + + if __name__ == "__main__": print("ERROR: This script is no longer used in standalone mode")