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 __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
stable_gains = rospy.get_param(
'/position_controller_node/gains/stable/', {
'p': 1,
'i': 0.0,
'd': 0.0
})
Kp, Ki, Kd = stable_gains['p'], stable_gains['i'], stable_gains['d']
# 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.position_PIDs = [PID(Kp, Ki, Kd, self.rate) for _ in range(3)]
# Get the setpoints
self.setpoints = np.array([0.0, 0.0, 3.0], dtype=np.float64)
# Create the current output readings
self.current_position = np.zeros(3, dtype=np.float64)
# Create the subscribers
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)
# Create the publishers
self.vel_set_sub = rospy.Publisher('/uav/input/velocity',
Vector3,
queue_size=1)
# Run the control loop
self.ControlLoop()
def __init__(self):
# Initial setpoint for the z-position of the drone
self.initial_set_z = 30.0
# Setpoint for the z-position of the drone
self.set_z = self.initial_set_z
# Current z-position of the drone according to sensor data
self.current_z = 0
# Tracks x, y velocity error and z-position error
self.error = axes_err()
# PID controller
self.pid = PID()
# Setpoint for the x-velocity of the drone
self.set_vel_x = 0
# Setpoint for the y-velocity of the drone
self.set_vel_y = 0
# Time of last heartbeat from the web interface
self.last_heartbeat = None
# Commanded yaw velocity of the drone
self.cmd_yaw_velocity = 0
# Tracks previous drone attitude
self.prev_angx = 0
self.prev_angy = 0
# Time of previous attitude measurement
self.prev_angt = None
# Angle compensation values (to account for tilt of drone)
self.mw_angle_comp_x = 0
self.mw_angle_comp_y = 0
self.mw_angle_alt_scale = 1.0
self.mw_angle_coeff = 10.0
self.angle = TwistStamped()
# Desired and current mode of the drone
self.commanded_mode = self.DISARMED
self.current_mode = self.DISARMED
# Flight controller that receives commands to control motion of the drone
self.board = MultiWii("/dev/ttyUSB0")
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
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()) + ": Lauching 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
# 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)
# Run the communication node
self.ControlLoop()
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")
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()
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']
# 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.rpy_PIDS = [PID(Kp, Ki, Kd, self.rate) for _ in range(3)]
# Create the setpoints
self.rpy_setpoints = np.zeros(3, dtype=np.float64)
self.thrust_setpoint = 10
# Create the current output readings
self.rpy_readings = np.zeros(3, dtype=np.float64)
# Check if the drone has been armed
self.armed = False
# Create the subscribers
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)
self.armed_sub = rospy.Subscriber("/uav/armed", Bool,
self.armed_callback)
self.yaw_sub = rospy.Subscriber("/uav/input/yaw", Float64,
self.set_yaw_output)
# Create the publishers
self.rate_pub = rospy.Publisher('/uav/input/rateThrust',
RateThrust,
queue_size=10)
# Run the control loop
self.ControlLoop()
import sys
import signal
import yaml
# stefie10: High-level comments: 1) Make a class 2) put everything
# inside a main method and 3) no global variables.
landing_threshold = 9.
initial_set_z = 0.13
set_z = initial_set_z
init_z = 0
smoothed_vel = np.array([0, 0, 0])
alpha = 1.0
ultra_z = 0
pid = PID()
first = True
error = axes_err()
cmds = [1500, 1500, 1500, 900]
current_mode = 4
set_vel_x = 0
set_vel_y = 0
errpub = rospy.Publisher('/pidrone/err', axes_err, queue_size=1)
modepub = rospy.Publisher('/pidrone/mode', Mode, queue_size=1)
flow_height_z = 0.000001
reset_pid = True
pid_is = [0,0,0,0]
last_heartbeat = None
cmd_velocity = [0, 0]
cmd_yaw_velocity = 0
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")
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()
x += dx
return x
VIDEO = 1
im_count = 0
x1 = x0 + 0.0
x2 = x0 + 0.0
x3 = x0 + 0.0
spring_history = np.zeros((2, steps))
noise_history = np.zeros((2, steps))
pid_history = np.zeros((2, steps))
pid = PID(dt)
target = np.ones(steps)
target[:100] = 0
target[100:300] = -5.0
target[300:500] = 10.0
target[500:] = 0
for t in range(steps):
x1 = transition(x1, 0, noise[t])
# x3 = transition(x3, control, noise[t])
control = pid.control(x3[0], target[t])
x3 = transition(x3, control, noise[t])
# spring_history[:,t] = x1.ravel()
noise_history[:, t] = x1.ravel()
global init_z
vrpn_pos = data
if init_z is None:
init_z = vrpn_pos.pose.position.z
def ultra_callback(data):
global ultra_z
if data.range != -1:
ultra_z = data.range
if __name__ == '__main__':
rospy.init_node('velocity_flight')
rospy.Subscriber("/pidrone/est_pos", PoseStamped, vrpn_update_pos)
rospy.Subscriber("/pidrone/ultrasonic", Range, ultra_callback)
homography = Homography()
pid = PID()
first = True
board = MultiWii("/dev/ttyACM0")
while vrpn_pos is None:
if not rospy.is_shutdown():
print "No VRPN :("
time.sleep(0.01)
else:
print "Shutdown Recieved"
sys.exit()
stream = streamPi()
try:
while not rospy.is_shutdown():
curr_img = cv2.cvtColor(np.array(stream.next()), cv2.COLOR_RGB2GRAY)
if first:
homography.update_H(curr_img, curr_img)
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 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 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()
def __init__(self):
# Initialize the current and desired modes
self.current_mode = Mode('DISARMED')
self.desired_mode = Mode('DISARMED')
# Initialize in velocity control
self.position_control = False
# Initialize the current and desired positions
self.current_position = Position()
self.desired_position = Position(z=0.3)
self.last_desired_position = Position(z=0.3)
# Initialize the position error
self.position_error = Error()
# Initialize the current and desired velocities
self.current_velocity = Velocity()
self.desired_velocity = Velocity()
# Initialize the velocity error
self.velocity_error = Error()
# Set the distance that a velocity command will move the drone (m)
self.desired_velocity_travel_distance = 0.1
# Set a static duration that a velocity command will be held
self.desired_velocity_travel_time = 0.1
# Set a static duration that a yaw velocity command will be held
self.desired_yaw_velocity_travel_time = 0.25
# Store the start time of the desired velocities
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
# Initialize the primary PID
self.pid = PID()
# Initialize the error used for the PID which is vx, vy, z where vx and
# vy are velocities, and z is the error in the altitude of the drone
self.pid_error = Error()
# Initialize the 'position error to velocity error' PIDs:
# left/right (roll) pid
self.lr_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# front/back (pitch) pid
self.fb_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# Initialize the pose callback time
self.last_pose_time = None
# Initialize the desired yaw velocity
self.desired_yaw_velocity = 0
# Initialize the current and previous roll, pitch, yaw values
self.current_rpy = RPY()
self.previous_rpy = RPY()
# initialize the current and previous states
self.current_state = State()
self.previous_state = State()
# Store the command publisher
self.cmdpub = None
# a variable used to determine if the drone is moving between disired
# positions
self.moving = False
# a variable that determines the maximum magnitude of the position error
# Any greater position error will overide the drone into velocity
# control
self.safety_threshold = 1.5
# determines if the position of the drone is known
self.lost = False
# determines if the desired poses are aboslute or relative to the drone
self.absolute_desired_position = False
# determines whether to use open loop velocity path planning which is
# accomplished by calculate_travel_time
self.path_planning = True
class PIDController(object):
''' Controls the flight of the drone by running a PID controller on the
error calculated by the desired and current velocity and position of the drone
'''
def __init__(self):
# Initialize the current and desired modes
self.current_mode = Mode('DISARMED')
self.desired_mode = Mode('DISARMED')
# Initialize in velocity control
self.position_control = False
# Initialize the current and desired positions
self.current_position = Position()
self.desired_position = Position(z=0.3)
self.last_desired_position = Position(z=0.3)
# Initialize the position error
self.position_error = Error()
# Initialize the current and desired velocities
self.current_velocity = Velocity()
self.desired_velocity = Velocity()
# Initialize the velocity error
self.velocity_error = Error()
# Set the distance that a velocity command will move the drone (m)
self.desired_velocity_travel_distance = 0.1
# Set a static duration that a velocity command will be held
self.desired_velocity_travel_time = 0.1
# Set a static duration that a yaw velocity command will be held
self.desired_yaw_velocity_travel_time = 0.25
# Store the start time of the desired velocities
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
# Initialize the primary PID
self.pid = PID()
# Initialize the error used for the PID which is vx, vy, z where vx and
# vy are velocities, and z is the error in the altitude of the drone
self.pid_error = Error()
# Initialize the 'position error to velocity error' PIDs:
# left/right (roll) pid
self.lr_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# front/back (pitch) pid
self.fb_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# Initialize the pose callback time
self.last_pose_time = None
# Initialize the desired yaw velocity
self.desired_yaw_velocity = 0
# Initialize the current and previous roll, pitch, yaw values
self.current_rpy = RPY()
self.previous_rpy = RPY()
# initialize the current and previous states
self.current_state = State()
self.previous_state = State()
# Store the command publisher
self.cmdpub = None
# a variable used to determine if the drone is moving between disired
# positions
self.moving = False
# a variable that determines the maximum magnitude of the position error
# Any greater position error will overide the drone into velocity
# control
self.safety_threshold = 1.5
# determines if the position of the drone is known
self.lost = False
# determines if the desired poses are aboslute or relative to the drone
self.absolute_desired_position = False
# determines whether to use open loop velocity path planning which is
# accomplished by calculate_travel_time
self.path_planning = True
# ROS SUBSCRIBER CALLBACK METHODS
#################################
def current_state_callback(self, state):
""" Store the drone's current state for calculations """
self.previous_state = self.current_state
self.current_state = state
self.state_to_three_dim_vec_structs()
def desired_pose_callback(self, msg):
""" Update the desired pose """
# store the previous desired position
self.last_desired_position = self.desired_position
# set the desired positions equal to the desired pose message
if self.absolute_desired_position:
self.desired_position.x = msg.position.x
self.desired_position.y = msg.position.y
# the desired z must be above z and below the range of the ir sensor (.55meters)
self.desired_position.z = msg.position.z if 0 <= desired_z <= 0.5 else self.last_desired_position.z
# set the desired positions relative to the current position (except for z to make it more responsive)
else:
self.desired_position.x = self.current_position.x + msg.position.x
self.desired_position.y = self.current_position.y + msg.position.y
# set the disired z position relative to the last desired position (doesn't limit the mag of the error)
# the desired z must be above z and below the range of the ir sensor (.55meters)
desired_z = self.last_desired_position.z + msg.position.z
self.desired_position.z = desired_z if 0 <= desired_z <= 0.5 else self.last_desired_position.z
if self.desired_position != self.last_desired_position:
# the drone is moving between desired positions
self.moving = True
print 'moving'
def desired_twist_callback(self, msg):
""" Update the desired twist """
self.desired_velocity.x = msg.linear.x
self.desired_velocity.y = msg.linear.y
self.desired_velocity.z = msg.linear.z
self.desired_yaw_velocity = msg.angular.z
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
if self.path_planning:
self.calculate_travel_time()
def current_mode_callback(self, msg):
""" Update the current mode """
self.current_mode = msg.mode
def desired_mode_callback(self, msg):
""" Update the desired mode """
self.desired_mode = msg.mode
def position_control_callback(self, msg):
""" Set whether or not position control is enabled """
self.position_control = msg.data
if self.position_control:
self.reset_callback(Empty())
print "Position Control", self.position_control
def reset_callback(self, empty):
""" Reset the desired and current poses of the drone and set
desired velocities to zero """
self.current_position = Position(z=self.current_position.z)
self.desired_position = Position(z=self.current_position.z)
self.desired_velocity.x = 0
self.desired_velocity.y = 0
def lost_callback(self, msg):
self.lost = msg.data
# Step Method
#############
def step(self):
""" Returns the commands generated by the pid """
self.calc_error()
if self.position_control:
if self.position_error.planar_magnitude(
) < self.safety_threshold and not self.lost:
if self.moving:
if self.position_error.magnitude() > 0.05:
self.pid_error -= self.velocity_error * 100
else:
self.moving = False
print 'not moving'
else:
self.position_control_pub.publish(False)
if self.desired_velocity.magnitude() > 0 or abs(
self.desired_yaw_velocity) > 0:
self.adjust_desired_velocity()
return self.pid.step(self.pid_error, self.desired_yaw_velocity)
# HELPER METHODS
################
def state_to_three_dim_vec_structs(self):
"""
Convert the values from the state estimator into ThreeDimVec structs to
make calculations concise
"""
# store the positions
pose = self.current_state.pose_with_covariance.pose
self.current_position.x = pose.position.x
self.current_position.y = pose.position.y
self.current_position.z = pose.position.z
# store the linear velocities
twist = self.current_state.twist_with_covariance.twist
self.current_velocity.x = twist.linear.x
self.current_velocity.y = twist.linear.y
self.current_velocity.z = twist.linear.z
# store the orientations
self.previous_rpy = self.current_rpy
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
r, p, y = tf.transformations.euler_from_quaternion(quaternion)
self.current_rpy = RPY(r, p, y)
def adjust_desired_velocity(self):
""" Set the desired velocity back to 0 once the drone has traveled the
amount of time that causes it to move the specified desired velocity
travel distance if path_planning otherwise just set the velocities back
to 0 after the . This is an open loop method meaning that the specified
travel distance cannot be guarenteed. If path planning_planning is false,
just set the velocities back to zero, this allows the user to move the
drone for as long as they are holding down a key
"""
curr_time = rospy.get_time()
# set the desired planar velocities to zero if the duration is up
if self.desired_velocity_start_time is not None:
# the amount of time the set point velocity is not zero
duration = curr_time - self.desired_velocity_start_time
if duration > self.desired_velocity_travel_time:
self.desired_velocity.x = 0
self.desired_velocity.y = 0
self.desired_velocity_start_time = None
else:
self.desired_velocity_start_time = curr_time
# set the desired yaw velocity to zero if the duration is up
if self.desired_yaw_velocity_start_time is not None:
# the amount of time the set point velocity is not zero
duration = curr_time - self.desired_yaw_velocity_start_time
if duration > self.desired_yaw_velocity_travel_time:
self.desired_yaw_velocity = 0
self.desired_yaw_velocity_start_time = None
else:
self.desired_yaw_velocity_start_time = curr_time
def calc_error(self):
""" Calculate the error in velocity, and if in position hold, add the
error from lr_pid and fb_pid to the velocity error to control the
position of the drone
"""
# store the time difference
pose_dt = 0
if self.last_pose_time != None:
pose_dt = rospy.get_time() - self.last_pose_time
self.last_pose_time = rospy.get_time()
# calculate the velocity error
self.velocity_error = self.desired_velocity - self.current_velocity
# calculate the z position error
dz = self.desired_position.z - self.current_position.z
# calculate the pid_error from the above values
self.pid_error.x = self.velocity_error.x
self.pid_error.y = self.velocity_error.y
self.pid_error.z = dz
# multiply by 100 to account for the fact that code was originally written using cm
self.pid_error = self.pid_error * 100
if self.position_control:
# calculate the position error
self.position_error = self.desired_position - self.current_position
# calculate a value to add to the velocity error based based on the
# position error in the x (roll) direction
lr_step = self.lr_pid.step(self.position_error.x, pose_dt)
# calculate a value to add to the velocity error based based on the
# position error in the y (pitch) direction
fb_step = self.fb_pid.step(self.position_error.y, pose_dt)
self.pid_error.x += lr_step
self.pid_error.y += fb_step
def calculate_travel_time(self):
''' return the amount of time that desired velocity should be used to
calculate the error in order to move the drone the specified travel
distance for a desired velocity
'''
if self.desired_velocity.magnitude() > 0:
# tiime = distance / velocity
travel_time = self.desired_velocity_travel_distance / self.desired_velocity.planar_magnitude(
)
else:
travel_time = 0.0
self.desired_velocity_travel_time = travel_time
def reset(self):
''' Set desired_position to be current position, set
filtered_desired_velocity to be zero, and reset both the PositionPID
and VelocityPID
'''
# reset position control variables
self.position_error = Error(0, 0, 0)
self.desired_position = Position(self.current_position.x,
self.current_position.y, 0.3)
# reset velocity control_variables
self.velocity_error = Error(0, 0, 0)
self.desired_velocity = Velocity(0, 0, 0)
# reset the pids
self.pid.reset()
self.lr_pid.reset()
self.fb_pid.reset()
def ctrl_c_handler(self, signal, frame):
""" Gracefully handles ctrl-c """
print 'Caught ctrl-c\n Stopping Controller'
sys.exit()
def publish_cmd(self, cmd):
"""Publish the controls to /pidrone/fly_commands """
msg = RC()
msg.roll = cmd[0]
msg.pitch = cmd[1]
msg.yaw = cmd[2]
msg.throttle = cmd[3]
self.cmdpub.publish(msg)
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_PIDs = [PID(Kp_xy, Ki_xy, Kd_xy, self.rate) for _ in range(2)]
self.vel_PIDs.append(PID(Kp_z, Ki_z, Kd_z, self.rate))
# Get the setpoints
self.setpoints = np.zeros(3, dtype=np.float64)
# Create the current output readings
self.vel_readings = np.zeros(3, dtype=np.float64)
# Reading the yaw
self.yaw_reading = 0
# Create the subscribers
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.sub = rospy.Subscriber('/uav/sensors/attitude', Vector3,
self.euler_angle_callback)
# Create the publishers
self.att_pub = rospy.Publisher("/uav/input/attitude",
Vector3,
queue_size=1)
self.thrust_pub = rospy.Publisher('/uav/input/thrust',
Float64,
queue_size=1)
# Run the control loop
self.ControlLoop()
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 __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)
def callback(data):
global alive_1
global vel
###PID parameter to be tuned for simulation and real drone
max_val = 0.8 #maximum value of command velocity
min_val = -0.8 #minimum value of command velocity
p_x = PID(2.5, 0.5, 1.2) #parameters P, I and D. Tune it accordingly
p_x.setPoint(0.5) #set point for cmd_vel, range is 0 - 1. 0.5 is center
pid_x = p_x.update(data.x) #update value
pid_x = clamp(pid_x, min_val, max_val) #clamp value
vel.twist.linear.x = pid_x #update vel value to be published
p_y = PID(2.5, 0.5, 1.2)
p_y.setPoint(0.5)
pid_y = p_y.update(data.y)
pid_y = clamp(pid_y, min_val, max_val)
vel.twist.linear.y = pid_y
p_z = PID(3.0, 0.5, 1.2)
p_z.setPoint(1)
if data.z > 0.95 and data.z < 1.05:
pid_z = 0
else:
pid_z = p_z.update(data.z) / 5
pid_z = clamp(pid_z, min_val, max_val)
vel.twist.linear.z = pid_z
alive_1 += 1
vel.header.stamp = rospy.Time.now()
