<commit_before>import rospy
import roslaunch
import subprocess
import os

from sensor_msgs.msg import LaserScan, NavSatFix
from std_msgs.msg import Float64;
from gazebo_msgs.msg import ModelStates

from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped
from std_srvs.srv import Empty

cur_pose = PoseStamped()
def pos_cb(msg):
    global cur_pose
    cur_pose = msg


NUM_UAV=5
local_pos = [None for i in range(NUM_UAV)]
mode_proxy = [None for i in range(NUM_UAV)]
arm_proxy = [None for i in range(NUM_UAV)]
pos_sub = [None for i in range(NUM_UAV)]
start_pos = [None for i in range(NUM_UAV)]


def mavrosTopicStringRoot(uavID=0):
    return ('mavros' + str(uavID+1))

startPosX = [5, 4, 4]
startPosY = [0, 0, 0]
startPosZ = [5, 5, 5]

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


def mavrosTopicStringRoot(uavID=0):
    return ('mavros' + str(uavID+1))

for uavID in range(0,NUM_UAV):
    local_pos[uavID] = rospy.Publisher(mavrosTopicStringRoot(uavID) + '/setpoint_position/local', PoseStamped, queue_size=10)
    mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode)
    arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool)
    pos_sub[uavID] = rospy.Subscriber(mavrosTopicStringRoot(uavID) + '/local_position/pose', PoseStamped, callback=pos_cb)

    start_pos[uavID] = PoseStamped()
    start_pos[uavID].pose.position.x = startPosX[uavID]
    start_pos[uavID].pose.position.y = startPosY[uavID]
    start_pos[uavID].pose.position.z = startPosZ[uavID]
    print uavID


data = [None for i in range(NUM_UAV)]

while None in data:
    for uavID in range(0, NUM_UAV):
        try:
            print mavrosTopicStringRoot(uavID) + '/global_position/rel_alt'
            data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5)
        except:
            pass

for uavID in range(0, NUM_UAV):
    print "wait for service"
    rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode')
    print "got service"

success = []
for uavID in range(0, NUM_UAV):
    try:
        print mode_proxy[uavID]
        success[uavID] = mode_proxy[uavID](1,'OFFBOARD')
        print 'mode '
        print success[uavID]
    except rospy.ServiceException, e:
        print ("mavros/set_mode service call failed: %s"%e)



#Main method
rate = rospy.Rate(10)
print "Main Running"
while not rospy.is_shutdown():
    for uavID in range(0, NUM_UAV):
        local_pos[uavID].publish(start_pos[uavID])
    rate.sleep()


#Arm
success = [None for i in range(NUM_UAV)]
for uavID in range(0, NUM_UAV):
    #Arm
    print 'arming' + mavrosTopicStringRoot(uavID)
    rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming')

<commit_msg>separating multi-uav launch script from from commanding<commit_after>import rospy
import roslaunch
import subprocess
import os

from sensor_msgs.msg import LaserScan, NavSatFix
from std_msgs.msg import Float64;
from gazebo_msgs.msg import ModelStates

from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped,Pose,Vector3,Twist,TwistStamped
from std_srvs.srv import Empty

cur_pose = PoseStamped()
def pos_cb(msg):
    global cur_pose
    cur_pose = msg


NUM_UAV=5
local_pos = [None for i in range(NUM_UAV)]
mode_proxy = [None for i in range(NUM_UAV)]
arm_proxy = [None for i in range(NUM_UAV)]
pos_sub = [None for i in range(NUM_UAV)]
start_pos = [None for i in range(NUM_UAV)]


def mavrosTopicStringRoot(uavID=0):
    return ('mavros' + str(uavID+1))

startPosX = [5, 4, 4, 2, 3]
startPosY = [0, 0, 0, 2, 3]
startPosZ = [5, 5, 5, 5, 5]

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


def mavrosTopicStringRoot(uavID=0):
    return ('mavros' + str(uavID+1))

for uavID in range(0,NUM_UAV):
    local_pos[uavID] = rospy.Publisher(mavrosTopicStringRoot(uavID) + '/setpoint_position/local', PoseStamped, queue_size=10)
    mode_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/set_mode', SetMode)
    arm_proxy[uavID] = rospy.ServiceProxy(mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool)
    pos_sub[uavID] = rospy.Subscriber(mavrosTopicStringRoot(uavID) + '/local_position/pose', PoseStamped, callback=pos_cb)

    start_pos[uavID] = PoseStamped()
    start_pos[uavID].pose.position.x = startPosX[uavID]
    start_pos[uavID].pose.position.y = startPosY[uavID]
    start_pos[uavID].pose.position.z = startPosZ[uavID]
    print uavID


data = [None for i in range(NUM_UAV)]

while None in data:
    for uavID in range(0, NUM_UAV):
        try:
            print mavrosTopicStringRoot(uavID) + '/global_position/rel_alt'
            data[uavID] = rospy.wait_for_message(mavrosTopicStringRoot(uavID) + '/global_position/rel_alt', Float64, timeout=5)
        except:
            pass

for uavID in range(0, NUM_UAV):
    print "wait for service"
    rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/set_mode')
    print "got service"

success = []
for uavID in range(0, NUM_UAV):
    try:
        print mode_proxy[uavID]
        success[uavID] = mode_proxy[uavID](1,'OFFBOARD')
        print 'mode '
        print success[uavID]
    except rospy.ServiceException, e:
        print ("mavros/set_mode service call failed: %s"%e)



#Main method
rate = rospy.Rate(10)
print "Main Running"
while not rospy.is_shutdown():
    for uavID in range(0, NUM_UAV):
        local_pos[uavID].publish(start_pos[uavID])
        print start_pos[uavID]
    rate.sleep()


#Arm
success = [None for i in range(NUM_UAV)]
for uavID in range(0, NUM_UAV):
    #Arm
    print 'arming' + mavrosTopicStringRoot(uavID)
    rospy.wait_for_service(mavrosTopicStringRoot(uavID) + '/cmd/arming')


{'fraction_non_alphanumeric': 0.11426101987899741, 'fraction_numerical': 0.011754537597234226, 'mean_word_length': 4.347504621072089, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2872936', 'n_tokens_mistral': 2307, 'n_tokens_neox': 2236, 'n_words': 472}