def __init__(self):
rospy.init_node('examplePilot')
self.subState = 'GPS'
self.GPSMode = True
self.enable = False
self.rate = rospy.Rate(20)
self.exampleGPSPub = rospy.Publisher(targetWP_GPS,
mavSP.PoseStamped,
queue_size=5)
self.exampleATTIPub = rospy.Publisher(targetWP_Atti,
mavros_msgs.msg.AttitudeTarget,
queue_size=5)
self.pilotSubstatePub = rospy.Publisher(onB_substateSub,
String,
queue_size=1)
rospy.Subscriber(onB_StateSub, String, self.onStateChange)
rospy.Subscriber(
mavros.get_topic('local_position',
'pose'), mavSP.PoseStamped, self.onPositionChange
) # uses mavros.get topic to find the name of the current XYZ position of the drone
self.currUAVPos = mavSP.PoseStamped() # used for GPS setpoints
self.msgSPAtti = mavSP.TwistStamped() # used for Attitude setpoints
rospy.loginfo('examplePilot Initialised.')
def control_loop(self):
rate = rospy.Rate(self.rate) # 10hz
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
#while not self.task_done:
while not rospy.is_shutdown():
if not self.task_done:
self.pidx.update(self.x)
self.pidy.update(self.y)
self.pidz.update(self.z)
msg.twist.linear.x = self.pidx.output
msg.twist.linear.y = self.pidy.output
msg.twist.linear.z = self.pidz.output
#print self.name + " is publishing"
self.pub.publish(msg)
rate.sleep()
def control_pid(self):
msg = SP.TwistStamped(
header=SP.Header(
frame_id="Drone_Vel_setpoint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
#todo controle PID
self.error.x.act = self.x - DronePose.x
self.error.y.act = self.y - DronePose.y
self.error.z.act = self.z - DronePose.z
PX = self.error.x.act * KPx
PY = self.error.y.act * KPy
PZ = self.error.z.act * KPz
_time_bet_run = (rate.sleep_dur.nsecs / 1000000000.0)
#rospy.loginfo((rate.sleep_dur.nsecs / 1000000000.0))
DX = ((self.error.x.Deri) * float(KDx)) / _time_bet_run
DY = ((self.error.y.Deri) * float(KDy)) / _time_bet_run
DZ = ((self.error.z.Deri) * float(KDz)) / _time_bet_run
self.error.x.Intg = self.error.x.act * KIx * _time_bet_run
IX = self.error.x.Intg
self.error.y.Intg = self.error.y.act * KIy * _time_bet_run
IY = self.error.y.Intg
self.error.z.Intg = self.error.z.act * KIz * _time_bet_run
IZ = self.error.z.Intg
# rospy.loginfo((rate.sleep_dur.nsecs / 1000000000.0))
#rospy.loginfo(PX, DX, IX, "\n", PY, DY, IY, "\n", PZ, DZ, IZ)
#rospy.loginfo(PX, DX, IX)
rospy.loginfo(IY)
self.x_vel = PX + DX + IX
self.y_vel = PY + DY + IY
self.z_vel = PZ + DZ + IZ
msg.twist.linear.x = self.x_vel
msg.twist.linear.y = self.y_vel
msg.twist.linear.z = self.z_vel
# msg.twist.angular = SP.TwistStamped.twist.angular()
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
rate.sleep()
def control_loop(self):
rate = rospy.Rate(self.rate) # 10hz
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
#while not self.task_done:
while not rospy.is_shutdown():
if not self.task_done:
self.pidx.update(self.x)
self.pidy.update(self.y)
self.pidz.update(self.z)
err_x = self.x - self.x_ref
err_y = self.y - self.y_ref
err_z = self.z - self.z_ref
if self.is_near('X', err_x, 0.0, self.wp_radius) and \
self.is_near('Y', err_y, 0.0, self.wp_radius) and \
self.is_near('Z', err_z, 0.0, self.wp_radius):
msg.twist.linear.x = self.pidx.output
msg.twist.linear.y = self.pidy.output
msg.twist.linear.z = self.pidz.output
else:
alpha = atan2(-err_y, -err_x)
beta = atan2(-err_z, sqrt(pow(err_x, 2) + pow(err_y, 2)))
msg.twist.linear.x = self.v0 * cos(beta) * cos(alpha)
msg.twist.linear.y = self.v0 * cos(beta) * sin(alpha)
msg.twist.linear.z = self.v0 * sin(beta)
#print "alpha = %0.2f beta = %0.2f errx = %0.2f erry = %0.2f errz = %0.2f" % (alpha*180.0/pi, beta*180.0/pi, err_x, err_y, err_z)
#print self.name + " is publishing"
self.pub.publish(msg)
rate.sleep()
def navigate(self):
#att_pub = SP.get_pub_attitude_pose(queue_size=10)
#thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
#thd_pub.publish(data=self.throttle_update)
#thd_pub.publish(data=0.6)
rate = rospy.Rate(10) # 10hz
magnitude = 1 # in meters/sec
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
i = 0
while not rospy.is_shutdown():
#print "publishing velocity"
self.throttle_update = self.pid_throttle.update(self.current_alt)
pid_offset = self.pid_alt.update(self.current_alt)
if self.use_pid:
msg.twist.linear = geometry_msgs.msg.Vector3(
self.vx * magnitude, self.vy * magnitude,
self.vz * magnitude + pid_offset)
else:
msg.twist.linear = geometry_msgs.msg.Vector3(
self.vx * magnitude, self.vy * magnitude,
self.vz * magnitude)
#print msg.twist.linear
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.yaw # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
if self.velocity_publish:
#msg.twist.angular.z = 0.5
self.pub_vel.publish(msg)
#print "PID: ", pid_offset
rate.sleep()
i += 1
def navigate(self):
rate = rospy.Rate(10) # 10hz
magnitude = 1 # in meters/sec
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
i = 0
while i < 120:
msg.twist.linear = geometry_msgs.msg.Vector3(
self.vx * magnitude, self.vy * magnitude, self.vz * magnitude)
print msg.twist.linear
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
self.pub.publish(msg)
rate.sleep()
i += 1
def publish_angular_linear_orientation(self):
#att_pub = SP.get_pub_attitude_pose(queue_size=10)
#thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
vel_pub = SP.get_pub_velocity_cmd_vel(queue_size=10)
rate = rospy.Rate(10) # 10hz
## msg = SP.PoseStamped(
## header=SP.Header(
## frame_id="base_footprint", # no matter, plugin don't use TF
## stamp=rospy.Time.now()), # stamp should update
## )
while not rospy.is_shutdown():
#msg.pose.position.x = self.x
#msg.pose.position.y = self.y
#msg.pose.position.z = self.z
twist = SP.TwistStamped(header=SP.Header(
stamp=rospy.get_rostime()))
twist.twist.linear = SP.Vector3(x=0, y=0, z=0)
twist.twist.angular = SP.Vector3(z=-0.4)
print "publishing orientation"
vel_pub.publish(twist)
def navigate(self):
rate = rospy.Rate(30) # 30hz
magnitude = 0.75 # in meters/sec
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # doesn't matter
stamp=rospy.Time.now()), # stamp should update
)
i = 0
self.home_lat = self.cur_pos_x
self.home_lon = self.cur_pos_y
self.home_alt = self.cur_alt
while not rospy.is_shutdown():
if self.click == "ABORT":
self.disarm()
break
if not self.override_nav: # heavy stuff right about here
vector_base = self.final_pos_x - self.cur_pos_x
vector_height = self.final_pos_y - self.cur_pos_y
try:
slope = vector_base / (vector_height + 0.000001)
p_slope = -vector_height / (vector_base + 0.000001)
except:
pass
copter_rad = self.cur_rad
vector_rad = atan(slope)
if self.final_pos_y < self.cur_pos_y:
vector_rad = vector_rad - pi
glob_vx = sin(vector_rad)
glob_vy = cos(vector_rad)
beta = ((vector_rad - copter_rad) *
(180.0 / pi) + 360.0 * 100.0) % (360.0)
beta = (beta + 90.0) / (180.0 / pi)
cx = self.cur_pos_x
cy = self.cur_pos_y
fx = self.final_pos_x
fy = self.final_pos_y
b_c = cy - cx * p_slope
b_f = fy - fx * p_slope
sign_dist = b_f - b_c
if self.last_sign_dist < 0.0 and sign_dist > 0.0:
self.reached = True
if self.last_sign_dist > 0.0 and sign_dist < 0.0:
self.reached = True
self.last_sign_dist = sign_dist
if self.reached:
self.last_sign_dist = 0.0
master_scalar = 1.0
master_hype = sqrt((cx - fx)**2.0 + (cy - fy)**2.0)
if master_hype > 1.0:
master_scalar = 1.0
else:
master_scalar = master_hype
self.vx = sin(beta) * master_scalar
self.vy = cos(beta) * master_scalar
if self.alt_control:
if self.vy > VELOCITY_CAP:
self.vy = VELOCITY_CAP
if self.vy < -VELOCITY_CAP:
self.vy = -VELOCITY_CAP
if self.vx > VELOCITY_CAP:
self.vx = VELOCITY_CAP
if self.vx < -VELOCITY_CAP:
self.vx = -VELOCITY_CAP
if abs(self.final_alt - self.cur_alt) < VELOCITY_CAP:
self.vz = (self.final_alt - self.cur_alt)
else:
if self.final_alt > self.cur_alt:
self.vz = VELOCITY_CAP
if self.final_alt < self.cur_alt:
self.vz = -VELOCITY_CAP
msg.twist.linear = geometry_msgs.msg.Vector3(
self.vx * magnitude, self.vy * magnitude,
self.vz * magnitude)
else:
msg.twist.linear = geometry_msgs.msg.Vector3(
self.vx * magnitude, self.vy * magnitude,
self.vz * magnitude)
try:
self.pub_vel.publish(msg)
except rospy.exceptions.ROSException as ex:
print "exception!! %s" % ex
print self.pub_vel
try:
rate.sleep()
except rospy.exceptions.ROSException as ex:
print "exception in rate!! %s" % ex
i += 1
def navigate(self):
rate = rospy.Rate(30) # 30hz
magnitude = 1.0 # in meters/sec
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # doesn't matter
stamp=rospy.Time.now()), # stamp should update
)
i =0
self.home_lat = self.cur_pos_x
self.home_lon = self.cur_pos_y
self.home_alt = self.cur_alt
while not rospy.is_shutdown():
if self.click == "ABORT":
self.disarm()
break
if not self.override_nav: # heavy stuff right about here
vector_base = self.final_pos_x - self.cur_pos_x
vector_height = self.final_pos_y - self.cur_pos_y
try:
slope = vector_base/(vector_height+0.000001)
p_slope = -vector_height/(vector_base+0.000001)
except:
print "This should never happen..."
copter_rad = self.cur_rad
vector_rad = atan(slope)
if self.final_pos_y < self.cur_pos_y:
vector_rad = vector_rad - pi
glob_vx = sin(vector_rad)
glob_vy = cos(vector_rad)
beta = ((vector_rad-copter_rad) * (180.0/pi) + 360.0*100.0) % (360.0)
beta = (beta + 90.0) / (180.0/pi)
#print beta
if True: # not self.reached: #this is issues
cx = self.cur_pos_x
cy = self.cur_pos_y
fx = self.final_pos_x
fy = self.final_pos_y
b_c = cy - cx * p_slope
b_f = fy - fx * p_slope
sign_dist = b_f - b_c
if self.last_sign_dist < 0.0 and sign_dist > 0.0:
self.reached = True
if self.last_sign_dist > 0.0 and sign_dist < 0.0:
self.reached = True
#print "THE", self.last_sign_dist, sign_dist, self.reached
self.last_sign_dist = sign_dist
if self.reached:
self.last_sign_dist = 0.0
if True: #else: #this switch is gonna cause isses
master_scalar = 1.0
master_hype = sqrt((cx - fx)**2.0 + (cy - fy)**2.0)
if master_hype > 1.0:
master_scalar = 1.0
else:
master_scalar = master_hype
self.vx = sin(beta) * master_scalar
self.vy = cos(beta) * master_scalar
#print "THE VIX: ", self.vx, " THE VIY: ", self.vy
if True:
if self.alt_control:
#pid_offset = self.pid_alt.update(self.cur_alt)
#if pid_offset > 1.0:
# pid_offset = 1.0
#if pid_offset < -1.0:
# pid_offset = -1.0
if self.vy > 0.5:
self.vy = 0.5
if self.vy < -0.5:
self.vy = -0.5
if self.vx > 0.5:
self.vx = 0.5
if self.vx < -0.5:
self.vx = -0.5
#ned.
if self.final_alt > self.cur_alt:
self.vz = 0.5
if self.final_alt < self.cur_alt:
self.vz = -0.5
if abs(self.final_alt-self.cur_alt) < 0.9:
self.vz = 0.0
msg.twist.linear = geometry_msgs.msg.Vector3(self.vx*magnitude, self.vy*magnitude, self.vz*magnitude)
else:
msg.twist.linear = geometry_msgs.msg.Vector3(self.vx*magnitude, self.vy*magnitude, self.vz*magnitude)
if True:
#print self.vx, self.vy
self.pub_vel.publish(msg)
rate.sleep()
i +=1
def control_pid(self):
msg = SP.TwistStamped(
header=SP.Header(
frame_id="Drone_Vel_setpoint", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
ref_pose_msg = SP.PoseStamped(
header=SP.Header(
frame_id="setpoint_position", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
fuz_msg = fuzzy_msg(
header=SP.Header(
frame_id="fuzzy_values", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
smc_msg = vehicle_smc_gains_msg(
header=SP.Header(
frame_id="smc_values", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
if self.fuzzyfy:
# TODO: fuzzy com y e z
rospy.loginfo_once("initalized fuzzyfication for the fist time")
if not self.defuzzed.P_X == 0:
rospy.loginfo_once("first value of P_X modified by fuzzy system")
rospy.loginfo_once("P_x = " + str(self.defuzzed.P_X))
global KPx
KPx += self.defuzzed.P_X/SOFTNESS
if KPx <=0.6: KPx=0.6
if KPx >=5.6: KPx=5.6
global KIx
KIx += self.defuzzed.I_X/SOFTNESS
if KIx <= 0.3: KIx = 0.3
if KIx >= 4.5: KIx = 4.5
global KDx
KDx += self.defuzzed.D_X/SOFTNESS
if KDx <= 0.1: KDx = 0.1
if KDx >= 0.8: KDx = 0.8
self.error.x.act = self.x - DronePose.x
self.error.y.act = self.y - DronePose.y
self.error.z.act = self.z - DronePose.z
# rospy.loginfo(KPx)
PX = self.error.x.act * KPx
PY = self.error.y.act * KPy
PZ = self.error.z.act * KPz
_time_bet_run = (rate.sleep_dur.nsecs / 1000000000.0)
DX = ((self.error.x.Deri) * float(KDx)) / _time_bet_run
DY = ((self.error.y.Deri) * float(KDy)) / _time_bet_run
DZ = ((self.error.z.Deri) * float(KDz)) / _time_bet_run
self.error.x.Intg = self.error.x.act * KIx * _time_bet_run
IX = self.error.x.Intg
self.error.y.Intg = self.error.y.act * KIy * _time_bet_run
IY = self.error.y.Intg
self.error.z.Intg = self.error.z.act * KIz * _time_bet_run
IZ = self.error.z.Intg
# rospy.loginfo(IY)
self.x_vel = PX + DX + IX
self.y_vel = PY + DY + IY
self.z_vel = PZ + DZ + IZ
fuz_msg.Error = self.error.x.act
fuz_msg.D_Error = self.error.x.Deri
fuz_msg.P_val = KPx
fuz_msg.I_val = KIx
fuz_msg.D_val = KDx
ref_pose_msg.pose.position.x = setpoint_vel.x
ref_pose_msg.pose.position.y = setpoint_vel.y
ref_pose_msg.pose.position.z = setpoint_vel.z
msg.twist.linear.x = self.x_vel
msg.twist.linear.y = self.y_vel
msg.twist.linear.z = self.z_vel
smc_msg.k_gains = K_roll, K_pitch, K_yaw
smc_msg.beta_gains = B_roll, B_pitch, B_yaw
smc_msg.lambda_gains = L_roll, L_pitch, L_yaw
fuz_msg.header.stamp = rospy.Time.now()
self.pub_fuz.publish(fuz_msg)
ref_pose_msg.header.stamp = rospy.Time.now()
self.pub_refpos.publish(ref_pose_msg)
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
smc_msg.header.stamp = rospy.Time.now()
self.pub_smc.publish(smc_msg)
rate.sleep()