class PositionController(): def __init__(self): # Allow the simulator to start time.sleep(1) # When this node shutsdown rospy.on_shutdown(self.shutdown_sequence) # Set the rate self.rate = 100.0 self.dt = 1.0 / self.rate # Getting the PID parameters stable_gains = rospy.get_param( '/position_controller_node/gains/stable/', { 'p': 1, 'i': 0.0, 'd': 0.0 }) Kp_s, Ki_s, Kd_s = stable_gains['p'], stable_gains['i'], stable_gains[ 'd'] # If the speed is set to unstable waypoint Kp = Kp_s Ki = Ki_s Kd = Kd_s # Display incoming parameters rospy.loginfo( str(rospy.get_name()) + ": Lauching with the following parameters:") rospy.loginfo(str(rospy.get_name()) + ": p - " + str(Kp)) rospy.loginfo(str(rospy.get_name()) + ": i - " + str(Ki)) rospy.loginfo(str(rospy.get_name()) + ": d - " + str(Kd)) rospy.loginfo(str(rospy.get_name()) + ": rate - " + str(self.rate)) # Creating the PID's self.pos_x_PID = PID(Kp, Ki, Kd, self.rate) self.pos_y_PID = PID(Kp, Ki, Kd, self.rate) self.pos_z_PID = PID(Kp, Ki, Kd, self.rate) # Get the setpoints self.x_setpoint = 0 self.y_setpoint = 0 self.z_setpoint = 3 # Create the current output readings self.x_pos = 0 self.y_pos = 0 self.z_pos = 0 # Create the subscribers and publishers self.vel_set_sub = rospy.Publisher('/uav/input/velocity', Vector3, queue_size=1) self.gps_sub = rospy.Subscriber("uav/sensors/gps", PoseStamped, self.get_gps) self.pos_set_sub = rospy.Subscriber("uav/input/position", Vector3, self.set_pos) # Run the communication node self.ControlLoop() # This is the main loop of this class def ControlLoop(self): # Set the rate rate = rospy.Rate(1000) # Keep track how many loops have happend loop_counter = 0 # While running while not rospy.is_shutdown(): # Use a PID to calculate the velocity you want x_proportion = self.pos_x_PID.get_output(self.x_setpoint, self.x_pos) y_proportion = self.pos_y_PID.get_output(self.y_setpoint, self.y_pos) z_proportion = self.pos_z_PID.get_output(self.z_setpoint, self.z_pos) # Initialize the components of the vector x_vel = 0 y_vel = 0 z_vel = 0 # Set the velocity based on distance x_vel = x_proportion y_vel = y_proportion z_vel = z_proportion # Create and publish the data velocity = Vector3(x_vel, y_vel, z_vel) self.vel_set_sub.publish(velocity) # Sleep any excess time rate.sleep() # Call back to get the gps data def get_gps(self, msg): self.x_pos = msg.pose.position.x self.y_pos = msg.pose.position.y self.z_pos = msg.pose.position.z # Call back to get the position setpoints def set_pos(self, msg): # If our set point changes reset the PID build up check_x = self.x_setpoint != msg.x check_y = self.y_setpoint != msg.y check_z = self.z_setpoint != msg.z if check_x or check_y or check_z: self.pos_x_PID.remove_buildup() self.pos_y_PID.remove_buildup() self.pos_z_PID.remove_buildup() self.x_setpoint = msg.x self.y_setpoint = msg.y self.z_setpoint = msg.z # Called on ROS shutdown def shutdown_sequence(self): rospy.loginfo(str(rospy.get_name()) + ": Shutting Down")
class GateNavigation: def __init__(self): # Init the ROS node rospy.init_node('GateNavigationNode') self._log("Node Initialized") # On shutdown do the following rospy.on_shutdown(self.stop) # Set the rate self.rate = 30.0 self.dt = 1.0 / self.rate # Init the drone and program state self._quit = False self._innavigationmode = False self._gatefound = True # Used to compute the total mass of gate in each part of the image self.upper_left_mass = 0 self.upper_right_mass = 0 self.lower_left_mass = 0 self.lower_right_mass = 0 # Used to switch navigation mode from full gate to close gate navigation self.close_gate_navigation = False # Holds the center of the image: self.camera_center_x = None self.camera_center_y = None # Used to keep track of the object self.gate_center_x_arr = None self.gate_center_y_arr = None self.object_arr_length = 10 # PID Class to navigate to the gate gains = rospy.get_param(rospy.get_name() + '/gains', {'p': 1.0, 'i': 0.0, 'd': 0.0}) Kp, Ki, Kd = gains['p'], gains['i'], gains['d'] self.z_controller = PID(Kp, Ki, Kd, self.rate) self.y_controller = PID(Kp, Ki, Kd, self.rate) # Display the variables self._log(": p - " + str(Kp)) self._log(": i - " + str(Ki)) self._log(": d - " + str(Kd)) # Create the bridge self.bridge = CvBridge() # Variable to check if movement is allowed self.allow_movement = True # Variable to tell the algorithm if we can see the whole gate or only partial gate self.full_gate_found = False self.full_gate_ever_found = False # Used to store the current gates area (used as a metric for distance to gate) self._gate_area = 0 # Define the upper and lower bound lb = rospy.get_param(rospy.get_name() + '/lower_bound', {'H': 0, 'S': 0, 'V': 40}) ub = rospy.get_param(rospy.get_name() + '/upper_bound', {'H': 20, 'S': 200, 'V': 200}) self.lower_bound = (lb["H"], lb["S"], lb["V"]) self.upper_bound = (ub["H"], ub["S"], ub["V"]) # Init all the publishers and subscribers self.move_pub = rospy.Publisher("/uav1/input/beforeyawcorrection/move", Move, queue_size=10) self.drone_state_sub = rospy.Subscriber("/uav1/input/state", Int16, self._getstate) self.image_sub = rospy.Subscriber("/mixer/sensors/camera", Image, self._getImage) self.allow_movement_sub = rospy.Subscriber("/uav1/status/yaw_out_of_range", Bool, self._getOutOfRange) # Publish the image with the center self.img_pub = rospy.Publisher("/gate_navigation/sensors/camera", Image, queue_size=10) def start(self): self._mainloop() def stop(self): self._quit = True def _getOutOfRange(self, msg): # Set the allow_movement flag to the incoming message self.allow_movement = msg.data def _getstate(self, msg): # Check if we are in navigation mode if msg.data == DroneState.GATENAVIGATION.value: self._innavigationmode = True def _log(self, msg): print(str(rospy.get_name()) + ": " + str(msg)) def _getImage(self, msg): img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') # Save the size of the image if self.camera_center_x is None or self.camera_center_y is None: self.camera_center_x = msg.width / 2.0 self.camera_center_y = msg.height / 2.0 # Convert to hsv rbg_img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) hsv_img = cv2.cvtColor(rbg_img, cv2.COLOR_RGB2HSV) # Compute where the center is mask = cv2.inRange(hsv_img, self.lower_bound, self.upper_bound) gate_centre_y, gate_centre_x = ndimage.measurements.center_of_mass(mask) if math.isnan(gate_centre_x) or math.isnan(gate_centre_y): if self._gatefound: self._log("No gate found.") self._gatefound = False return else: self._gatefound = True # Make sure we are looking at a full gate and not only an image thresh = cv2.adaptiveThreshold(mask, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY_INV, 11, 2) contours, hierarchy = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contour_list = [] area_list = [0] for contour in contours: area = cv2.contourArea(contour) # Filter based on length and area if (area > 20000): contour_list.append(contour) area_list.append(area) # Get the largest gate area self._gate_area = max(area_list) # Check if there are any full gates found: if len(contour_list) <= 0: self.full_gate_found = False else: self.full_gate_found = True self.full_gate_ever_found = True rbg_img = cv2.drawContours(rbg_img, contour_list, -1, (0, 255, 0), 2) # Draw a horizontal and vertical line through the image point1_vertical = (int(rbg_img.shape[1]/2), 0) point2_vertical = (int(rbg_img.shape[1]/2), img.shape[0]) point1_horizontal = (0, int(rbg_img.shape[0]/2)) point2_horizontal = (img.shape[1], int(rbg_img.shape[0]/2)) rbg_img = cv2.line(rbg_img, pt1=point1_vertical, pt2=point2_vertical, color=(0, 0, 255), thickness=2) rbg_img = cv2.line(rbg_img, pt1=point1_horizontal, pt2=point2_horizontal, color=(0, 0, 255), thickness=2) # If we are too close for full navigation if self.close_gate_navigation: # Reset the center point gate_centre_x = self.camera_center_x gate_centre_y = self.camera_center_y # Compute the total mass of gate in each section of the image mask_horizontal_center = int(mask.shape[0] / 2) mask_vertical_center = int(mask.shape[1] / 2) self.upper_left_mass = np.sum(mask[0:mask_horizontal_center, 0:mask_vertical_center]) self.upper_right_mass = np.sum(mask[0:mask_horizontal_center, mask_vertical_center:]) self.lower_left_mass = np.sum(mask[mask_horizontal_center:, 0:mask_vertical_center]) self.lower_right_mass = np.sum(mask[mask_horizontal_center:, mask_vertical_center:]) # Check if there is gate on the upper left if self.upper_left_mass >= 1000: gate_centre_x += 0.025 * mask.shape[1] gate_centre_y += 0.025 * mask.shape[0] # Check if there is gate on the upper right if self.upper_right_mass >= 1000: gate_centre_x -= 0.025 * mask.shape[1] gate_centre_y += 0.025 * mask.shape[0] # Check if there is gate on the lower left if self.lower_left_mass >= 1000: gate_centre_x += 0.025 * mask.shape[1] gate_centre_y -= 0.025 * mask.shape[0] # Check if there is gate on the lower right if self.lower_right_mass >= 1000: gate_centre_x -= 0.025 * mask.shape[1] gate_centre_y -= 0.025 * mask.shape[0] # Save the results if self.gate_center_x_arr is None: self.gate_center_x_arr = np.full(self.object_arr_length, gate_centre_x) self.gate_center_y_arr = np.full(self.object_arr_length, gate_centre_y) else: self.gate_center_x_arr = np.roll(self.gate_center_x_arr, 1) self.gate_center_y_arr = np.roll(self.gate_center_y_arr, 1) self.gate_center_x_arr[0] = gate_centre_x self.gate_center_y_arr[0] = gate_centre_y # Publish the image with the dot center_coordinates = (int(round(gate_centre_x, 0)), int(round(gate_centre_y, 0))) radius = 2 color = (0, 255, 0) thickness = 2 rbg_img = cv2.circle(rbg_img, center_coordinates, radius, color, thickness) # Convert back to ros message and publish image_message = self.bridge.cv2_to_imgmsg(rbg_img, encoding="rgb8") self.img_pub.publish(image_message) def _mainloop(self): r = rospy.Rate(self.rate) # Hold samples for the last 2 seconds total_samples = int(self.rate * 2) # These arrays hold the values used to line up with the gate. line_up_window_y = np.full((total_samples,), 10) line_up_window_z = np.full((total_samples,), 10) move_forward = False stop_counter = 0 while not self._quit: if self._innavigationmode: # Calculate what the drones current direction try: cen_x = np.average(self.gate_center_x_arr) cen_y = np.average(self.gate_center_y_arr) except: continue # Calculate the error percentage_leftright = (cen_x - self.camera_center_x) / self.camera_center_x percentage_updown = (cen_y - self.camera_center_y) / self.camera_center_y # Put the error into the corresponding PID controller leftright = self.y_controller.get_output(0, -1 * percentage_leftright) updown = self.z_controller.get_output(0, -1 * percentage_updown) # Set the output direction_y = leftright direction_z = updown direction_backforward = 0 # Record the values and check if on average we have lined up if (self._gatefound): line_up_window_y = np.roll(line_up_window_y, 1) line_up_window_z = np.roll(line_up_window_z, 1) line_up_window_y[0] = abs(direction_y) line_up_window_z[0] = abs(direction_z) # Check if we have lined up avg_y = np.mean(line_up_window_y) avg_z = np.mean(line_up_window_z) # if (avg_y <= 1) and (avg_z <= 1) and (not move_forward): if (avg_y <= 1.5) and (avg_z <= 1.5) and (not move_forward): self._log("Moving forward with visual navigation") move_forward = True # While you are moving towards the gate elif ((avg_y >= 2) or (avg_z >= 2)) and (move_forward): # elif ((avg_y >= 2) or (avg_z >= 2)) and (move_forward): # Only pause if we have found the gate and its not too close if self._gatefound and self.full_gate_found and self._gate_area <= 180000: self._log(self._gate_area) self._log("Pausing for re-alignment") move_forward = False # Send quick burst to kill forward momemtum direction_backforward = 100 # If we want to start moving forward, as the gate is now lined up if move_forward == True and not self.close_gate_navigation: direction_backforward = -1 # If we cant see the gate anymore and were moving forward, we need to switch to move forward only mode if not self.full_gate_found: self._log("Gate too close, switching to quadrant navigation") self.close_gate_navigation = True # Fly forward to make it through the gate if self.close_gate_navigation: if self._gatefound: # The center of mass is now calculated different and so we can use it to do the final navigation direction_y = leftright direction_z = -1 * updown direction_backforward = -5 # If we cant see any gate go straight else: direction_y = 0 direction_z = 0 direction_backforward = -7 # Move backwards so you can see the gate again if (not self.full_gate_found) and (not self.close_gate_navigation): direction_backforward = 2 # If the full gate has never been found slowly move forward until you find it if (self.full_gate_ever_found == False): direction_backforward = -1 # Allow movement is triggered if the yaw is off if not self.allow_movement: direction_y = 0 direction_z = 0 direction_backforward = 0 # Make sure the commands are int's between -100 and 100 direction_y = int(round(max(min(direction_y, 100), -100),0)) direction_z = int(round(max(min(direction_z, 100), -100),0)) direction_backforward = int(round(max(min(direction_backforward, 100), -100),0)) # If you can't see a gate anymore reset PID and send stop command if self._gatefound: stop_counter = 0 else: self.z_controller.remove_buildup() self.y_controller.remove_buildup() stop_counter += 1 # If we havent seen the gate for 2 seconds stop if stop_counter >= self.rate * 2: direction_y = 0 direction_z = 0 direction_backforward = 0 # Publish the message msg = Move() msg.left_right = direction_y msg.up_down = direction_z msg.front_back = direction_backforward msg.yawl_yawr = 0 self.move_pub.publish(msg) r.sleep()