Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
90b59dc
fixes testing stream issue with blocking video on foxglove
leshy Jul 11, 2025
e2945bb
unitree replay working
leshy Jul 11, 2025
98911c2
control module threading fix
leshy Jul 11, 2025
9cf06f0
potential real unitree go2 fix
leshy Jul 11, 2025
bcb0b80
all working
leshy Jul 11, 2025
e43b03b
removed pytest dep from core
leshy Jul 11, 2025
340bf24
lcmspy cli
leshy Jul 11, 2025
a0fbef3
Merge branch 'lcmspy_cli2' into testing_stream_fix
leshy Jul 11, 2025
634d4c4
total traffic measure
leshy Jul 11, 2025
1a3f2ac
Merge branch 'lcmspy_cli2' into testing_stream_fix
leshy Jul 11, 2025
8e032b3
lcmspy test import fix
leshy Jul 11, 2025
02a059b
Merge branch 'lcmspy_cli2' into testing_stream_fix
leshy Jul 11, 2025
8447b51
scheduler fix for replay
leshy Jul 11, 2025
dad44e0
global map publishing
leshy Jul 12, 2025
8d278c7
foxglove dash
leshy Jul 12, 2025
2797854
test fix, moved stuff to multiprocess/
leshy Jul 12, 2025
44967d0
trying to fix tests in CI
leshy Jul 12, 2025
d88146b
lcmservice autoconf fix
leshy Jul 12, 2025
22fc003
lcmservice autoconf fix
leshy Jul 12, 2025
e084699
lcmservice test fixes
leshy Jul 12, 2025
5e2737a
Merge branch 'lcmspy_cli2' into testing_stream_fix
leshy Jul 12, 2025
e445234
lcmspy doesn't limit topic display width
leshy Jul 12, 2025
89a8189
disabled LCM tests in CI
leshy Jul 12, 2025
6e1026c
Merge branch 'lcmspy_cli2' into testing_stream_fix
leshy Jul 12, 2025
d9a06dc
mini updates to foxglove dash, publishing global map every 2.5 seconds
leshy Jul 12, 2025
d351271
PR fixes
leshy Jul 12, 2025
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
221 changes: 221 additions & 0 deletions assets/foxglove_unitree_lcm_dashboard.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,221 @@
{
"configById": {
"3D!18i6zy7": {
"layers": {
"845139cb-26bc-40b3-8161-8ab60af4baf5": {
"visible": true,
"frameLocked": true,
"label": "Grid",
"instanceId": "845139cb-26bc-40b3-8161-8ab60af4baf5",
"layerId": "foxglove.Grid",
"lineWidth": 0.5,
"position": [
0,
0,
0
],
"rotation": [
0,
0,
0
],
"order": 1,
"size": 30,
"divisions": 30
}
},
"cameraState": {
"perspective": true,
"distance": 22.16066481982733,
"phi": 34.816360162458444,
"thetaOffset": 110.38709006815255,
"targetOffset": [
0.6543165797799305,
-5.069343424147507,
5.603018248159094e-16
],
"target": [
0,
0,
0
],
"targetOrientation": [
0,
0,
0,
1
],
"fovy": 45,
"near": 0.5,
"far": 5000
},
"followMode": "follow-pose",
"scene": {
"enableStats": true,
"ignoreColladaUpAxis": false,
"syncCamera": false
},
"transforms": {},
"topics": {
"/lidar": {
"stixelsEnabled": false,
"visible": true,
"colorField": "z",
"colorMode": "colormap",
"colorMap": "turbo",
"pointShape": "square",
"pointSize": 10,
"explicitAlpha": 1,
"minValue": -0.1,
"decayTime": 0,
"cubeSize": 0.06
},
"/odom": {
"visible": true,
"axisScale": 1
},
"/video": {
"visible": false
},
"/global_map": {
"visible": true,
"colorField": "z",
"colorMode": "colormap",
"colorMap": "turbo",
"pointSize": 10,
"minValue": -0.1,
"decayTime": 0,
"pointShape": "square",
"cubeOutline": false,
"cubeSize": 0.05,
"gradient": [
"#06011dff",
"#d1e2e2ff"
],
"stixelsEnabled": false
}
},
"publish": {
"type": "point",
"poseTopic": "/move_base_simple/goal",
"pointTopic": "/clicked_point",
"poseEstimateTopic": "/estimate",
"poseEstimateXDeviation": 0.5,
"poseEstimateYDeviation": 0.5,
"poseEstimateThetaDeviation": 0.26179939
},
"imageMode": {},
"foxglovePanelTitle": "test"
},
"Image!3mnp456": {
"cameraState": {
"distance": 20,
"perspective": true,
"phi": 60,
"target": [
0,
0,
0
],
"targetOffset": [
0,
0,
0
],
"targetOrientation": [
0,
0,
0,
1
],
"thetaOffset": 45,
"fovy": 45,
"near": 0.5,
"far": 5000
},
"followMode": "follow-pose",
"scene": {
"enableStats": true
},
"transforms": {},
"topics": {},
"layers": {},
"publish": {
"type": "point",
"poseTopic": "/move_base_simple/goal",
"pointTopic": "/clicked_point",
"poseEstimateTopic": "/initialpose",
"poseEstimateXDeviation": 0.5,
"poseEstimateYDeviation": 0.5,
"poseEstimateThetaDeviation": 0.26179939
},
"imageMode": {
"imageTopic": "/video",
"colorMode": "gradient"
},
"foxglovePanelTitle": "/video"
},
"RawMessages!os6rgs": {
"diffEnabled": true,
"diffMethod": "previous message",
"diffTopicPath": "/lidar",
"showFullMessageForDiff": false,
"topicPath": "/odom",
"fontSize": 12
},
"Plot!a1gj37": {
"paths": [
{
"timestampMethod": "receiveTime",
"value": "/odom.pose.position.y",
"enabled": true,
"color": "#4e98e2"
},
{
"timestampMethod": "receiveTime",
"value": "/odom.pose.position.x",
"enabled": true,
"color": "#f5774d"
},
{
"timestampMethod": "receiveTime",
"value": "/odom.pose.position.z",
"enabled": true,
"color": "#f7df71"
}
],
"showXAxisLabels": true,
"showYAxisLabels": true,
"showLegend": true,
"legendDisplay": "floating",
"showPlotValuesInLegend": false,
"isSynced": true,
"xAxisVal": "timestamp",
"sidebarDimension": 240
}
},
"globalVariables": {},
"userNodes": {},
"playbackConfig": {
"speed": 1
},
"drawerConfig": {
"tracks": []
},
"layout": {
"first": "3D!18i6zy7",
"second": {
"first": "Image!3mnp456",
"second": {
"first": "RawMessages!os6rgs",
"second": "Plot!a1gj37",
"direction": "row",
"splitPercentage": 21.133036282622545
},
"direction": "column",
"splitPercentage": 45.83732057416268
},
"direction": "row",
"splitPercentage": 61.01509144891709
}
}
7 changes: 7 additions & 0 deletions bin/lcmspy
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
# current script dir + ..dimos


script_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"

python $script_dir/../dimos/utils/cli/lcmspy_cli.py "$@"
9 changes: 0 additions & 9 deletions dimos/core/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import time
from typing import Optional

import pytest
from dask.distributed import Client, LocalCluster
from rich.console import Console

Expand Down Expand Up @@ -82,14 +81,6 @@ def deploy(
return dask_client


@pytest.fixture
def dimos():
process_count = 3 # we chill
client = start(process_count)
yield client
stop(client)


def start(n: Optional[int] = None) -> Client:
console = Console()
if not n:
Expand Down
12 changes: 9 additions & 3 deletions dimos/core/test_core.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,19 +24,25 @@
Out,
RemoteOut,
ZenohTransport,
dimos,
pLCMTransport,
rpc,
start,
stop,
)
from dimos.robot.unitree_webrtc.type.lidar import LidarMessage
from dimos.robot.unitree_webrtc.type.odometry import Odometry
from dimos.types.vector import Vector
from dimos.utils.testing import SensorReplay

# never delete this line
if dimos:
...


@pytest.fixture
def dimos():
"""Fixture to create a Dimos client for testing."""
client = start(2)
yield client
stop(client)


class RobotClient(Module):
Expand Down
2 changes: 1 addition & 1 deletion dimos/msgs/geometry_msgs/PoseStamped.py
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class PoseStamped(Pose, Timestamped):
@dispatch
def __init__(self, *args, ts: float = 0, frame_id: str = "", **kwargs) -> None:
self.frame_id = frame_id
self.ts = ts if ts is not 0 else time.time()
self.ts = ts if ts != 0 else time.time()
super().__init__(*args, **kwargs)

def lcm_encode(self) -> bytes:
Expand Down
2 changes: 1 addition & 1 deletion dimos/protocol/rpc/spec.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ def call(*args, fname=fname):


class RPCServer(Protocol):
def serve(self, f: Callable, name: str) -> None: ...
def serve_rpc(self, f: Callable, name: str) -> None: ...


class RPC(RPCServer, RPCClient): ...
32 changes: 24 additions & 8 deletions dimos/protocol/service/lcmservice.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
import sys
import threading
import traceback
import os
from functools import cache
from dataclasses import dataclass
from typing import Any, Callable, Optional, Protocol, runtime_checkable

Expand All @@ -26,27 +28,39 @@
from dimos.protocol.service.spec import Service


@cache
def check_root() -> bool:
"""Return True if the current process is running as root (UID 0)."""
try:
return os.geteuid() == 0 # type: ignore[attr-defined]
except AttributeError:
# Platforms without geteuid (e.g. Windows) – assume non-root.
return False


def check_multicast() -> list[str]:
"""Check if multicast configuration is needed and return required commands."""
commands_needed = []

sudo = "" if check_root() else "sudo "

# Check if loopback interface has multicast enabled
try:
result = subprocess.run(["ip", "link", "show", "lo"], capture_output=True, text=True)
if "MULTICAST" not in result.stdout:
commands_needed.append("sudo ifconfig lo multicast")
commands_needed.append(f"{sudo}ifconfig lo multicast")
except Exception:
commands_needed.append("sudo ifconfig lo multicast")
commands_needed.append(f"{sudo}ifconfig lo multicast")

# Check if multicast route exists
try:
result = subprocess.run(
["ip", "route", "show", "224.0.0.0/4"], capture_output=True, text=True
)
if not result.stdout.strip():
commands_needed.append("sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo")
commands_needed.append(f"{sudo}route add -net 224.0.0.0 netmask 240.0.0.0 dev lo")
except Exception:
commands_needed.append("sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo")
commands_needed.append(f"{sudo}route add -net 224.0.0.0 netmask 240.0.0.0 dev lo")

return commands_needed

Expand All @@ -55,22 +69,24 @@ def check_buffers() -> list[str]:
"""Check if buffer configuration is needed and return required commands."""
commands_needed = []

sudo = "" if check_root() else "sudo "

# Check current buffer settings
try:
result = subprocess.run(["sysctl", "net.core.rmem_max"], capture_output=True, text=True)
current_max = int(result.stdout.split("=")[1].strip())
if current_max < 2097152:
commands_needed.append("sudo sysctl -w net.core.rmem_max=2097152")
commands_needed.append(f"{sudo}sysctl -w net.core.rmem_max=2097152")
except Exception:
commands_needed.append("sudo sysctl -w net.core.rmem_max=2097152")
commands_needed.append(f"{sudo}sysctl -w net.core.rmem_max=2097152")

try:
result = subprocess.run(["sysctl", "net.core.rmem_default"], capture_output=True, text=True)
current_default = int(result.stdout.split("=")[1].strip())
if current_default < 2097152:
commands_needed.append("sudo sysctl -w net.core.rmem_default=2097152")
commands_needed.append(f"{sudo}sysctl -w net.core.rmem_default=2097152")
except Exception:
commands_needed.append("sudo sysctl -w net.core.rmem_default=2097152")
commands_needed.append(f"{sudo}sysctl -w net.core.rmem_default=2097152")

return commands_needed

Expand Down
Loading