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()
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()
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):
# 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 __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()
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()
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
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()
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
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)
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()
