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 publish_orientation(self, ang=180):
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
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, ang)
pose.pose.orientation = SP.Quaternion(*q)
#thd_pub.publish(data=self.throttle_update)
#thd_pub.publish(data=0.6)
att_pub.publish(pose)
print "pose orientation: ", self.throttle_update
def publish_pose(self):
self.pub = SP.get_pub_position_local(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
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
# 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)
msg.pose.orientation = SP.Quaternion(*quaternion)
if self.publish == True:
self.pub.publish(msg)
print "publishing position"
rate.sleep()
def navigate(self): msg = PositionTarget( header=SP.Header( frame_id="body", stamp=rospy.Time.now()), type_mask=0b0000101111000111, coordinate_frame=8 ) while not rospy.is_shutdown(): if not self.activated: break # Controla a velocidade em y no referencial do corpo KPy = 0.1 vy = KPy * self.dy # Controlador P para a altitude KPz = 0.5 erro_z = self.z - drone_pos.z vz = KPz * erro_z # Limite para a velocidade vertical máxima max_up_vel = 5.0 vz = min(vz, max_up_vel) msg.velocity.x = self.vx msg.velocity.y = vy msg.velocity.z = vz msg.yaw = self.yaw - np.pi/2 # Após reinstalação do ROS, foi necessário defasar em -90 graus # Publica a mensagem self.pub.publish(msg) rate.sleep()
def blocked_yaw(self, yaw=120):
throttle_pid = pid_controller.PID(P=1.0,
I=0.0,
D=0.5,
Integrator_max=0.5,
Integrator_min=0.0)
throttle_pid.setPoint(self.current_alt)
"attempting to maintain alt: ", self.current_alt
time.sleep(2)
while True:
att_pub = SP.get_pub_attitude_pose(queue_size=10)
thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
#while not rospy.is_shutdown():
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, 90)
pose.pose.orientation = SP.Quaternion(*q)
throt = throttle_pid.update(self.current_alt)
#if throt > 0.6:
# throt = 0.6
#if throt < 0.4:
# throt = 0.6
if self.attitude_publish or True:
att_pub.publish(pose)
thd_pub.publish(data=throt)
print "pose orientation: ", throt
def navigate(self):
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
quaternion = quaternion_from_euler(0, 0, self.yaw)
#msg.pose.orientation.x = quaternion[0]
#msg.pose.orientation.y = quaternion[1]
#msg.pose.orientation.z = quaternion[2]
#msg.pose.orientation.w = quaternion[3]
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 180 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, self.yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
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 navigate(self):
rate = rospy.Rate(10) # 10hz
while True:
msg = SP.PoseStamped(header=SP.Header(frame_id='',
stamp=rospy.Time.now()), )
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 3.0
rospy.loginfo(msg)
self.pub.publish(msg)
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):
msg = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # 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
yaw = radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def navigate(self):
rate = rospy.Rate(10)
msg = setpoint.PoseStamped(
header=setpoint.Header(
frame_id="map", # isn't used anyway
stamp=rospy.Time.now()), )
while not rospy.is_shutdown():
#msg.pose.position.x = self.y
#msg.pose.position.y = self.x
#msg.pose.position.z = - self.z
msg.pose.position.x = self.x
msg.pose.position.y = self.y
msg.pose.position.z = self.z
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): msg = PositionTarget( header=SP.Header( frame_id="world", stamp=rospy.Time.now()), coordinate_frame=1 ) while not rospy.is_shutdown(): if not self.activated: break # Preenchendo mensagem msg.position.x = self.x msg.position.y = self.y msg.position.z = self.z msg.yaw = self.yaw # Publica a mensagem self.pub.publish(msg) rate.sleep()
def navigate(self):
""" Continuously publishes position target to the flight controller"""
rate = rospy.Rate(8) # in Hz
rate_mode_check = rospy.Rate(20)
message_pos = setpoint.PoseStamped(
header=setpoint.Header(
frame_id="base_footprint", # no matter, plugin doesn't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
message_pos.pose.position.x = self.setpoint_x
message_pos.pose.position.y = self.setpoint_y
message_pos.pose.position.z = self.setpoint_z
yaw = 0
quaternion = quaternion_from_euler(0, 0, yaw)
message_pos.pose.orientation = setpoint.Quaternion(*quaternion)
self.setpoint_position.publish(message_pos)
rate.sleep()
def control_loop(self):
rate = rospy.Rate(self.rate) # 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_ref
msg.pose.position.y = self.y_ref
msg.pose.position.z = self.z_ref
# 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)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
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 navigate_position(self):
msg_set_pos = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
msg_set_pos.pose.position.x = self.x
msg_set_pos.pose.position.y = self.y
msg_set_pos.pose.position.z = self.z
yaw = math.radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg_set_pos.pose.orientation = SP.Quaternion(*quaternion)
# rospy.loginfo("there is life on navigate")
self.pub_setpoint.publish(msg_set_pos)
rate.sleep()
def navigate(self):
rate = rospy.Rate(10) # 10hz
command =Twist()
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
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.ya # North: ya=0
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
command.angular.z = kp*yaw #yaw pose control
self.pub_cmd_vel.publish(command) #publish yaw control
self.pub_pos.publish(msg) #publish position control
rate.sleep()
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 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()
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 _pubMsg(self, msg, topic):
msg.header = mavSP.Header(frame_id="att_pose", stamp=rospy.Time.now())
topic.publish(msg)
self.rate.sleep()
