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 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 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 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 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 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 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 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 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 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 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 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 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 _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 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"
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 __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 mocapcb(pose, twist):
## Initialise Attitude Target message, the control input to be sent to host_service.py
u = AttitudeTarget()
# ## Get orientation from mocap data and convert to Euler angles
# orientation_q = pose.pose.orientation
# orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
# (roll, pitch, yaw) = euler_from_quaternion(orientation_list)
# rospy.loginfo('%f' % roll + ' and %f' % pitch)
## Calculate the velocity command from integral backstepping: https://doi.org/10.3929/ethz-a-010039365
chi6 = 0 ## IMPLEMENT INTEGRATION
chi7 = 0 ## IMPLEMENT INTEGRATION
e10 = xref - twist.twist.linear.x
e11 = yref - twist.twist.linear.y
u.thrust = zref
u.orientation.x = (alt_gains.m / u.thrust) * ((
(alt_gains.c12 + alt_gains.c13) * e10) + (alt_gains.lam6 * chi6))
u.orientation.y = (alt_gains.m / u.thrust) * ((
(alt_gains.c14 + alt_gains.c15) * e11) + (alt_gains.lam7 * chi7))
rospy.loginfo('Calculated z %f, phi %f and theta %f', u.thrust,
u.orientation.x, u.orientation.y)
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 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 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 __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 __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)
