diff --git a/dimos/control/test_trajectory.py b/dimos/control/test_trajectory.py new file mode 100644 index 0000000000..46da4b4f8d --- /dev/null +++ b/dimos/control/test_trajectory.py @@ -0,0 +1,105 @@ +# Copyright 2026 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. + +import functools +from typing import TypedDict + +import pytest + +from dimos.core import LCMTransport +from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.mapping.pointclouds.occupancy import height_cost_occupancy +from dimos.mapping.voxels import VoxelGridMapper +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.utils.data import get_data +from dimos.utils.testing.replay import TimedSensorReplay + + +class Moment(TypedDict): + global_map: PointCloud2 + costmap: OccupancyGrid + navigation_costmap: OccupancyGrid + path: Path + + +@pytest.fixture(scope="session") +def get_moment(): + @functools.lru_cache(maxsize=1) + def moment_provider(seek=10.0) -> Moment: + data_dir = "unitree_go2_bigoffice" + get_data(data_dir) + + lidar_data = TimedSensorReplay(f"{data_dir}/lidar") + + voxels = VoxelGridMapper() + + for frame in lidar_data: + voxels.add_frame(frame) + + global_map = voxels.get_global_pointcloud2() + voxels.stop() + + costmap = height_cost_occupancy(global_map) + + cfg = GlobalConfig() + nav_map = NavigationMap(cfg) + nav_map.update(costmap) + navigation_costmap = nav_map.make_gradient_costmap() + + start = Vector3(-12.5, 12.5) + goal = PoseStamped(position=Vector3(-3, -12)) + + path = min_cost_astar(navigation_costmap, goal.position, start) + assert path is not None, f"No path found from {start} to {goal}" + path = smooth_resample_path(path, goal, spacing=0.1) + + return Moment( + global_map=global_map, + costmap=costmap, + navigation_costmap=navigation_costmap, + path=path, + ) + + return moment_provider + + +@pytest.fixture(scope="session") +def publish_moment(): + def publisher(moment: Moment) -> None: + for key, value in moment.items(): + t = LCMTransport(f"/{key}", type(value)) + t.publish(value) + t.lcm.stop() + + return publisher + + +def test_basic(get_moment, publish_moment): + """Test that we can create a Moment and publish it.""" + moment = get_moment() + print(moment) + assert "global_map" in moment + assert "costmap" in moment + assert "navigation_costmap" in moment + assert "path" in moment + assert len(moment["path"].poses) > 0 + print(moment.get("path")) + publish_moment(moment) diff --git a/dimos/robot/unitree_webrtc/__init__.py b/dimos/robot/unitree_webrtc/__init__.py index 451aa53128..65168dcbd3 100644 --- a/dimos/robot/unitree_webrtc/__init__.py +++ b/dimos/robot/unitree_webrtc/__init__.py @@ -12,20 +12,3 @@ # 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. - -"""Compatibility package for legacy dimos.robot.unitree_webrtc imports.""" - -from importlib import import_module -import sys - -_ALIAS_MODULES = { - "demo_error_on_name_conflicts": "dimos.robot.unitree.demo_error_on_name_conflicts", - "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", - "mujoco_connection": "dimos.robot.unitree.mujoco_connection", - "type": "dimos.robot.unitree.type", - "unitree_g1_skill_container": "dimos.robot.unitree.g1.skill_container", - "unitree_skill_container": "dimos.robot.unitree.unitree_skill_container", -} - -for alias, target in _ALIAS_MODULES.items(): - sys.modules[f"{__name__}.{alias}"] = import_module(target)