Fixed multi-drone global plan

robot/ros_ws/manual_exploration.sh

@@ -2,10 +2,10 @@
 set -euo pipefail
 
 # Pair 2 (run in parallel)
-ros2 service call /robot_1/behavior/global_plan_toggle \
+ros2 service call /${ROBOT_NAME}/behavior/global_plan_toggle \
   std_srvs/srv/Trigger "{}" & p1=$!
 
-ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \
+ros2 service call /${ROBOT_NAME}/trajectory_controller/set_trajectory_mode \
   airstack_msgs/srv/TrajectoryMode "{mode: 3}" & p2=$!
 
 wait $p1 $p2

robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle"
+    srv_global_plan_toggle_topic: "~/global_plan_toggle"
     way_point_planners: 
       - name: "random_walk"
         config: 

robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml

@@ -14,7 +14,7 @@
     sub_vdb_topic: "vdb_grid"
     sub_robot_tf_topic: "/tf"
     sub_target_path_topic: "/perspective_goals"
-    srv_exploration_toggle_topic: "/robot_1/behavior/global_plan_toggle"
+    srv_exploration_toggle_topic: "~/global_plan_toggle"
 
     publish_visualizations: false # should trajectory visualizations be published
     num_paths_to_generate: 5 # how many paths to string together

robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml

@@ -6,6 +6,7 @@
         <remap from="vdb_grid" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_grid" />
         <remap from="sensor_pointcloud" to="/$(env ROBOT_NAME)/sensors/ouster/point_cloud" />
         <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
 </launch>
 
@@ -17,5 +18,6 @@
         <remap from="vdb_grid" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_grid" />
         <remap from="sensor_pointcloud" to="/$(env ROBOT_NAME)/sensors/ouster/point_cloud" />
         <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
 </launch> -->

robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml

@@ -11,6 +11,7 @@
         <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
         <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
         <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node> -->
 
     <!-- Exploration Planning -->

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml

@@ -6,7 +6,7 @@
     pub_trajectory_viz_topic: "~/traj_viz"
     sub_map_topic: "vdb_map_visualization"
     sub_robot_tf_topic: "/tf"
-    srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle"
+    srv_random_walk_toggle_topic: "~/global_plan_toggle"
 
     publish_visualizations: false # should trajectory visualizations be published
     num_paths_to_generate: 5 # how many random walk paths to string together

robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml

@@ -11,6 +11,7 @@
         <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
         <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
         <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
 
 </launch>

robot/ros_ws/src/robot_bringup/rviz/robot.rviz

@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 70
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -15,8 +15,9 @@ Panels:
         - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
         - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
         - /Global1
+        - /Global1/Global Plan1
       Splitter Ratio: 0.590062141418457
-    Tree Height: 551
+    Tree Height: 341
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -37,7 +38,7 @@ Panels:
     SyncSource: Lidar
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
-    topic: /robot_1/behavior/behavior_tree_graphviz
+    topic: /robot_2/behavior/behavior_tree_graphviz
     zoom_factor: 0.1919851303100586
 Visualization Manager:
   Class: ""
@@ -119,44 +120,43 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        map:
-          base_link:
-            base_link_body_body_link:
-              OS1_REV6_128_10hz___512_resolution:
-                lidar:
-                  {}
-              base_link_ZED_X:
-                camera_left:
-                  macvo_ned:
+        world:
+          map:
+            base_link:
+              base_link_body_body_link:
+                OS1_REV6_128_10hz___512_resolution:
+                  lidar:
+                    {}
+                base_link_ZED_X:
+                  camera_left:
+                    macvo_ned:
+                      {}
+                  camera_right:
+                    {}
+                  imu:
                     {}
-                camera_right:
+                rotor0:
                   {}
-                imu:
+                rotor1:
                   {}
-              rotor0:
-                {}
-              rotor1:
-                {}
-              rotor2:
-                {}
-              rotor3:
+                rotor2:
+                  {}
+                rotor3:
+                  {}
+              base_link_frd:
                 {}
-            base_link_frd:
+            base_link_stabilized:
+              {}
+            look_ahead_point:
+              {}
+            look_ahead_point_stabilized:
+              {}
+            map_ned:
+              {}
+            tracking_point:
+              {}
+            tracking_point_stabilized:
               {}
-          base_link_stabilized:
-            {}
-          look_ahead_point:
-            {}
-          look_ahead_point_stabilized:
-            {}
-          map_ned:
-            {}
-          tracking_point:
-            {}
-          tracking_point_stabilized:
-            {}
-        world:
-          {}
       Update Interval: 0
       Value: true
     - Alpha: 0.5
@@ -273,8 +273,8 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 0.017708778381347656
-            Min Value: -0.07378101348876953
+            Max Value: -0.03934618458151817
+            Min Value: -0.0849701464176178
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -492,8 +492,7 @@ Visualization Manager:
               Enabled: true
               Name: Trimmed Global Plan for DROAN
               Namespaces:
-                global_plan: true
-                global_plan_extra_text: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -640,12 +639,7 @@ Visualization Manager:
                   Enabled: true
                   Name: Traj Debug
                   Namespaces:
-                    collision_points: true
-                    collision_trajectories: true
-                    free_points: true
-                    free_trajectories: true
-                    unseen_points: true
-                    unseen_trajectories: true
+                    {}
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
@@ -657,7 +651,7 @@ Visualization Manager:
                   Enabled: true
                   Name: Graph Vis
                   Namespaces:
-                    default: true
+                    {}
                   Topic:
                     Depth: 5
                     Durability Policy: Volatile
@@ -683,8 +677,7 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                global_plan: true
-                global_plan_extra_text: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -696,7 +689,7 @@ Visualization Manager:
               Enabled: true
               Name: Rewind Info
               Namespaces:
-                rewind_info: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -713,7 +706,6 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
-                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -782,6 +774,20 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Global
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/sensors/front_stereo/left/image_rect
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -828,7 +834,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 16.645448684692383
+      Distance: 42.59531784057617
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -843,34 +849,36 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7353981733322144
+      Pitch: 0.2153983861207962
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 0.13039594888687134
+      Yaw: 4.4285736083984375
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
     collapsed: false
   Displays:
-    collapsed: true
+    collapsed: false
   Front Right Depth:
     collapsed: false
   Front Right RGB:
     collapsed: true
   Height: 1376
-  Hide Left Dock: true
+  Hide Left Dock: false
   Hide Right Dock: true
+  Image:
+    collapsed: false
   MACVO Disparity:
     collapsed: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001ce00000356fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a005600690065007700730000000078000000a0000000a000fffffffb000000100044006900730070006c006100790073000000011e000002b0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c90000000000000000fb00000028004d004100430056004f00200049006d00610067006500200046006500610074007500720065007300000002ba000000ca0000000000000000fb0000002200460072006f006e00740020005200690067006800740020004400650070007400680000000336000001d20000002800ffffff00000001000001f600000356fc0200000008fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001e00460072006f006e007400200052006900670068007400200052004700420000000078000001d50000002800fffffffc000000f7000001700000000000fffffffa000000010100000002fb0000002200460072006f006e00740020005200690067006800740020004400650070007400680000000000ffffffff0000009e00fffffffb0000002200460072006f006e007400200052006900670068007400200044006500700074006801000007c4000001f60000000000000000fb0000002200460072006f006e0074002000520069006700680074002000440065007000740068010000014a000001cc0000000000000000fb0000001e004d004100430056004f002000440069007300700061007200690074007900000002530000017b0000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000004dd00000037fc0100000002fb0000000800540069006d00650100000000000004dd0000025300fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004dd00000171fc0100000002fb00000022004200650068006100760069006f0072005400720065006500500061006e0065006c0100000000000004dd000001fa00fffffffb0000000800540069006d00650100000000000004500000000000000000000004dd0000035600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
-  Width: 1245
-  X: 3235
-  Y: 27
+    collapsed: false
+  Width: 1745
+  X: 1263
+  Y: 60