Cherrypicked icra demo merge

.github/workflows/docs.yaml

@@ -39,7 +39,7 @@ jobs:
     - name: Setup pixi
       uses: prefix-dev/setup-pixi@v0.8.1
       with:
-        pixi-version: v0.69.0
+        pixi-version: v0.70.2
         # enable caching of installed envs
         cache: true
         # only write new caches on main pushes (TODO: Enable)

.github/workflows/tests.yaml

@@ -14,7 +14,7 @@ jobs:
       - name: Setup Pixi (installs pixi + caches envs) # https://github.com/marketplace/actions/setup-pixi
         uses: prefix-dev/setup-pixi@v0.9.3            # pin the action version
         with:
-          pixi-version: v0.69.0                       # pin the pixi binary version (optional)
+          pixi-version: v0.70.2                       # pin the pixi binary version (optional)
           cache: true                                 # enable caching of installed envs
           # only write new caches on main pushes (TODO: Enable)
           # cache-write: ${{ github.event_name == 'push' && github.ref_name == 'main' }}

swarm_gpt/core/backend.py

@@ -341,7 +341,7 @@ def deploy(self, drone_ids: list[int] | None = None) -> bool:
             landing_pos = obs["pos"] if self.settings["land_on_docks"] else d["pos"]
             # TODO fix hard coded yaw
             init_pos_dict[uri] = np.array([*init_pos, 0.0])
-            final_pos_dict[uri] = np.array([*landing_pos + np.array([0.0, 0.0, 0.8]), 0.0])
+            final_pos_dict[uri] = np.array([*landing_pos + np.array([0.0, 0.0, 0.5]), 0.0])
             landing_pos_dict[uri] = np.array([*landing_pos - np.array([0.0, 0.0, 0.2]), 0.0])
             choreography_dict[uri] = self.splines[i]
             color_top[uri] = {
@@ -366,7 +366,9 @@ def deploy(self, drone_ids: list[int] | None = None) -> bool:
                     taken_off = False
                     continue
                 try:
+                    logger.debug(f"Trying to get obs for {uri}")
                     obs = swarm.get_obs(uri)
+                    logger.debug(f"got obs for {uri}")
                     z = obs["pos"][2]
                     qw = np.abs(obs["quat"][-1])
                 # Demo fix: If the drone is disconnected, we cannot get its position. We assume it has not taken off.
@@ -384,16 +386,17 @@ def deploy(self, drone_ids: list[int] | None = None) -> bool:
                         "VLC could not start playback; skipping choreography (drones will land)."
                     )
                 else:
+                    logger.debug("Starting choreography execution")
                     swarm.execute_choreography(
                         choreography_dict,
                         self.waypoints["time"][0, -1],
                         color_top=color_top,
                         color_bot=color_bot,
                     )
             swarm.goto(final_pos_dict, duration=2.0)  # Transition from ideal point to hover pos
-            # if self.settings["land_on_docks"]: # Commented out for demo
-            #     swarm.goto(final_pos_dict, duration=5.0)  # Hovering
-            swarm.goto(landing_pos_dict, duration=1.5)  # Landing
+            if self.settings["land_on_docks"]: # Commented out for demo
+                swarm.goto(final_pos_dict, duration=3.0)  # Hovering
+            swarm.land(duration=1.5)  # Landing
         finally:
             swarm.close()
         self.music_manager.song = original_song

swarm_gpt/core/drone_swarm.py

@@ -96,7 +96,8 @@ def __init__(
 
     def get_obs(self, uri: str) -> dict[str, Array]:
         """Generate the observation for a drone using mocap or lighthouse."""
-        return self._run(self._read_observation(uri))
+        obs = self._run(self._parallel_by_uri("Reading observation", [uri], self._read_observation))
+        return obs[0]
 
     def missing_uris(self) -> list[str]:
         """Return configured URIs that are not currently active."""
@@ -420,6 +421,7 @@ async def _parallel_by_uri(
                 logger.error(f"{uri} disconnected or unreachable. {action_name} failed: {result}")
             else:
                 logger.error(f"{action_name} failed for {uri}: {result}")
+        return results
 
     async def _emergency_stop(self, uri: str) -> None:
         await self._cf(uri).localization().emergency().send_emergency_stop()
@@ -483,6 +485,8 @@ async def _read_lighthouse_observation(self, uri: str) -> dict[str, Array]:
                 "rpy": rpy,
                 "is_outdated": False,
             }
+        except Exception as exc:
+            logger.error(f"Reading observation from {uri} failed: {exc}")
         finally:
             await stream.stop()