WIP Release detections

assets/foxglove_dashboards/unitree.json → assets/foxglove_dashboards/go2.json

@@ -2,14 +2,14 @@
   "configById": {
     "3D!3ezwzdr": {
       "cameraState": {
-        "perspective": true,
-        "distance": 11.974738784563408,
-        "phi": 46.81551198543342,
-        "thetaOffset": 71.81370622323534,
+        "perspective": false,
+        "distance": 10.26684166532264,
+        "phi": 29.073691502600532,
+        "thetaOffset": 93.32472375597958,
         "targetOffset": [
-          0.7911075559165568,
-          1.8116053369121197,
-          3.4025317970075274e-16
+          3.280168913303102,
+          -1.418093876569801,
+          -2.6619087209849424e-16
         ],
         "target": [
           0,
@@ -29,7 +29,8 @@
       "followMode": "follow-pose",
       "scene": {
         "transforms": {
-          "labelSize": 0.1
+          "labelSize": 0.1,
+          "axisSize": 0.51
         }
       },
       "transforms": {
@@ -49,10 +50,10 @@
           "visible": false
         },
         "frame:map": {
-          "visible": true
+          "visible": false
         },
         "frame:world": {
-          "visible": true
+          "visible": false
         }
       },
       "topics": {
@@ -63,7 +64,7 @@
           "colorMap": "turbo",
           "pointSize": 2.85,
           "decayTime": 6,
-          "pointShape": "cube"
+          "pointShape": "circle"
         },
         "/detectorDB/scene_update": {
           "visible": true
@@ -86,7 +87,9 @@
           "visible": false
         },
         "/camera_info": {
-          "visible": true
+          "visible": true,
+          "distance": 1,
+          "color": "#c4bcffff"
         },
         "/detectorDB/pointcloud/0": {
           "visible": true,
@@ -121,19 +124,21 @@
           "colorField": "z",
           "colorMode": "colormap",
           "colorMap": "turbo",
-          "pointSize": 3.66,
-          "pointShape": "cube",
+          "pointSize": 4,
+          "pointShape": "circle",
           "explicitAlpha": 1,
           "cubeSize": 0.05,
           "cubeOutline": false,
-          "flatColor": "#ed8080ff"
+          "flatColor": "#ed8080ff",
+          "minValue": -0.1,
+          "decayTime": 0
         },
         "/global_costmap": {
-          "visible": false,
+          "visible": true,
           "colorMode": "custom",
-          "unknownColor": "#80808000",
-          "minColor": "#282975ff",
-          "maxColor": "#ff0000ff",
+          "unknownColor": "#ff000000",
+          "minColor": "#484981ff",
+          "maxColor": "#000000ff",
           "frameLocked": false,
           "drawBehind": false
         },
@@ -203,6 +208,18 @@
           "maxColor": "#ff2222ff",
           "minColor": "#00006bff",
           "unknownColor": "#80808000"
+        },
+        "/debug_navigation": {
+          "visible": false,
+          "cameraInfoTopic": "/camera_info"
+        },
+        "/path": {
+          "visible": true,
+          "lineWidth": 0.03,
+          "gradient": [
+            "#ff6b6bff",
+            "#ff0000ff"
+          ]
         }
       },
       "layers": {
@@ -228,6 +245,31 @@
           "frameId": "world",
           "divisions": 25,
           "lineWidth": 1
+        },
+        "aac2d29a-9580-442f-8067-104830c336c8": {
+          "displayMode": "auto",
+          "fallbackColor": "#ffffff",
+          "showAxis": false,
+          "axisScale": 1,
+          "showOutlines": true,
+          "opacity": 1,
+          "visible": true,
+          "frameLocked": true,
+          "instanceId": "aac2d29a-9580-442f-8067-104830c336c8",
+          "label": "URDF",
+          "layerId": "foxglove.Urdf",
+          "sourceType": "filePath",
+          "url": "",
+          "filePath": "/home/lesh/coding/dimos/dimos/robot/unitree/go2/go2.urdf",
+          "parameter": "",
+          "topic": "",
+          "framePrefix": "",
+          "order": 2,
+          "links": {
+            "base_link": {
+              "visible": true
+            }
+          }
         }
       },
       "publish": {
@@ -240,7 +282,67 @@
         "poseEstimateThetaDeviation": 0.26179939
       },
       "imageMode": {},
-      "followTf": "world"
+      "followTf": "map"
+    },
+    "command-center-extension.command-center!3xr2po0": {},
+    "Plot!3cog9zw": {
+      "paths": [
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/metrics/_calculate_costmap.data",
+          "enabled": true,
+          "color": "#4e98e2",
+          "id": "a1ff9a80-7a45-48ff-bdb1-232bda7bd492"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/metrics/get_global_pointcloud.data",
+          "enabled": true,
+          "color": "#f5774d",
+          "id": "5fe70fbd-33f9-4b15-849f-c7c49988af95"
+        },
+        {
+          "timestampMethod": "receiveTime",
+          "value": "/metrics/add_frame.data",
+          "enabled": true,
+          "color": "#f7df71",
+          "id": "bb4a56f8-78ae-45cb-850e-48c462dab40f"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
+    },
+    "Plot!47kna9v": {
+      "paths": [
+        {
+          "timestampMethod": "publishTime",
+          "value": "/global_map.header.stamp.sec",
+          "enabled": true,
+          "color": "#4e98e2",
+          "id": "19f95865-4d9e-4d38-b9d7-d227319d8ebd"
+        },
+        {
+          "timestampMethod": "publishTime",
+          "value": "/global_costmap.header.stamp.sec",
+          "enabled": true,
+          "color": "#f5774d",
+          "id": "86ddc0e2-8e9c-4d52-bd5a-d02cb0357efe"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240
     },
     "Image!3mnp456": {
       "cameraState": {
@@ -308,7 +410,7 @@
           "visible": false
         },
         "/global_costmap": {
-          "visible": true,
+          "visible": false,
           "minColor": "#ffffff00",
           "maxColor": "#ff0000ff",
           "unknownColor": "#80808000"
@@ -351,7 +453,8 @@
           "colorMode": "colormap",
           "colorMap": "turbo",
           "pointShape": "cube",
-          "pointSize": 5
+          "pointSize": 5,
+          "cubeSize": 0.03
         },
         "/detected/pointcloud/1": {
           "visible": false,
@@ -480,7 +583,21 @@
   "layout": {
     "direction": "row",
     "first": "3D!3ezwzdr",
-    "second": "Image!3mnp456",
-    "splitPercentage": 70.55232558139535
+    "second": {
+      "first": "command-center-extension.command-center!3xr2po0",
+      "second": {
+        "first": {
+          "first": "Plot!3cog9zw",
+          "second": "Plot!47kna9v",
+          "direction": "row"
+        },
+        "second": "Image!3mnp456",
+        "direction": "column",
+        "splitPercentage": 38.08411214953271
+      },
+      "direction": "column",
+      "splitPercentage": 50.116550116550115
+    },
+    "splitPercentage": 63.706720977596746
   }
 }

dimos/e2e_tests/conftest.py

@@ -0,0 +1,86 @@
+# Copyright 2025 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from collections.abc import Callable, Iterator
+
+import pytest
+
+from dimos.core.transport import pLCMTransport
+from dimos.e2e_tests.dimos_cli_call import DimosCliCall
+from dimos.e2e_tests.lcm_spy import LcmSpy
+from dimos.msgs.geometry_msgs import PoseStamped, Quaternion
+from dimos.msgs.geometry_msgs.Vector3 import make_vector3
+from dimos.msgs.std_msgs.Bool import Bool
+
+
+def _pose(x: float, y: float, theta: float) -> PoseStamped:
+    return PoseStamped(
+        position=make_vector3(x, y, 0),
+        orientation=Quaternion.from_euler(make_vector3(0, 0, theta)),
+        frame_id="map",
+    )
+
+
+@pytest.fixture
+def lcm_spy() -> Iterator[LcmSpy]:
+    lcm_spy = LcmSpy()
+    lcm_spy.start()
+    yield lcm_spy
+    lcm_spy.stop()
+
+
+@pytest.fixture
+def follow_points(lcm_spy: LcmSpy):
+    def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None:
+        topic = "/goal_reached#std_msgs.Bool"
+        lcm_spy.save_topic(topic)
+
+        for x, y, theta in points:
+            lcm_spy.publish("/goal_request#geometry_msgs.PoseStamped", _pose(x, y, theta))
+            lcm_spy.wait_for_message_result(
+                topic,
+                Bool,
+                predicate=lambda v: bool(v),
+                fail_message=fail_message,
+                timeout=60.0,
+            )
+
+    yield fun
+
+
+@pytest.fixture
+def start_blueprint() -> Iterator[Callable[[str], DimosCliCall]]:
+    dimos_robot_call = DimosCliCall()
+
+    def set_name_and_start(demo_name: str) -> DimosCliCall:
+        dimos_robot_call.demo_name = demo_name
+        dimos_robot_call.start()
+        return dimos_robot_call
+
+    yield set_name_and_start
+
+    dimos_robot_call.stop()
+
+
+@pytest.fixture
+def human_input():
+    transport = pLCMTransport("/human_input")
+    transport.lcm.start()
+
+    def send_human_input(message: str) -> None:
+        transport.publish(message)
+
+    yield send_human_input
+
+    transport.lcm.stop()