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 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 setUp(self):
self.local_position = PoseStamped()
self.state = State()
self.att = AttitudeTarget()
self.sub_topics_ready = {
key: False
for key in ['local_pos', 'home_pos', 'state']
}
# setup ROS topics and services
try:
rospy.wait_for_service('mavros/cmd/arming', 30)
rospy.wait_for_service('mavros/set_mode', 30)
except rospy.ROSException:
self.fail("failed to connect to mavros services")
self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped,
self.local_position_callback)
self.home_pos_sub = rospy.Subscriber('mavros/home_position/home',
HomePosition,
self.home_position_callback)
self.state_sub = rospy.Subscriber('mavros/state', State,
self.state_callback)
self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
# send setpoints in seperate thread to better prevent failsafe
self.att_thread = Thread(target=self.send_att, args=())
self.att_thread.daemon = True
self.att_thread.start()
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 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_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 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 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 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 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 _send_command(self, data):
#rospy.loginfo(data)
self.local_pose = data
# debug
quaternion = (data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
#print 'roll is %5.2f deg, and pitch is %5.2f deg, and yaw is %5.2f deg' % (euler[0]*180/3.1425, euler[1]*180/3.1425, euler[2]*180/3.1425)
[self.cpitch,self.croll,self.cyaw,self.cheight] = \
self.controller.genQUADcontrol(self.local_pose.pose,self.waypoint.pose,self.velocity)
#print 'y command %5.2f, x command %5.2f, yaw command %5.2f, thrust %5.2f' % (self.croll, self.cpitch, self.cyaw, self.cheight + self.hoverth)
self.AttitudeTarget = AttitudeTarget()
self.AttitudeTarget.orientation = Quaternion(
*quaternion_from_euler(self.croll, self.cpitch, self.cyaw))
self.AttitudeTarget.thrust = self.cheight + self.hoverth
if inside_boundary(self.local_pose.pose, self.boundary):
result_mode = self.change_mode(0, "OFFBOARD")
self.pub_sp.publish(self.AttitudeTarget)
else:
result_mode = self.change_mode(0, "POSCTL")
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 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 __init__(self):
self.rate = rospy.Rate(4)
self.connected = False
self.took_off = False
self.init_control = False
self.m = 30
self.c12 = 1
self.c13 = 1
self.c14 = 1
self.c15 = 1
self.c16 = 0.0
self.c17 = 0.0
# self.c16 = 0.015
# self.c17 = 0.015
self.lam6 = 0.015
self.lam7 = 0.015
self.ffactor = 1
self.kp = 50
self.agents = {}
self.network = two_agents
self.agent_names = [
rospy.get_param(s) for s in rospy.get_param_names()
if "mambo_request/name" in s
]
rospy.loginfo(self.agent_names)
for s in self.agent_names:
## Dictionary of each agent's state information. The two zeros are the integration of velocity errors.
self.agents[s] = [
PoseStamped(),
TwistStamped(),
TwistStamped(),
AttitudeTarget(), (0.0, 0.0, 0.0), 0.0, 0.0
]
## Publishers and subscribers for each agent.
self.pub_disconnect = rospy.Publisher('/mambo_srv/' + s +
'/disconnect',
String,
queue_size=10)
self.pub_fly_command = rospy.Publisher('/mambo_srv/' + s +
'/fly_command',
AttitudeTarget,
queue_size=10)
self.sub_pose = rospy.Subscriber(
'/vrpn_client_node/' + s + '/pose', PoseStamped,
self.callback_pose, s)
self.sub_twist = rospy.Subscriber(
'/vrpn_client_node/' + s + '/twist', TwistStamped,
self.callback_twist, s)
self.sub_accel = rospy.Subscriber(
'/vrpn_client_node/' + s + '/accel', TwistStamped,
self.callback_accel, s)
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 setUp(self):
super(MavrosOffboardAttctlTest, self).setUp()
self.att = AttitudeTarget()
self.att_setpoint_pub = rospy.Publisher(
'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# send setpoints in seperate thread to better prevent failsafe
self.att_thread = Thread(target=self.send_att, args=())
self.att_thread.daemon = True
self.att_thread.start()
def __init__(self):
self.rate = rospy.Rate(4)
self.connected = False
self.took_off = False
self.init_control = False
self.m = 30
self.c12 = 1
self.c13 = 1
self.c14 = 1
self.c15 = 1
self.c16 = 0.015
self.c17 = 0.015
self.lam6 = 0.015
self.lam7 = 0.015
self.chi6 = 0
self.chi7 = 0
self.ffactor = 1
self.kp = 50
self.pose = PoseStamped()
self.twist = TwistStamped()
self.accel = TwistStamped()
self.u = AttitudeTarget()
self.xdref = 0
self.ydref = 0
self.zdref = 0
self.agent_names = [
rospy.get_param(s) for s in rospy.get_param_names()
if "mambo_request/name" in s
]
rospy.loginfo(self.agent_names)
for s in self.agent_names:
## Publishers and subscribers for each agent.
self.pub_disconnect = rospy.Publisher('/mambo_srv/' + s +
'/disconnect',
String,
queue_size=10)
self.pub_fly_command = rospy.Publisher('/mambo_srv/' + s +
'/fly_command',
AttitudeTarget,
queue_size=10)
self.sub_pose = rospy.Subscriber(
'/vrpn_client_node/' + s + '/pose', PoseStamped,
self.callback_pose)
self.sub_twist = rospy.Subscriber(
'/vrpn_client_node/' + s + '/twist', TwistStamped,
self.callback_twist)
self.sub_accel = rospy.Subscriber(
'/vrpn_client_node/' + s + '/accel', TwistStamped,
self.callback_accel)
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 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 __init__(self, id=""):
self.current_state = State()
self.uav_id = id
self.targetattitude = AttitudeTarget()
self.targetattitude.orientation.x = 0
self.targetattitude.orientation.y = 0
self.targetattitude.orientation.z = 0
self.targetattitude.orientation.w = 1
self.targetattitude.thrust = 0.1
self.position = PoseStamped()
self.velocity = TwistStamped()
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 __init__(self):
rospy.init_node('pad', anonymous=True)
self.threshold = .1 #because the controler is a little sticky
#self.offset=rospy.get_param('~offset')
self.axes_map = {
'roll': 3,
'pitch': 4,
'yaw': 0,
'throttle': 1,
'mode_set1': 6,
'mode_set2': 7
}
self.velocity_scale = {'angular': 1.5, 'linear': 2.0}
self.button_map = {
'arm': 0, #a
'disarm': 1, #b
'takeoff': 2, #x
'land': 3, #y
'enable': 4 #rb
}
self.mode = "POS" # three modes: POS, VEL, ATD
self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_cb)
#state info
self.pose = Pose()
self.agentPose_sub = rospy.Subscriber('mavros/local_position/pose',
PoseStamped, self.poseCB)
#pose control
self.cmdPose = Pose()
self.cmdPose.orientation.w = -1
self.cmdPose_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
#atd control
self.cmdAtd = AttitudeTarget()
self.cmdAtd_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
#velocity control
self.cmdVel = Twist()
self.cmdVel_pub = self.agentVel_cmd = rospy.Publisher(
'mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)
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 __init__(self,
quad_model,
quad_constraints,
t_horizon,
n_nodes,
sim_required=False,
max_hight=1.0):
self.model = quad_model
self.constraints = quad_constraints
self.g = 9.8066
self.T = t_horizon
self.N = n_nodes
self.simulation_required = sim_required
robot_name_ = rospy.get_param("~robot_name", "bebop1_r")
self.current_pose = None
self.current_state = np.zeros((13, 1))
self.dt = 0.02
self.rate = rospy.Rate(1 / self.dt)
self.time_stamp = None
self.trajectory_path = None
self.current_twist = np.zeros(3)
self.att_command = AttitudeTarget()
self.att_command.type_mask = 3
self.pendulum_state = np.zeros(4)
# subscribers
# the robot state
robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry,
self.robot_state_callback)
# pendulum state
pendulum_state_sub_ = rospy.Subscriber('/pendulum_pose', Odometry,
self.pendulum_state_callback)
# trajectory
robot_trajectory_sub_ = rospy.Subscriber(
'/robot_trajectory', itm_trajectory_msg,
self.trajectory_command_callback)
# publisher
self.att_setpoint_pub = rospy.Publisher(
'/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
# create a server
server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server)
# setup optimizer
self.quadrotor_optimizer_setup()
# # It seems that thread cannot ensure the performance of the time
self.att_thread = Thread(target=self.send_command, args=())
self.att_thread.daemon = True
self.att_thread.start()
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 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
