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 __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