From d3ae4176e1860f359b47c7f1c14eff395bd3847e Mon Sep 17 00:00:00 2001
From: Shaur Kumar <shaurkinthewater@gmail.com>
Date: Thu, 28 May 2026 08:01:41 -0600
Subject: [PATCH] fix: cd objective library not properly calling do_objective

---
 example_scripts/cd_objective_lib.py | 28 ++++++++++++++++++++--------
 install.sh                          | 11 ++++++++++-
 2 files changed, 30 insertions(+), 9 deletions(-)

diff --git a/example_scripts/cd_objective_lib.py b/example_scripts/cd_objective_lib.py
index 85ab14e..1d6097e 100644
--- a/example_scripts/cd_objective_lib.py
+++ b/example_scripts/cd_objective_lib.py
@@ -26,7 +26,6 @@
 
 ACTION_NAME = "/do_objective"
 ACTION_TYPE = "moveit_studio_sdk_msgs/action/DoObjectiveSequence"
-ACTION_STATUS_TOPIC = f"{ACTION_NAME}/_action/status"
 
 TOTAL_TIMEOUT_S = 3600
 POLL_INTERVAL_S = 10
@@ -108,29 +107,42 @@ def _connect_rosbridge(client: roslibpy.Ros, deadline: float) -> None:
     _fail("Timeout: rosbridge websocket did not connect within 1h")
 
 
-def _get_topics(client: roslibpy.Ros) -> list:
-    result = {"topics": None}
+def _get_action_servers(client: roslibpy.Ros) -> list:
+    """Query rosapi for the list of advertised action servers.
+
+    /rosapi/topics filters hidden topics (anything matching `_action/*`), so
+    enumerating via topics misses every ROS 2 action. /rosapi/action_servers
+    returns the action names directly.
+    """
+    result = {"servers": []}
     done = Event()
 
     def _ok(resp):
-        result["topics"] = resp.get("topics", []) if isinstance(resp, dict) else []
+        # resp is roslibpy.ServiceResponse (dict-like but not a dict subclass)
+        # in current roslibpy; access via .get to stay compatible if that ever
+        # changes back to a plain dict.
+        result["servers"] = resp.get("action_servers", []) or []
         done.set()
 
     def _err(err):
-        print(f"rosapi topics call failed: {err}")
+        print(f"rosapi action_servers call failed: {err}")
         done.set()
 
-    client.get_topics(_ok, _err)
+    service = roslibpy.Service(
+        client, "/rosapi/action_servers", "rosapi_msgs/srv/GetActionServers"
+    )
+    service.call(roslibpy.ServiceRequest(), callback=_ok, errback=_err)
     if not done.wait(timeout=ROSAPI_CALL_TIMEOUT_S):
+        print("rosapi action_servers: timed out waiting for response", file=sys.stderr)
         return []
-    return result["topics"] or []
+    return result["servers"]
 
 
 def _wait_for_action_server(client: roslibpy.Ros, deadline: float) -> None:
     attempt = 0
     while time.monotonic() < deadline:
         attempt += 1
-        if ACTION_STATUS_TOPIC in _get_topics(client):
+        if ACTION_NAME in _get_action_servers(client):
             print(f"{ACTION_NAME} action server is up (attempt {attempt})")
             return
         print(f"Waiting for {ACTION_NAME} action server... (attempt {attempt})")
diff --git a/install.sh b/install.sh
index 6ee9437..a31d57a 100755
--- a/install.sh
+++ b/install.sh
@@ -30,7 +30,16 @@ fi
 if ! python3 -c "import roslibpy" 2>/dev/null; then
     echo "Installing roslibpy"
     sudo apt-get install -y python3-pip
-    python3 -m pip install --break-system-packages --ignore-installed roslibpy
+    # PEP 668: pip >= 23 on externally-managed Pythons (Ubuntu 23.04+,
+    # Debian 12+) refuses system-wide installs without --break-system-packages.
+    # Older pip (Ubuntu 22.04 ships 22.0.2) does not recognize the flag at all.
+    # Gate on the EXTERNALLY-MANAGED marker — present iff the flag is needed
+    # and supported.
+    PIP_ARGS=(--ignore-installed)
+    if compgen -G "/usr/lib/python3*/EXTERNALLY-MANAGED" > /dev/null; then
+        PIP_ARGS+=(--break-system-packages)
+    fi
+    python3 -m pip install "${PIP_ARGS[@]}" roslibpy
 fi
 
 echo "Installing CD objective shared library to /usr/lib/moveit-pro-scripts/"