max/new_thea
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max 2021-09-21 13:06:49 +01:00
parent eeb693432f
commit 0220a136d1
2 changed files with 76 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Control/Jetson/dronekit_server_client.py
View File

@ -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)

143
Control/Jetson/vision_cargo_approach.py
View File

@ -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")