Masuk ke Aeronet, Episode 4: Cari dan Persetan

 #!/usr/bin/env python # coding=UTF-8 # baloon position steering import rospy import mavros import mavros.command as mc from mavros_msgs.msg import State from geometry_msgs.msg import PoseStamped, Twist, Quaternion, TwistStamped from mavros_msgs.srv import CommandBool from mavros_msgs.srv import SetMode import tf.transformations as t import math current_state=State() current_pose = PoseStamped() current_vel = Twist() baloon_twist = TwistStamped() def state_callback(data): global current_state current_state=data def localpose_callback(data): global current_pose current_pose = data def baloon_callback(data): global baloon_twist baloon_twist = data def publish_setvel(event): global current_pose, setvel_pub, setvel, setvel_forward, baloon_twist q=current_pose.pose.orientation.x, current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w roll, pitch, yaw = t.euler_from_quaternion(q) setvel.linear.x = setvel_forward * math.cos(yaw) setvel.linear.y = setvel_forward * math.sin(yaw) setvel_pub.publish(setvel) def main(): global current_pose, setvel, setvel_pub, setvel_forward, baloon_twist rospy.init_node("offbrd",anonymous=True) rate=rospy.Rate(10) state=rospy.Subscriber("/mavros/state",State,state_callback) pose_sub=rospy.Subscriber("/mavros/local_position/pose",PoseStamped,localpose_callback) baloon_sub=rospy.Subscriber("/baloon_detector/twist",TwistStamped,baloon_callback) setvel_pub=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) arming_s=rospy.ServiceProxy("/mavros/cmd/arming",CommandBool) set_mode=rospy.ServiceProxy("/mavros/set_mode",SetMode) setvel=Twist() setvel_forward = 0 arming_s(True) set_mode(0,"AUTO.TAKEOFF") print 'Taking off.....\r' rospy.sleep(5) for i in range (0,10): setvel_pub.publish(setvel) rate.sleep() set_mode(0,"OFFBOARD") setvel_timer = rospy.Timer(rospy.Duration(0.05), publish_setvel) while not rospy.is_shutdown(): time_delay = rospy.Time.now().to_sec() - baloon_twist.header.stamp.to_sec() #print baloon_twist print 'time delay = ',time_delay if time_delay<0.2:#    0.2   if baloon_twist.twist.linear.x > 0.8: setvel_forward = 1.5 elif baloon_twist.twist.linear.x > 0.8: setvel_forward = 0.0 else: setvel_forward = -0.5 setvel.angular.z = baloon_twist.twist.angular.z*4 if baloon_twist.twist.angular.y<0: setvel.linear.z=0.5 elif baloon_twist.twist.angular.y>0.2: setvel.linear.z=-0.25 else: setvel.linear.z=0 else:#    setvel.angular.z=setvel_forward=setvel.linear.z=0 print setvel, setvel_forward rate.sleep() set_mode(0,"AUTO.LAND") print 'Landing.......\r' setvel_timer.shutdown() rospy.sleep(5) if __name__=="__main__": main()