Swarms finish

TPODAvia · Jun 11, 2023 · c866292 · c866292
1 parent 95599de
commit c866292
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 9 deletions.
diff --git a/px4_sim/launch/5mavros_posix_only.launch b/px4_sim/launch/5mavros_posix_only.launch
@@ -11,7 +11,7 @@
     <arg name="Y" default="0"/>
     <!-- vehicle model and world -->
     <!-- <arg name="est" default="ekf2"/> -->
-    <arg name="ID" default="1"/>
+    <arg name="ID" default="0"/>
     <!-- any vehicle: iris, frame450, acker_rover, diff_rover, avia_vtol, typhoon_h480 etc -->
     <arg name="vehicle" default="frame450"/>
     <!-- Any world: empty, flat, city, forest etc -->
@@ -36,9 +36,6 @@
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
     <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
-    <!-- <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen"
-        args="$(find px4)/build/px4_sitl_default/etc 
-        -s etc/init.d-posix/rcS $(arg px4_command_arg1)" required="true"/> -->
 
     <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc 
     -s etc/init.d-posix/rcS 

diff --git a/px4_sim/launch/5multi_uav_qgc_sitl.launch b/px4_sim/launch/5multi_uav_qgc_sitl.launch
@@ -5,7 +5,7 @@
     <!-- vehicle model: only frame450 are available for swarming -->
     <arg name="vehicle" default="frame450"/>
     <!-- Any world: empty, flat, city, forest etc -->
-    <arg name="world" default="$(find px4_sim)/worlds/empty.world"/>
+    <arg name="world" default="$(find px4_sim)/worlds/aruco.world"/>
 
     <!-- gazebo configs -->
     <arg name="gui" default="false"/>

diff --git a/swarms/src/sequence.py b/swarms/src/sequence.py
@@ -214,8 +214,8 @@ def main():
 
         print("Drones available", available_drones)
         swarm_data.data, flight_trigger = caller(my_drone_id)
-        if str(my_drone_id) == "0":
-            swarm_data.data = f"Drone {my_drone_id} in_mission reached"
+        # if str(my_drone_id) == "0":
+        #     swarm_data.data = f"Drone {my_drone_id} in_mission reached"
         my_drone_id_ready_pub.publish(swarm_data)
         if flight_trigger:
             PX4modes.setAutoMissionMode()
@@ -255,11 +255,11 @@ def caller(my_drone_id):
                         return swarm_data, False
 
                 count = sum(1 for drone_id in drone_ids if drone_id < my_drone_id)
-                # print(f"Number of drone_ids smaller than {my_drone_id}: {count}")
+                print(f"Number of drone_ids smaller than {my_drone_id}: {count}")
 
                 # Check the priority of the drone
                 if count == 0:
-                    print(f"Count is 0 {my_drone_id}")
+                    print(f"Count is: {my_drone_id}")
                     swarm_data = f"Drone {my_drone_id} in_mission not_reached"
                     return swarm_data, True