def hover_rec(self, time_flying):
recursions = self.calculate_recursions(time_flying)
print(recursions)
for i in range(recursions):
print(i)
print(self.down_sensor_distance)
if self.beh_type == 'HOVER' and (self.hover_sensor_altitude_max >=
self.down_sensor_distance >=
self.hover_sensor_altitude_min):
print("The drone is hovering")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) #was 0.005 (now 50hz ,500loops)
elif self.beh_type == 'HOVER' and (
self.hover_sensor_altitude_max <=
self.down_sensor_distance): # We have to go down
print("Recovering hover position - going down")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif self.beh_type == 'HOVER' and (
self.down_sensor_distance <=
self.hover_sensor_altitude_min): # We have to go up
print("Recovering hover position - going up")
self.beh_type = 'HOVER'
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
print("time of hovering has ended")
self.beh_type = "LANDING"
self.landing_rec()
def secure_landing_phase_rec(self): # Secure landing part - last cm
self.beh_type = "LANDING"
recursions = self.calculate_recursions(self.Secure_time_landing)
thrust = self.hover_thrust
for i in range(recursions):
if thrust <= 0:
break
elif i < 200:
print("Secure hover before landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif i < 700:
print("Smooth landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.hover_thrust - self.Deaccumulating_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
else:
print("Landing")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
self.disarm()
def main1():
global setpoint
# thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10)
# thrust = Thrust()
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10)
point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10)
attitude = AttitudeTarget()
attitude.type_mask = 3
attitude.header = msg.header
# attitude.header.frame_id = 'FRAME_LOCAL_NED'
# quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0)
# attitude.orientation.x = quaternion[0]
# attitude.orientation.y = quaternion[1]
# attitude.orientation.z = quaternion[2]
# attitude.orientation.w = quaternion[3]
attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0))
attitude.body_rate.z = msg.yaw_rate
t = msg.thrust.z/100
if t>1:
t=1
elif t<-1:
t=-1
attitude.thrust = (t+1)/2
attitude_pub.publish(attitude)
point_pub.publish(setpoint)
def imu_cb(self, data):
q = [
data.orientation.x, data.orientation.y, data.orientation.z,
data.orientation.w
]
if self.imu_set:
q = self.imu_q
euler = euler_from_quaternion(q)
q_step = quaternion_from_euler(0.0, 0.0, np.deg2rad(self.step_size))
new_q = quaternion_multiply(q, q_step)
new_euler = euler_from_quaternion(new_q)
self.imu_set = True
self.imu_q = q
print "Current Yaw: " + str(np.rad2deg(euler[2]))
print "New Yaw: " + str(np.rad2deg(new_euler[2]))
msg = AttitudeTarget()
msg.header.stamp = rospy.Time.now()
msg.thrust = self.thrust
msg.orientation.x = new_q[0]
msg.orientation.y = new_q[1]
msg.orientation.z = new_q[2]
msg.orientation.w = new_q[3]
self.setpoint_pub.publish(msg)
def callback(data):
global pub, trim_x, trim_y, trim_z
cmd_ = AttitudeTarget()
cmd_.body_rate.x = valmap(-data.axes[0], -1, 1, -100, 100) + trim_x
cmd_.body_rate.y = valmap(data.axes[1], -1, 1, -100, 100) + trim_y
cmd_.body_rate.z = valmap(-data.axes[2], -1, 1, -200, 200) + trim_z
#cmd_.body_rate.x = valmap(-data.axes[0], -1, 1, -1250, 1250)
#cmd_.body_rate.y = valmap(data.axes[1], -1, 1, -1250, 1250)
#cmd_.body_rate.z = valmap(data.axes[2], -1, 1, -6000, 6000)
cmd_.thrust = valmap(data.axes[3], -1, 1, 0, 700)
if data.buttons[11]:
trim_x += 1
rospy.loginfo("trim_x: " + str(trim_x))
if data.buttons[10]:
trim_x -= 1
rospy.loginfo("trim_x: " + str(trim_x))
if data.buttons[9]:
trim_y += 1
rospy.loginfo("trim_y: " + str(trim_y))
if data.buttons[8]:
trim_y -= 1
rospy.loginfo("trim_y: " + str(trim_y))
if data.buttons[7]:
trim_z += 1
rospy.loginfo("trim_z: " + str(trim_z))
if data.buttons[6]:
trim_z -= 1
rospy.loginfo("trim_z: " + str(trim_z))
pub.publish(cmd_)
def callback(self, rc):
#rospy.loginfo('{} {} {} {}'.format(rc.channels[0], rc.channels[1], rc.channels[2], rc.channels[3]))
cmd = AttitudeTarget()
Ts = (rospy.Time.now() - self.previous_time).to_sec()
self.previous_time = rospy.Time.now()
cmd.header.stamp = rospy.Time.now()
cmd.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE
#ROS use ENU convention
roll = .6 * (rc.channels[1] - 1492.) / 500.
pitch = -.6 * (rc.channels[2] - 1493.) / 500.
yawrate = -3. * (rc.channels[3] - 1498.) / 500.
self.yaw = self.yaw + yawrate * Ts
thrust = (rc.channels[0] - 1000.) / 1200.
rospy.loginfo(
'Ts = {Ts:.3f} r = {r:.2f} p = {p:.2f} y = {y:.2f} t = {t:.2f}'.
format(Ts=Ts, r=roll, p=pitch, y=self.yaw, t=thrust))
q = quaternion_from_euler(roll, pitch, self.yaw)
cmd.orientation.x = q[0]
cmd.orientation.y = q[1]
cmd.orientation.z = q[2]
cmd.orientation.w = q[3]
cmd.body_rate.x = 0.
cmd.body_rate.y = 0.
cmd.body_rate.z = yawrate
cmd.thrust = thrust
self.pub.publish(cmd)
def set_ang_vel_thrust(self, ang_vel, thrust, freq):
"""
Offboard method that sends angular velocity and thrust references to the PX4 autopilot of the the drone.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through the mathematical expression of the thrust curve of the vehicle.
Parameters
----------
ang_vel : np.array of floats with shape (3,1)
Desired angular velocity for the drone.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.type_mask = 128
msg.body_rate.x, msg.body_rate.y, msg.body_rate.z = ned_to_enu(ang_vel)
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def track_and_trajectory_dock_control(self,des_trajectory_point, curr_odom, yaw_des, time_now, docker):
if docker == self.num:
pass
else:
return
Rot_des = np.matrix(np.zeros((4, 4)))
Rot_des[3, 3] = 1
attitude_des = AttitudeTarget()
thrust = Thrust()
des_acc = Vector3()
if self.A_init_flag:
thrust.header = curr_odom.header
attitude_des.header = curr_odom.header
dt = time_now - self.time_prev
ex = des_trajectory_point.position.x - curr_odom.position.x
ey = des_trajectory_point.position.y - curr_odom.position.y
ez = des_trajectory_point.position.z - curr_odom.position.z
evx = des_trajectory_point.velocity.x - curr_odom.velocity.x
evy = des_trajectory_point.velocity.y - curr_odom.velocity.y
evz = des_trajectory_point.velocity.z - curr_odom.velocity.z
self.intx = self.intx + ex * dt
self.inty = self.inty + ey * dt
self.intz = self.intz + ez * dt
des_acc.x = des_trajectory_point.acceleration.x + self.Kp_track.x * ex + self.Kd_track.x * evx + self.Ki_track.x * self.intx
des_acc.y = des_trajectory_point.acceleration.y + self.Kp_track.y * ey + self.Kd_track.y * evy + self.Ki_track.y * self.inty
des_acc.z = des_trajectory_point.acceleration.z + self.Kp_track.z * ez + self.Kd_track.z * evz + self.Ki_track.z * self.intz
R_waitmod_w = np.matrix([[np.cos(self.tag_angle),-np.sin(self.tag_angle),0],[np.sin(self.tag_angle),np.cos(self.tag_angle),0],[0,0,1]]).transpose()
curr_quaternion = [curr_odom.orientation.x, curr_odom.orientation.y,
curr_odom.orientation.z, curr_odom.orientation.w]
H_curr = tf.transformations.quaternion_matrix(curr_quaternion)
Rot_curr = np.matrix(H_curr[:3, :3])
des_acc_relative = R_waitmod_w*np.matrix([[des_acc.x],
[des_acc.y],
[des_acc.z]])
Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * des_acc_relative
Force_des_body = Rot_curr * Force_des
thrust.thrust = Force_des_body[2]
Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des))
x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]])
y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0)
Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world))
Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0))
quad_des = tf.transformations.quaternion_from_matrix(Rot_des)
attitude_des.orientation.x = quad_des[0]
attitude_des.orientation.y = quad_des[1]
attitude_des.orientation.z = quad_des[2]
attitude_des.orientation.w = quad_des[3]
attitude_des.thrust = thrust.thrust
self.mavros_attitude_pub.publish(attitude_des)
self.mavros_thrust_pub.publish(thrust)
self.time_prev = time_now
def construct_target_attitude(self,
body_x=0,
body_y=0,
body_z=0,
thrust=0,
roll_angle=0,
pitch_angle=0,
yaw_angle=0):
target_raw_attitude = AttitudeTarget(
) # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_attitude.header.stamp = rospy.Time.now()
#target_raw_attitude.orientation = self.imu.orientation
q = self.to_quaternion(
roll_angle + self.angle_roll_left_right_trim,
pitch_angle + self.angle_pitch_forward_back_trim, yaw_angle)
#q = self.to_quaternion(roll_angle, pitch_angle, yaw_angle)
target_raw_attitude.orientation.w = q[0]
target_raw_attitude.orientation.x = q[1]
target_raw_attitude.orientation.y = q[2]
target_raw_attitude.orientation.z = q[3]
target_raw_attitude.body_rate.x = body_x # ROLL_RATE
target_raw_attitude.body_rate.y = body_y # PITCH_RATE
target_raw_attitude.body_rate.z = body_z # YAW_RATE
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages)
def set_att_thrust(self, att, att_type, thrust, freq):
"""
Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle.
Parameters
----------
att : np.array of floats with shape (3,1) or with shape (4,1)
Desired attitude for the vehicle, expressed in euler angles or in a quaternion.
att_type : str
Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
if att_type == 'euler':
msg.orientation = Quaternion(*quaternion_from_euler(
*ned_to_enu(att)))
elif att_type == 'quaternion':
msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att))
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def update_attitude(self):
# Create message
msg = AttitudeTarget(header=Header(stamp=rospy.Time.now()))
# Ignore all but thrust
msg.type_mask = AttitudeTarget.IGNORE_ROLL_RATE | AttitudeTarget.IGNORE_PITCH_RATE | AttitudeTarget.IGNORE_YAW_RATE | AttitudeTarget.IGNORE_ATTITUDE
# # Set attitude
# msg.orientation.x = 0
# msg.orientation.y = 0
# msg.orientation.z = 0
# msg.orientation.w = 0
# # Set rates
# msg.body_rate.x = 0.1
# msg.body_rate.y = 0.1
# msg.body_rate.z = 0.1
# Set velocity
msg.thrust = self._thrust
# Navigate until location is reached
while not self._done.is_set() and not rospy.is_shutdown():
# Publish
self._rawatt_pub.publish(msg)
# Ensure proper communication rate
self._rate.sleep()
def att_controller(self, r=0, p=0, y=0, z=2):
if hasattr(self, "roll_zero"):
r += self.roll_zero
if hasattr(self, "pitch_zero"):
p + self.pitch_zero
att = AttitudeTarget()
q = tf.transformations.quaternion_from_euler(r, p, y)
att.orientation.x = q[0]
att.orientation.y = q[1]
att.orientation.z = q[2]
att.orientation.w = q[3]
rx, ry, rz, r, p, y = self.parse_local_position(mode="e")
vx, vy, vz, wx, wy, wz = self.parse_velocity()
try:
param = 1.0 / (np.cos(r) * np.cos(p))
except Exception as e:
param = 1
finally:
vzt = self.pidz.step(z - rz)
att.thrust = (self.pidzv.step(vzt - vz) + 0.57) * param
# for plot only..
self.thrust = att.thrust
self.pidzv_out = self.pidzv.out
self.pidz_out = self.pidz.out
att.header = Header()
att.header.frame_id = "map"
att.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(att)
def test_control(self):
self.toggle_arm(True)
#self.takeoff(1.0)
self.set_offboard_mode()
self.move_to(0,0,5)
#sleep(2)
'''
self.move_to(-2,2,3.3)
sleep(2)
self.move_to(0,4,3.3)
sleep(2)
self.move_to(2,2,3.3)
sleep(2)'''
############
print('Starting')
sleep(5)
rate = rospy.Rate(20)
sr = AttitudeTarget()
sr.type_mask = 135
sr.thrust = 0.9
for i in range(20):
print('stg 1')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 1 done')
sr.type_mask = 134
sr.body_rate.x = 1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(20):
print('stg 2')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 2 done')
sr.type_mask = 134
sr.body_rate.x = -1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(5):
print('final')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Roll Complete!!')
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust=0):
target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = body_x # ROLL_RATE
target_raw_attitude.body_rate.y = body_y # PITCH_RATE
target_raw_attitude.body_rate.z = body_z # YAW_RATE
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
def set_attitude(self, r, p, y, z, mode="height"):
att = AttitudeTarget()
q = tf.transformations.quaternion_from_euler(r, p, y)
att.orientation.x = q[0]
att.orientation.y = q[1]
att.orientation.z = q[2]
att.orientation.w = q[3]
rx, ry, rz, qx, qy, qz, qw = self.get_uav_pos()
if mode == "height":
try:
rpy = tf.transformations.euler_from_quaternion(
(qx, qy, qz, qw))
param = 1.0 / (np.cos(rpy[0]) * np.cos(rpy[1]))
except:
param = 1
finally:
att.thrust = self.pidz.step(z - rz) * param
else:
att.thrust = z
self.att_setpoint_pub.publish(att)
def thrust_recursion(self, thrust):
if self.status_thrust == "up":
if (thrust >= 1):
print ("the thrust has reached its desired point")
print ("the motors should start slowing down...")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = 1
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
self.status_thrust = "slow down"
return self.thrust_recursion(target_raw_attitude.thrust)
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust + 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
elif self.status_thrust == "slow down":
if (thrust <= 0):
print ("the motors should stop")
self.status_thrust = "stop"
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust - 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
def landing_rec(self, thrust, beh_type, time_flying):
if (time_flying <= 0 or self.down_sensor_distance <=
self.landing_sensor_altitude_min) and beh_type == "LANDING":
print("Entering secure landing")
return self.secure_landing_phase_rec(thrust, beh_type,
self.Secure_time_landing)
elif thrust > self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.landing_rec(
thrust, beh_type,
time_flying) #bublishing a constant parameter "not
elif thrust <= self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.landing_rec(
thrust, beh_type, time_flying
) #bublishing a constant parameter "not updating thrust argument"
def send_commands(self, event):
# Create the AttitudeTarget command message.
command_msg = AttitudeTarget()
# Fill out the message.
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.ignore_mask
command_msg.thrust = self.thrust_val
# Publish the message.
self.command_pub.publish(command_msg)
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust = 0.2):
target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_attitude.header.stamp = rospy.Time.now()
#target_raw_attitude.orientation. = self.imu.orientation
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
#target_raw_attitude.body_rate.x = body_x # ROLL_RATE
#target_raw_attitude.body_rate.y = body_y # PITCH_RATE
#target_raw_attitude.body_rate.z = body_z # YAW_RATE
target_raw_attitude.thrust = thrust
return target_raw_attitude
def lift_off_rec(self, thrust, beh_type, time_flying):
if (time_flying <= 0 or self.down_sensor_distance <=
self.hover_sensor_altitude_min) and beh_type == "TAKE OFF":
print("time of lift off has ended")
beh_type = "HOVER"
return self.hover_rec(thrust, beh_type, self.Hover_time)
elif beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min:
print("The drone is lifting off at constant thrust")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.Liftoff_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(
self.Time_between_messages) #was 0.005 (now 50hz ,500loops)
time_flying = time_flying - self.Time_between_messages
return self.lift_off_rec(target_raw_attitude.thrust, beh_type,
time_flying)
elif beh_type == "TAKE OFF":
print("Lifting the drone up slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust + self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.lift_off_rec(target_raw_attitude.thrust, beh_type,
time_flying)
def landing_rec(self): # Landing phase
self.beh_type = "LANDING"
recursions = self.calculate_recursions(self.Max_time_landing)
print(recursions)
thrust = self.hover_thrust
for i in range(recursions):
print self.down_sensor_distance
if self.down_sensor_distance <= self.landing_sensor_altitude_min:
break
elif thrust > self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust - self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif thrust <= self.Landing_thrust: #and beh_type == "HOVER":
print("Landing the drone down slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
self.secure_landing_phase_rec2()
def thrust_recursion(self, thrust):
if (thrust >= 1):
print("the thrust has reached its desired point")
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust + 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
def get_message(sp_att):
msg = AttitudeTarget()
msg.header.stamp = rospy.get_rostime()
msg.type_mask = AT.IGNORE_ROLL_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_YAW_RATE
q = quaternion_from_euler(sp_att[0], sp_att[1], sp_att[2])
msg.orientation.x = q[0]
msg.orientation.y = q[1]
msg.orientation.z = q[2]
msg.orientation.w = q[3]
msg.thrust = sp_att[3]
return msg
def send_attitude_command(self):
command_msg = AttitudeTarget()
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.attitude_mask
command_msg.body_rate.x = 0.0
command_msg.body_rate.y = 0.0
command_msg.body_rate.z = 0.0
command_msg.thrust = self.thrust_val
# Publish.
self.attitude_pub_mavros.publish(command_msg)
def pos_control(self,des_trajectory_point,curr_pose, curr_vel, yaw_des, time_now):
Rot_des = np.matrix(np.zeros((4, 4)))
Rot_des[3, 3] = 1
attitude_des = AttitudeTarget()
thrust = Thrust()
des_acc = Vector3()
if self.A_init_flag:
thrust.header = curr_pose.header
attitude_des.header = curr_pose.header
dt = time_now - self.time_prev
ex = des_trajectory_point.position.x - curr_pose.pose.position.x
ey = des_trajectory_point.position.y - curr_pose.pose.position.y
ez = des_trajectory_point.position.z - curr_pose.pose.position.z
evx = des_trajectory_point.velocity.x - curr_vel.twist.linear.x
evy = des_trajectory_point.velocity.y - curr_vel.twist.linear.y
evz = des_trajectory_point.velocity.z - curr_vel.twist.linear.z
self.intx = self.intx + ex * dt
self.inty = self.inty + ey * dt
self.intz = self.intz + ez * dt
des_acc.x = des_trajectory_point.acceleration.x + self.Kp.x * ex + self.Kd.x * evx + self.ki.x * self.intx
des_acc.y = des_trajectory_point.acceleration.y + self.Kp.y * ey + self.Kd.y * evy + self.ki.y * self.inty
des_acc.z = des_trajectory_point.acceleration.z + self.Kp.z * ez + self.Kd.z * evz + self.ki.z * self.intz
curr_quaternion = [curr_pose.pose.orientation.x, curr_pose.pose.orientation.y,
curr_pose.pose.orientation.z, curr_pose.pose.orientation.w]
H_curr = tf.transformations.quaternion_matrix(curr_quaternion)
Rot_curr = np.matrix(H_curr[:3, :3])
Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]])
Force_des_body = Rot_curr * Force_des
thrust.thrust = Force_des_body[2]
Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des))
x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]])
y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0)
Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world))
Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0))
quad_des = tf.transformations.quaternion_from_matrix(Rot_des)
attitude_des.orientation.x = quad_des[0]
attitude_des.orientation.y = quad_des[1]
attitude_des.orientation.z = quad_des[2]
attitude_des.orientation.w = quad_des[3]
attitude_des.thrust = thrust.thrust
self.mavros_attitude_pub.publish(attitude_des)
self.mavros_thrust_pub.publish(thrust)
self.time_prev = time_now
def lift_off_rec(self, thrust, time_flying):
recursions = self.calculate_recursions(time_flying)
print(recursions)
self.beh_type = "TAKE OFF"
for i in range(recursions):
print(i)
if self.beh_type == "TAKE OFF" and thrust < self.Liftoff_thrust: #and beh_type == "HOVER":
print("Lifting the drone up slowly")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = thrust
thrust = thrust + self.accumulating_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
elif self.beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min:
print("The drone is lifting off at constant thrust")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
thrust = self.Liftoff_thrust
target_raw_attitude.thrust = thrust
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(self.Time_between_messages
) #was 0.005 (now 50hz ,500loops)
else:
break
self.beh_type = 'HOVER'
print("time of lifting off has ended")
self.hover_rec(self.hover_time)
def send_commands(self, event):
command_msg = AttitudeTarget()
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.ignore_mask
command_msg.body_rate.x = 0.0
command_msg.body_rate.y = 0.0
command_msg.body_rate.z = 0.0
command_msg.thrust = self.thrust_val
print self.thrust_val
print "here"
# Publish.
self.land_pub.publish(command_msg)
def publishControlInputs(self):
hcc = AttitudeControlExt()
hcc.header.stamp = rospy.Time.now()
hcc.thrust = self.desiredThrust
#gain = 100.0 *0.5
hcc.roll = self.taus[0]
hcc.pitch = -self.taus[1]
hcc.yaw = -self.taus[2]
#####################################################
target = AttitudeTarget()
target.header.stamp=rospy.Time.now()
target.body_rate.x = self.taus[0]
target.body_rate.y = -self.taus[1]
target.body_rate.z = -self.taus[2]
target.thrust = self.desiredThrust
#########################################################
output = HippocampusOutput_2()
output.frame_stamp = rospy.Time.now()
output.des_rates.x = self.desired_rates[0]
output.des_rates.y = self.desired_rates[1]
output.des_rates.z = self.desired_rates[2]
output.current_rates.x = self.body_rate[0]
output.current_rates.y = self.body_rate[1]
output.current_rates.z = self.body_rate[2]
outputx = HippocampusOutput()
outputx.frame_stamp = rospy.Time.now()
outputx.current_orientation.x = self.current_axis[0]
outputx.current_orientation.y = self.current_axis[1]
outputx.current_orientation.z = self.current_axis[2]
outputx.des_orientation.x = self.desired_axis[0]
outputx.des_orientation.y = self.desired_axis[1]
outputx.des_orientation.z = self.desired_axis[2]
outputx.current_velocity.x = self.desired_rates[0]
outputx.current_velocity.y = self.desired_rates[1]
outputx.current_velocity.z = self.desired_rates[2]
# self.output_pub.publish(outputx)
# self.output_2_pub.publish(output)
# self.control_pub.publish(hcc)
self.control_pub3.publish(target)
def body_recursion(self, thrust):
if self.status_thrust == "up":
if (thrust <= -1):
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = 0.5
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = thrust - 0.003 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.body_recursion(target_raw_attitude.body_rate.y)
def secure_landing_phase_rec(self, thrust, time_flying):
if time_flying <= 0:
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.orientation = self.imu.orientation
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = 0 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
target_raw_attitude.thrust = self.Landing_thrust
thrust = self.Landing_thrust
self.attitude_target_pub.publish(target_raw_attitude)
time_flying = time_flying - self.Time_between_messages
time.sleep(self.Time_between_messages
) # was 0.005 (now 50hz ,500 loops ,5sec)
return self.secure_landing_phase_rec(
thrust, beh_type, time_flying
) #bublishing a constant parameter "not updating thrust argument"
