别管别管别管

import rospy

from std_msgs.msg import String

import roslaunch

import threading

follow_flag = True

def drone_takeoff():

launch = roslaunch.scriptapi.ROSLaunch()

launch.start()

node = roslaunch.core.Node('package_name', '1.launch', args="takeoff")

launch.launch(node)

def follow_apriltag():

while follow_flag:

launch = roslaunch.scriptapi.ROSLaunch()

launch.start()

node = roslaunch.core.Node('package_name', '1.launch', args="follow_apriltag")

launch.launch(node)

def drone_land():

launch = roslaunch.scriptapi.ROSLaunch()

launch.start()

node = roslaunch.core.Node('package_name', '1.launch', args="land")

launch.launch(node)

def adjust_gimbal(param_value):

launch = roslaunch.scriptapi.ROSLaunch()

launch.start()

node = roslaunch.core.Node('package_name', 'uav.launch', args="foo:={}".format(param_value))

launch.launch(node)

def take_photo():

launch = roslaunch.scriptapi.ROSLaunch()

launch.start()

node = roslaunch.core.Node('package_name', '1.launch', args="take_photo")

launch.launch(node)

def adjust_take():

adjust_gimbal_thread = threading.Thread(target=adjust_gimbal, args=(90,))

adjust_gimbal_thread.start()

rospy.sleep(2)

take_photo_thread = threading.Thread(target=take_photo)

take_photo_thread.start()

take_photo_thread.join()

adjust_gimbal_thread = threading.Thread(target=adjust_gimbal, args=(-90,))

adjust_gimbal_thread.start()

adjust_gimbal_thread.join()

def docontrol(data):

global follow_flag

if data.data == "load":

follow_flag = False

drone_land()

elif data.data == "takeoff":

follow_flag = True

drone_takeoff()

elif data.data == "takephoto":

adjust_take()

def main():

rospy.init_node('drone_node', anonymous=True)

global drone_to_car_cmd_pub

drone_to_car_cmd_pub = rospy.Publisher('drone_to_car_cmd', String, queue_size=10)

# 无人机起飞

drone_takeoff()

# 等待5秒确保无人机起飞完毕

rospy.sleep(5)

# 发布指令给智能车，告知可以启动

drone_to_car_cmd_pub.publish("start")

# 无人机跟随AprilTag(线程)

follow_thread = threading.Thread(target=follow_apriltag)

follow_thread.start()

# 智能车在拍照点发送'takephoto'给无人机，无人机订阅消息

rospy.Subscriber('car_to_drone_cmd', String, docontrol)

# 智能车在充电点发送'load"给无人机，无人机订阅消息

rospy.Subscriber('car_to_drone_cmd', String, docontrol)

# 充电5s

rospy.sleep(5)

# 智能车在充电点发送"takeoff"给无人机，无人机订阅消息

rospy.Subscriber('car_to_drone_cmd', String, docontrol)

# 智能车在终点发送"load"给无人机，无人机订阅消息

rospy.Subscriber('car_to_drone_cmd', String, docontrol)

 

follow_thread.join()

rospy.spin()

if __name__ == '__main__':

main()