class PersonNavigation:
def __init__(self):
# Init the ROS node
rospy.init_node('PersonNavigationNode')
self._log("Node Initialized")
# On shutdown do the following
rospy.on_shutdown(self.stop)
# Set the rate
self.rate = 15.0
self.dt = 1.0 / self.rate
# Init the drone and program state
self._quit = False
self._infollowmode = False
# Used to keep track of the object
self.object_center_x_arr = None
self.object_center_y_arr = None
self.object_size_arr = None
self.object_arr_length = 5
self.image_size = None
# PID Class to keep the person in view
a_gains = rospy.get_param(rospy.get_name() + '/gains', {
'p_alignment': 0.25,
'i_alignment': 0.0,
'd_alignment': 1.0
})
d_gains = rospy.get_param(rospy.get_name() + '/gains', {
'p_distance': 1.0,
'i_distance': 0.0,
'd_distance': 0.0
})
alignment_Kp, alignment_Ki, alignment_Kd = a_gains[
'p_alignment'], a_gains['i_alignment'], a_gains['d_alignment']
distance_Kp, distance_Ki, distance_Kd = d_gains['p_distance'], d_gains[
'i_distance'], d_gains['d_distance']
# Display the variables
self._log(": Alignment p - " + str(alignment_Kp))
self._log(": Alignment i - " + str(alignment_Ki))
self._log(": Alignment d - " + str(alignment_Kd))
self._log(": Distance p - " + str(distance_Kp))
self._log(": Distance i - " + str(distance_Ki))
self._log(": Distance d - " + str(distance_Kd))
# Create the controllers
self.distance_controller = PID(distance_Kp, distance_Ki, distance_Kd,
self.rate)
self.alignment_controller = PID(alignment_Kp, alignment_Ki,
alignment_Kd, self.rate)
# 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.bounding_sub = rospy.Subscriber("darknet_ros/bounding_boxes",
BoundingBoxes, self._getbounding)
self.camera_sub = rospy.Subscriber("/mixer/sensors/camera", Image,
self._getimagesize)
# self.vicon_yaw_pub = rospy.Publisher("/uav1/input/vicon_setpoint/yaw/rate", Float32, queue_size=10)
def start(self):
self._mainloop()
def stop(self):
self._quit = True
def _getstate(self, msg):
if msg.data == DroneState.FOLLOWPERSON.value:
self._infollowmode = True
def _getimagesize(self, msg):
data_size = (msg.width, msg.height)
# Get the size of the image
if self.image_size is None:
self.image_size = data_size
else:
if self.image_size != data_size:
self.image_size = data_size
def _getbounding(self, msg):
# Arrays to store the bounding box information
person_data = []
# Go through each bounding box
for box in msg.bounding_boxes:
# If it is a person
if box.Class == "person" and box.probability >= 0.65:
person_data.append([box.xmin, box.xmax, box.ymin, box.ymax])
# If we have people data
if len(person_data) <= 0:
return
else:
# If there is more than one person
if len(person_data) > 1:
max_area = 0
max_data = []
# Find the largest person
for person in person_data:
area = (person[1] - person[0]) * (person[3] - person[2])
if area > max_area:
max_area = area
max_data = person
xmin = max_data[0]
xmax = max_data[1]
ymin = max_data[2]
ymax = max_data[3]
else:
xmin = person_data[0][0]
xmax = person_data[0][1]
ymin = person_data[0][2]
ymax = person_data[0][3]
# Compute the area and the center of the object
len_side_x = xmax - xmin
len_side_y = ymax - ymin
obj_centre_x = (len_side_x / 2.0) + xmin
obj_centre_y = (len_side_y / 2.0) + ymin
obj_area = len_side_x * len_side_y
# If we have never computed a center before
if self.object_center_x_arr is None:
self.object_center_x_arr = np.full(self.object_arr_length,
obj_centre_x)
self.object_center_y_arr = np.full(self.object_arr_length,
obj_centre_y)
self.object_size_arr = np.full(self.object_arr_length,
obj_area)
else:
self.object_center_x_arr = np.roll(self.object_center_x_arr, 1)
self.object_center_y_arr = np.roll(self.object_center_y_arr, 1)
self.object_size_arr = np.roll(self.object_size_arr, 1)
self.object_center_x_arr[0] = obj_centre_x
self.object_center_y_arr[0] = obj_centre_y
self.object_size_arr[0] = obj_area
def _log(self, msg):
print(str(rospy.get_name()) + ": " + str(msg))
def _mainloop(self):
r = rospy.Rate(self.rate)
movement_f = None
movement_x = None
movement_y = None
while not self._quit:
if self._infollowmode:
area = 0
cen_x = 0
cen_y = 0
# Calculate what the drones current points
try:
area = np.average(self.object_size_arr)
cen_x = np.average(self.object_center_x_arr)
cen_y = np.average(self.object_center_y_arr)
# Get a percentage away from the center lines
cen_x = (cen_x - self.image_size[0] / 2.0) / (
self.image_size[0] / 2.0)
cen_y = (cen_y - self.image_size[1] / 2.0) / (
self.image_size[1] / 2.0)
# How much of the screen you want to take up (i.e. 10th of the image)
percentage_covered = 1 / 10.0
desired_area = (self.image_size[0] *
self.image_size[1]) * percentage_covered
area = (area - desired_area) / desired_area
except (TypeError):
# This happens if you cant see the person
continue
# Compute how much you want to move backwards and forwards
forward_backward = self.distance_controller.get_output(
0, -1 * area)
forward_backward = min(max(forward_backward, -1), 1)
# Compute how much you want to move sideways
sideways_movement = self.alignment_controller.get_output(
0, -1 * cen_x)
sideways_movement = min(max(sideways_movement, -1), 1)
# Create the message
direction_yaw = 0
direction_x = 0
direction_y = sideways_movement
direction_z = forward_backward
msg = Move()
msg.up_down = int(round(direction_x * 100, 0))
msg.left_right = int(round(direction_y * 100, 0))
msg.front_back = int(round(direction_z * 100, 0))
msg.yawl_yawr = 0
# msg.yawl_yawr = int(round(cen_x * 100, 0))
# Compute how much to change the yaw in degrees
# yaw_rate = math.radians(-cen_x * 20)
# Publish the vicon yaw command
# self.vicon_yaw_pub.publish(Float32(yaw_rate))
# Publish the move command
self.move_pub.publish(msg)
r.sleep()
class VelocityController():
def __init__(self):
# Allow the simulator to start
time.sleep(5)
# 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
gains = rospy.get_param('/velocity_controller_node/gains', {
'p_xy': 1,
'i_xy': 0.0,
'd_xy': 0.0,
'p_z': 1,
'i_z': 0.0,
'd_z': 0.0
})
Kp_xy, Ki_xy, Kd_xy = gains['p_xy'], gains['i_xy'], gains['d_xy']
Kp_z, Ki_z, Kd_z = gains['p_z'], gains['i_z'], gains['d_z']
# Display incoming parameters
rospy.loginfo(
str(rospy.get_name()) +
": Launching with the following parameters:")
rospy.loginfo(str(rospy.get_name()) + ": p_xy - " + str(Kp_xy))
rospy.loginfo(str(rospy.get_name()) + ": i_xy - " + str(Ki_xy))
rospy.loginfo(str(rospy.get_name()) + ": d_xy - " + str(Kd_xy))
rospy.loginfo(str(rospy.get_name()) + ": p_z - " + str(Kp_z))
rospy.loginfo(str(rospy.get_name()) + ": i_z - " + str(Ki_z))
rospy.loginfo(str(rospy.get_name()) + ": d_z - " + str(Kd_z))
rospy.loginfo(str(rospy.get_name()) + ": rate - " + str(self.rate))
# Creating the PID's
self.vel_x_PID = PID(Kp_xy, Ki_xy, Kd_xy, self.rate)
self.vel_y_PID = PID(Kp_xy, Ki_xy, Kd_xy, self.rate)
self.vel_z_PID = PID(Kp_z, Ki_z, Kd_z, self.rate)
# Get the setpoints
self.x_setpoint = 0
self.y_setpoint = 0
self.z_setpoint = 0
# Create the current output readings
self.x_vel = 0
self.y_vel = 0
self.z_vel = 0
# Reading the yaw
self.yaw_reading = 0
# Create the subscribers and publishers
self.vel_read_sub = rospy.Subscriber("/uav/sensors/velocity",
TwistStamped, self.get_vel)
self.vel_set_sub = rospy.Subscriber('/uav/input/velocity', Vector3,
self.set_vel)
self.att_pub = rospy.Publisher("/uav/input/attitude",
Vector3,
queue_size=1)
self.thrust_pub = rospy.Publisher('/uav/input/thrust',
Float64,
queue_size=1)
self.sub = rospy.Subscriber('/uav/sensors/attitude', Vector3,
self.euler_angle_callback)
# Run the communication node
self.ControlLoop()
def euler_angle_callback(self, msg):
self.yaw_reading = msg.z
# This is the main loop of this class
def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# Data we will be publishing
z_output = Float64()
z_output.data = 0
# While running
while not rospy.is_shutdown():
# Use a PID to calculate the angle you want to hold and thrust you want
x_output_w = self.vel_x_PID.get_output(self.x_setpoint, self.x_vel)
y_output_w = self.vel_y_PID.get_output(self.y_setpoint, self.y_vel)
z_output.data = self.vel_z_PID.get_output(self.z_setpoint,
self.z_vel) + 9.8
# Rotation from a world frame to drone frame
x_output = -math.cos(self.yaw_reading) * x_output_w - math.sin(
self.yaw_reading) * y_output_w
y_output = math.sin(self.yaw_reading) * x_output_w - math.cos(
self.yaw_reading) * y_output_w
# Create and publish the data (0 yaw)
attitude = Vector3(x_output, y_output, -1.57)
self.att_pub.publish(attitude)
self.thrust_pub.publish(z_output)
# Limit the thrust to the drone
if z_output.data < 0:
z_output.data = 0
if z_output.data > 20:
z_output.data = 20
# Sleep any excess time
rate.sleep()
# Call back to get the velocity data
def get_vel(self, vel_msg):
self.x_vel = vel_msg.twist.linear.x
self.y_vel = vel_msg.twist.linear.y
self.z_vel = vel_msg.twist.linear.z
# Call back to get the velocity setpoints
def set_vel(self, vel_msg):
self.x_setpoint = vel_msg.x
self.y_setpoint = vel_msg.y
self.z_setpoint = vel_msg.z
# Called on ROS shutdown
def shutdown_sequence(self):
rospy.loginfo(str(rospy.get_name()) + ": Shutting Down")
class AngleController():
def __init__(self):
# Allow the simulator to start
time.sleep(5)
# Run the shutdown sequence on shutdown
rospy.on_shutdown(self.shutdown_sequence)
# Getting the PID parameters
gains = rospy.get_param('/angle_controller_node/gains', {
'p': 0.1,
'i': 0,
'd': 0
})
Kp, Ki, Kd = gains['p'], gains['i'], gains['d']
# Getting the yaw flag
self.no_yaw = rospy.get_param('/angle_controller_node/no_yaw', False)
# Set the rate
self.rate = 100.0
self.dt = 1.0 / self.rate
# Display incoming parameters
rospy.loginfo(
str(rospy.get_name()) +
": Launching 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.rollPID = PID(Kp, Ki, Kd, self.rate)
self.pitchPID = PID(Kp, Ki, Kd, self.rate)
if self.no_yaw:
self.yaw_output = 0.0
self.yawPID = PID(Kp, Ki, Kd, self.rate)
# Create the setpoints
self.roll_setpoint = 0
self.pitch_setpoint = 0
self.yaw_setpoint = -1.57
self.thrust_setpoint = 10
# Create the current output readings
self.roll_reading = 0
self.pitch_reading = 0
self.yaw_reading = 0
# Check if the drone has been armed
self.armed = False
# Publishers and Subscribers
self.rate_pub = rospy.Publisher('/uav/input/rateThrust',
RateThrust,
queue_size=10)
self.imu_sub = rospy.Subscriber("/uav/sensors/attitude", Vector3,
self.euler_angle_callback)
self.att_sub = rospy.Subscriber("/uav/input/attitude", Vector3,
self.attitude_set_callback)
self.thrust_sub = rospy.Subscriber("/uav/input/thrust", Float64,
self.thrust_callback)
if self.no_yaw:
self.yaw_sub = rospy.Subscriber("/uav/input/yaw", Float64,
self.set_yaw_output)
# Run the communication node
self.ControlLoop()
# This is the callback for the yaw subscriber
def set_yaw_output(self, msg):
#rospy.loginfo("Received yaw" + str(msg.data))
self.yaw_output = msg.data
# This is the main loop of this class
def ControlLoop(self):
# Set the rate
rate = rospy.Rate(50)
# Keep track how many loops have happend
loop_counter = 0
# While running
while not rospy.is_shutdown():
# Create the message we are going to send
msg = RateThrust()
msg.header.stamp = rospy.get_rostime()
# If the drone has not been armed
if self.armed == False:
# Arm the drone
msg.thrust = Vector3(0, 0, 10)
msg.angular_rates = Vector3(0, 0, 0)
self.armed = True
# Behave normally
else:
# Use a PID to calculate the rates you want to publish to maintain an angle
roll_output = self.rollPID.get_output(self.roll_setpoint,
self.roll_reading)
pitch_output = self.pitchPID.get_output(
self.pitch_setpoint, self.pitch_reading)
if self.no_yaw:
yaw_output = self.yaw_output
#rospy.loginfo("my output: " + str(yaw_output) + " old yaw:" + str(self.yawPID.get_output(self.yaw_setpoint, self.yaw_reading)))
else:
yaw_output = self.yawPID.get_output(
self.yaw_setpoint, self.yaw_reading)
# Create the message
msg.thrust = Vector3(0, 0, self.thrust_setpoint)
msg.angular_rates = Vector3(roll_output, pitch_output,
yaw_output)
# Publish the message
self.rate_pub.publish(msg)
# Sleep any excess time
rate.sleep()
# Save the new attitude readings
def euler_angle_callback(self, msg):
self.roll_reading = msg.x
self.pitch_reading = msg.y
self.yaw_reading = msg.z
# Save the new attitude setpoints
def attitude_set_callback(self, msg):
# Dont allow angles greater than 0.5 for x and y
self.roll_setpoint = max(min(msg.x, 0.5), -0.5)
self.pitch_setpoint = max(min(msg.y, 0.5), -0.5)
self.yaw_setpoint = msg.z
# Save the new thrust setpoints
def thrust_callback(self, msg):
self.thrust_setpoint = msg.data
# Called on ROS shutdown
def shutdown_sequence(self):
rospy.loginfo(str(rospy.get_name()) + ": Shutting Down")
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 ViconYawControl:
def __init__(self):
# Init the ROS node
rospy.init_node('ViconYawControlnNode')
self._log("Node Initialized")
# On shutdown do the following
rospy.on_shutdown(self.stop)
# Set the move goal
self._true_yaw = 0
self._desired_yaw = None
self._desired_yaw_wait_count = 1
# Set the rate
self.rate = 30.0
self.dt = 1.0 / self.rate
# Out of range param
self.out_of_range_value = rospy.get_param(
rospy.get_name() + '/out_of_range_yaw', 30)
self.use_starting_yaw = rospy.get_param(
rospy.get_name() + '/use_starting_yaw', True)
# Get the PID
gains = rospy.get_param(rospy.get_name() + '/gains', {
'p': 10.0,
'i': 0.0,
'd': 0.0
})
Kp, Ki, Kd = gains['p'], gains['i'], gains['d']
self.yaw_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))
self._log(": Out of range - " + str(self.out_of_range_value))
# Init the drone and program state
self._quit = False
# Create the publishers and subscribers
self.yaw_pub = rospy.Publisher("/uav1/input/yawcorrection",
Int8,
queue_size=10)
self.debug_desired_pub = rospy.Publisher("/debug/DesiredYaw",
Float32,
queue_size=10)
self.debug_actual_pub = rospy.Publisher("/debug/ActualYaw",
Float32,
queue_size=10)
self.out_of_range_pub = rospy.Publisher(
"/uav1/status/yaw_out_of_range", Bool, queue_size=10)
self.true_yaw_sub = rospy.Subscriber("/vicon/ANAFI1/ANAFI1",
TransformStamped, self._getvicon)
self.set_yaw_sub = rospy.Subscriber(
"/uav1/input/vicon_setpoint/yaw/rate", Float32,
self._updateSetpoint)
def start(self):
self._mainloop()
def stop(self):
self._quit = True
def _updateSetpoint(self, msg):
self._desired_yaw = self._true_yaw + msg.data
def _getvicon(self, msg):
rot = msg.transform.rotation
self._true_yaw = euler_from_quaternion(quaternion=(rot.x, rot.y, rot.z,
rot.w))[2]
def _log(self, msg):
print(str(rospy.get_name()) + ": " + str(msg))
def _mainloop(self):
r = rospy.Rate(self.rate)
count = 0
while not self._quit:
# Check if we want to use 0
if not self.use_starting_yaw:
self._desired_yaw = 0
# get the initial position of the drone
if self._desired_yaw is None:
if count < self._desired_yaw_wait_count:
count += 1
else:
self._desired_yaw = self._true_yaw
# We have our initial position
else:
# Debug the true and actual yaw
desired_yaw_msg = Float32()
desired_yaw_msg.data = self._desired_yaw
self.debug_desired_pub.publish(desired_yaw_msg)
actual_yaw_msg = Float32()
actual_yaw_msg.data = self._true_yaw
self.debug_actual_pub.publish(actual_yaw_msg)
# Check if we are out of range
range_msg = Bool()
range_msg.data = False
if abs(self._desired_yaw - self._true_yaw) < math.radians(
self.out_of_range_value):
range_msg.data = True
self.out_of_range_pub.publish(range_msg)
# Compute the output
output = self.yaw_controller.get_output(
self._desired_yaw, self._true_yaw)
output = -1 * output
msg = Int8()
msg.data = int(min(max(round(output, 0), -100), 100))
self.yaw_pub.publish(msg)
r.sleep()
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()