diff --git a/assets/policies/go1_policy.onnx b/assets/policies/go1_policy.onnx deleted file mode 100644 index af52536397..0000000000 --- a/assets/policies/go1_policy.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:386cde6a1eac679e2f2a313ade2b395d9f26905b151ee3b019c2c4163d49b6f2 -size 772916 diff --git a/assets/robots/go1/assets/hfield.png b/assets/robots/go1/assets/hfield.png deleted file mode 100644 index 62af27a2bc..0000000000 Binary files a/assets/robots/go1/assets/hfield.png and /dev/null differ diff --git a/assets/robots/go1/assets/rocky_texture.png b/assets/robots/go1/assets/rocky_texture.png deleted file mode 100644 index 1456b3ff47..0000000000 Binary files a/assets/robots/go1/assets/rocky_texture.png and /dev/null differ diff --git a/assets/robots/go1/robot.xml b/assets/robots/go1/robot.xml deleted file mode 100644 index 2b1251cbe7..0000000000 --- a/assets/robots/go1/robot.xml +++ /dev/null @@ -1,306 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bin/explore-cmd b/bin/explore-cmd new file mode 100755 index 0000000000..0f26d70182 --- /dev/null +++ b/bin/explore-cmd @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +""" +Simple command line tool to trigger robot exploration. + +Usage: + explore_cmd.py start # Start exploration + explore_cmd.py stop # Stop exploration +""" + +import sys +from dimos.protocol.pubsub.lcmpubsub import LCM, Topic +from dimos_lcm.std_msgs import Bool + + +def main(): + if len(sys.argv) != 2 or sys.argv[1] not in ["start", "stop"]: + print("Usage: bin/explore-cmd [start|stop]") + sys.exit(1) + + command = sys.argv[1] + + lcm = LCM() + + msg = Bool() + msg.data = True + + if command == "start": + topic = Topic("/explore_cmd", Bool) + lcm.publish(topic, msg) + print("✓ Sent start exploration command") + elif command == "stop": + topic = Topic("/stop_explore_cmd", Bool) + lcm.publish(topic, msg) + print("✓ Sent stop exploration command") + + +if __name__ == "__main__": + main() diff --git a/data/.lfs/mujoco_sim.tar.gz b/data/.lfs/mujoco_sim.tar.gz new file mode 100644 index 0000000000..6bfc95c831 --- /dev/null +++ b/data/.lfs/mujoco_sim.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3d607ce57127a6ac558f81ebb9c98bd23a71a86f9ffd5700b3389bf1a19ddf2 +size 59341859 diff --git a/dimos/agents/modules/base.py b/dimos/agents/modules/base.py index 4bebb52385..3917f865a5 100644 --- a/dimos/agents/modules/base.py +++ b/dimos/agents/modules/base.py @@ -76,6 +76,7 @@ def __init__( max_history: int = 20, rag_n: int = 4, rag_threshold: float = 0.45, + seed: Optional[int] = None, # Legacy compatibility dev_name: str = "BaseAgent", agent_type: str = "LLM", @@ -94,6 +95,7 @@ def __init__( max_history: Maximum conversation history to keep rag_n: Number of RAG results to fetch rag_threshold: Minimum similarity for RAG results + seed: Random seed for deterministic outputs (if supported by model) dev_name: Device/agent name for logging agent_type: Type of agent for logging """ @@ -105,6 +107,7 @@ def __init__( self._max_history = max_history self.rag_n = rag_n self.rag_threshold = rag_threshold + self.seed = seed self.dev_name = dev_name self.agent_type = agent_type @@ -224,15 +227,22 @@ async def _process_query_async(self, agent_msg: AgentMessage) -> AgentResponse: logger.debug(f"Tools available: {len(tools) if tools else 0}") logger.debug("======================") + # Prepare inference parameters + inference_params = { + "model": self.model, + "messages": messages, + "tools": tools, + "temperature": self.temperature, + "max_tokens": self.max_tokens, + "stream": False, + } + + # Add seed if provided + if self.seed is not None: + inference_params["seed"] = self.seed + # Make inference call - response = await self.gateway.ainference( - model=self.model, - messages=messages, - tools=tools, - temperature=self.temperature, - max_tokens=self.max_tokens, - stream=False, - ) + response = await self.gateway.ainference(**inference_params) # Extract response message = response["choices"][0]["message"] @@ -423,13 +433,20 @@ async def _handle_tool_calls( # Add tool results to messages messages.extend(tool_results) + # Prepare follow-up inference parameters + followup_params = { + "model": self.model, + "messages": messages, + "temperature": self.temperature, + "max_tokens": self.max_tokens, + } + + # Add seed if provided + if self.seed is not None: + followup_params["seed"] = self.seed + # Get follow-up response - response = await self.gateway.ainference( - model=self.model, - messages=messages, - temperature=self.temperature, - max_tokens=self.max_tokens, - ) + response = await self.gateway.ainference(**followup_params) # Extract final response final_message = response["choices"][0]["message"] diff --git a/dimos/agents/test_agent_image_message.py b/dimos/agents/test_agent_image_message.py index ff5193e95b..d29b22b4e2 100644 --- a/dimos/agents/test_agent_image_message.py +++ b/dimos/agents/test_agent_image_message.py @@ -44,6 +44,7 @@ def test_agent_single_image(): model="openai::gpt-4o-mini", system_prompt="You are a helpful vision assistant. Describe what you see concisely.", temperature=0.0, + seed=42, ) # Create AgentMessage with text and single image @@ -103,6 +104,7 @@ def test_agent_multiple_images(): model="openai::gpt-4o-mini", system_prompt="You are a helpful vision assistant that compares images.", temperature=0.0, + seed=42, ) # Create AgentMessage with multiple images @@ -169,6 +171,7 @@ def test_agent_image_with_context(): model="openai::gpt-4o-mini", system_prompt="You are a helpful vision assistant with good memory.", temperature=0.0, + seed=42, ) # First query with image @@ -216,6 +219,7 @@ def test_agent_mixed_content(): model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant that can see images when provided.", temperature=0.0, + seed=100, ) # Text-only query @@ -260,10 +264,10 @@ def test_agent_mixed_content(): # Another text-only query response3 = agent.query("What did I just show you?") - assert any( - word in response3.content.lower() - for word in ["office", "room", "hallway", "image", "scene"] - ) + words = ["office", "room", "hallway", "image", "scene"] + content = response3.content.lower() + + assert any(word in content for word in words), f"{content=}" # Check history structure assert agent.conversation.size() == 6 @@ -288,7 +292,10 @@ def test_agent_empty_image_message(): # Create agent agent = BaseAgent( - model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant.", temperature=0.0 + model="openai::gpt-4o-mini", + system_prompt="You are a helpful assistant.", + temperature=0.0, + seed=42, ) # AgentMessage with only images, no text @@ -335,6 +342,7 @@ def test_agent_non_vision_model_with_images(): model="openai::gpt-3.5-turbo", # This model doesn't support vision system_prompt="You are a helpful assistant.", temperature=0.0, + seed=42, ) # Try to send an image diff --git a/dimos/agents/test_agent_message_streams.py b/dimos/agents/test_agent_message_streams.py index 1a07b3fbcf..c41b657293 100644 --- a/dimos/agents/test_agent_message_streams.py +++ b/dimos/agents/test_agent_message_streams.py @@ -341,6 +341,7 @@ def test_agent_message_text_only(): model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant. Answer in 10 words or less.", temperature=0.0, + seed=42, ) # Test with text-only AgentMessage diff --git a/dimos/agents/test_agent_tools.py b/dimos/agents/test_agent_tools.py index 6f4a684c62..880625fe23 100644 --- a/dimos/agents/test_agent_tools.py +++ b/dimos/agents/test_agent_tools.py @@ -257,6 +257,7 @@ def test_base_agent_direct_tools(): skills=skill_library, temperature=0.0, memory=False, + seed=42, ) # Test calculation with explicit tool request diff --git a/dimos/agents/test_base_agent_text.py b/dimos/agents/test_base_agent_text.py index 14704a6330..f04e84e684 100644 --- a/dimos/agents/test_base_agent_text.py +++ b/dimos/agents/test_base_agent_text.py @@ -81,6 +81,7 @@ def test_base_agent_direct_text(): model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant. Answer in 10 words or less.", temperature=0.0, + seed=42, # Fixed seed for deterministic results ) # Test simple query with string (backward compatibility) @@ -125,7 +126,10 @@ async def test_base_agent_async_text(): # Create agent agent = BaseAgent( - model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant.", temperature=0.0 + model="openai::gpt-4o-mini", + system_prompt="You are a helpful assistant.", + temperature=0.0, + seed=42, ) # Test async query with string @@ -248,6 +252,7 @@ def test_base_agent_providers(model, provider): model=model, system_prompt="You are a helpful assistant. Answer in 10 words or less.", temperature=0.0, + seed=42, ) # Test query with AgentMessage @@ -274,6 +279,7 @@ def test_base_agent_memory(): system_prompt="You are a helpful assistant. Use the provided context when answering.", temperature=0.0, rag_threshold=0.3, + seed=42, ) # Add context to memory @@ -396,7 +402,10 @@ def test_base_agent_conversation_history(): # Create agent agent = BaseAgent( - model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant.", temperature=0.0 + model="openai::gpt-4o-mini", + system_prompt="You are a helpful assistant.", + temperature=0.0, + seed=42, ) # Test 1: Simple conversation @@ -477,6 +486,7 @@ def __call__(self) -> str: system_prompt="You are a helpful assistant with a calculator. Use the calculator tool when asked to compute something.", skills=skills, temperature=0.0, + seed=42, ) # Make a query that should trigger tool use diff --git a/dimos/agents/test_conversation_history.py b/dimos/agents/test_conversation_history.py index b14feb3469..2227944061 100644 --- a/dimos/agents/test_conversation_history.py +++ b/dimos/agents/test_conversation_history.py @@ -43,6 +43,7 @@ def test_conversation_history_basic(): model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant with perfect memory.", temperature=0.0, + seed=42, ) try: @@ -98,6 +99,7 @@ def test_conversation_history_with_images(): model="openai::gpt-4o-mini", system_prompt="You are a helpful vision assistant.", temperature=0.0, + seed=42, ) try: @@ -159,6 +161,7 @@ def test_conversation_history_trimming(): system_prompt="You are a helpful assistant.", temperature=0.0, max_history=3, # Keep 3 message pairs (6 messages total) + seed=42, ) try: @@ -232,6 +235,7 @@ class TestSkillLibrary(SkillLibrary): system_prompt="You are a helpful assistant with access to a calculator.", skills=TestSkillLibrary(), temperature=0.0, + seed=100, ) try: @@ -267,7 +271,7 @@ def test_conversation_thread_safety(): if not os.getenv("OPENAI_API_KEY"): pytest.skip("No OPENAI_API_KEY found") - agent = BaseAgent(model="openai::gpt-4o-mini", temperature=0.0) + agent = BaseAgent(model="openai::gpt-4o-mini", temperature=0.0, seed=42) try: @@ -300,7 +304,7 @@ def test_conversation_history_formats(): if not os.getenv("OPENAI_API_KEY"): pytest.skip("No OPENAI_API_KEY found") - agent = BaseAgent(model="openai::gpt-4o-mini", temperature=0.0) + agent = BaseAgent(model="openai::gpt-4o-mini", temperature=0.0, seed=42) try: # Create a conversation @@ -360,7 +364,10 @@ def test_conversation_edge_cases(): pytest.skip("No OPENAI_API_KEY found") agent = BaseAgent( - model="openai::gpt-4o-mini", system_prompt="You are a helpful assistant.", temperature=0.0 + model="openai::gpt-4o-mini", + system_prompt="You are a helpful assistant.", + temperature=0.0, + seed=42, ) try: diff --git a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py index 292b8e162d..9f94c7f79c 100644 --- a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py +++ b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py @@ -93,6 +93,8 @@ class WavefrontFrontierExplorer(Module): costmap: In[OccupancyGrid] = None odometry: In[PoseStamped] = None goal_reached: In[Bool] = None + explore_cmd: In[Bool] = None + stop_explore_cmd: In[Bool] = None # LCM outputs goal_request: Out[PoseStamped] = None @@ -159,6 +161,12 @@ def start(self): if self.goal_reached.transport is not None: self.goal_reached.subscribe(self._on_goal_reached) + # Subscribe to exploration commands + if self.explore_cmd.transport is not None: + self.explore_cmd.subscribe(self._on_explore_cmd) + if self.stop_explore_cmd.transport is not None: + self.stop_explore_cmd.subscribe(self._on_stop_explore_cmd) + logger.info("WavefrontFrontierExplorer started") @rpc @@ -180,6 +188,18 @@ def _on_goal_reached(self, msg: Bool): if msg.data: self.goal_reached_event.set() + def _on_explore_cmd(self, msg: Bool): + """Handle exploration command messages.""" + if msg.data: + logger.info("Received exploration start command via LCM") + self.explore() + + def _on_stop_explore_cmd(self, msg: Bool): + """Handle stop exploration command messages.""" + if msg.data: + logger.info("Received exploration stop command via LCM") + self.stop_exploration() + def _count_costmap_information(self, costmap: OccupancyGrid) -> int: """ Count the amount of information in a costmap (free space + obstacles). diff --git a/dimos/robot/unitree_webrtc/camera_module.py b/dimos/robot/unitree_webrtc/camera_module.py index beff3561ba..ef07c1be1f 100644 --- a/dimos/robot/unitree_webrtc/camera_module.py +++ b/dimos/robot/unitree_webrtc/camera_module.py @@ -98,6 +98,7 @@ def __init__( self._last_image = None self._last_timestamp = None self._last_depth = None + self._cannot_process_depth = False # Threading self._processing_thread: Optional[threading.Thread] = None @@ -182,6 +183,10 @@ def _main_processing_loop(self): def _process_depth(self, img_array: np.ndarray): """Process depth estimation using Metric3D.""" + if self._cannot_process_depth: + self._last_depth = None + return + try: logger.debug(f"Processing depth for image shape: {img_array.shape}") @@ -195,6 +200,7 @@ def _process_depth(self, img_array: np.ndarray): except Exception as e: logger.error(f"Error processing depth: {e}", exc_info=True) + self._cannot_process_depth = True def _publish_synchronized_data(self): """Publish all data synchronously.""" diff --git a/dimos/robot/unitree_webrtc/mujoco_connection.py b/dimos/robot/unitree_webrtc/mujoco_connection.py index 277977a8a6..7a840aa86d 100644 --- a/dimos/robot/unitree_webrtc/mujoco_connection.py +++ b/dimos/robot/unitree_webrtc/mujoco_connection.py @@ -26,6 +26,7 @@ from dimos.msgs.geometry_msgs import Vector3 from dimos.msgs.sensor_msgs import Image +from dimos.utils.data import get_data try: from dimos.simulation.mujoco.mujoco import MujocoThread @@ -43,6 +44,7 @@ class MujocoConnection: def __init__(self, *args, **kwargs): if MujocoThread is None: raise ImportError("'mujoco' is not installed. Use `pip install -e .[sim]`") + get_data("mujoco_sim") self.mujoco_thread = MujocoThread() self._stream_threads: List[threading.Thread] = [] self._stop_events: List[threading.Event] = [] diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 8e8460bd86..13f59ec20b 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -225,7 +225,7 @@ def __init__( output_dir: Directory for saving outputs (default: assets/output) websocket_port: Port for web visualization skill_library: Skill library instance - playback: If True, use recorded data instead of real robot connection + connection_type: webrtc, fake, or mujoco """ super().__init__() self.ip = ip @@ -347,6 +347,10 @@ def _deploy_navigation(self): "/goal_request", PoseStamped ) self.frontier_explorer.goal_reached.transport = core.LCMTransport("/goal_reached", Bool) + self.frontier_explorer.explore_cmd.transport = core.LCMTransport("/explore_cmd", Bool) + self.frontier_explorer.stop_explore_cmd.transport = core.LCMTransport( + "/stop_explore_cmd", Bool + ) self.global_planner.target.connect(self.navigator.goal) diff --git a/dimos/simulation/mujoco/depth_camera.py b/dimos/simulation/mujoco/depth_camera.py index 2920e9a140..3778d6f900 100644 --- a/dimos/simulation/mujoco/depth_camera.py +++ b/dimos/simulation/mujoco/depth_camera.py @@ -18,7 +18,9 @@ import numpy as np import open3d as o3d -RANGE_FINDER_MAX_RANGE = 4 +MAX_RANGE = 3 +MIN_RANGE = 0.2 +MAX_HEIGHT = 1.2 def depth_image_to_point_cloud( @@ -26,7 +28,6 @@ def depth_image_to_point_cloud( camera_pos: np.ndarray, camera_mat: np.ndarray, fov_degrees: float = 120, - min_range: float = 0.0, ) -> np.ndarray: """ Convert a depth image from a camera to a 3D point cloud using perspective projection. @@ -68,9 +69,12 @@ def depth_image_to_point_cloud( camera_points[:, 1] = -camera_points[:, 1] camera_points[:, 2] = -camera_points[:, 2] - # Filter points based on depth range (note: z is now negative) - valid_mask = (camera_points[:, 2] < -min_range) & ( - camera_points[:, 2] > -RANGE_FINDER_MAX_RANGE + # y (index 1) is up here + valid_mask = ( + (np.abs(camera_points[:, 0]) <= MAX_RANGE) + & (np.abs(camera_points[:, 1]) <= MAX_HEIGHT) + & (np.abs(camera_points[:, 2]) >= MIN_RANGE) + & (np.abs(camera_points[:, 2]) <= MAX_RANGE) ) camera_points = camera_points[valid_mask] diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index 465adbcea7..1543a80364 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -24,18 +24,14 @@ from dimos.simulation.mujoco.policy import OnnxController from dimos.simulation.mujoco.types import InputController -RANGE_FINDER_MAX_RANGE = 4 -RANGE_FINDER_MIN_RANGE = 0.2 -LIDAR_RESOLUTION = 0.05 -VIDEO_FREQUENCY = 30 -DEPTH_CAMERA_FOV = 160 - _HERE = epath.Path(__file__).parent def get_assets() -> dict[str, bytes]: + # Assets used from https://sketchfab.com/3d-models/mersus-office-8714be387bcd406898b2615f7dae3a47 + # Created by Ryan Cassidy and Coleman Costello assets: dict[str, bytes] = {} - assets_path = _HERE / "../../../assets/robots/go1" + assets_path = _HERE / "../../../data/mujoco_sim/go1" mjx_env.update_assets(assets, assets_path, "*.xml") mjx_env.update_assets(assets, assets_path / "assets") path = mjx_env.MENAGERIE_PATH / "unitree_go1" @@ -48,7 +44,7 @@ def load_model(input_device: InputController, model=None, data=None): mujoco.set_mjcb_control(None) model = mujoco.MjModel.from_xml_path( - (_HERE / "../../../assets/robots/go1/robot.xml").as_posix(), + (_HERE / "../../../data/mujoco_sim/go1/robot.xml").as_posix(), assets=get_assets(), ) data = mujoco.MjData(model) @@ -56,12 +52,12 @@ def load_model(input_device: InputController, model=None, data=None): mujoco.mj_resetDataKeyframe(model, data, 0) ctrl_dt = 0.02 - sim_dt = 0.004 + sim_dt = 0.01 n_substeps = int(round(ctrl_dt / sim_dt)) model.opt.timestep = sim_dt policy = OnnxController( - policy_path=(_HERE / "../../../assets/policies/go1_policy.onnx").as_posix(), + policy_path=(_HERE / "../../../data/mujoco_sim/go1/go1_policy.onnx").as_posix(), default_angles=np.array(model.keyframe("home").qpos[7:]), n_substeps=n_substeps, action_scale=0.5, diff --git a/dimos/simulation/mujoco/mujoco.py b/dimos/simulation/mujoco/mujoco.py index e7cc096983..4c87f0aacc 100644 --- a/dimos/simulation/mujoco/mujoco.py +++ b/dimos/simulation/mujoco/mujoco.py @@ -32,12 +32,11 @@ from dimos.simulation.mujoco.depth_camera import depth_image_to_point_cloud from dimos.simulation.mujoco.model import load_model -RANGE_FINDER_MAX_RANGE = 4 -RANGE_FINDER_MIN_RANGE = 0.2 LIDAR_RESOLUTION = 0.05 -VIDEO_FREQUENCY = 30 DEPTH_CAMERA_FOV = 160 -STEPS_PER_FRAME = 7 # Adjust this number to control physics speed +STEPS_PER_FRAME = 2 +VIDEO_FPS = 20 +LIDAR_FPS = 4 logger = logging.getLogger(__name__) @@ -96,36 +95,46 @@ def run_simulation(self): with viewer.launch_passive(self.model, self.data) as m_viewer: self._viewer = m_viewer - window_size = (320, 240) + camera_size = (320, 240) # Create separate renderers for RGB and depth self._rgb_renderer = mujoco.Renderer( - self.model, height=window_size[1], width=window_size[0] + self.model, height=camera_size[1], width=camera_size[0] ) self._depth_renderer = mujoco.Renderer( - self.model, height=window_size[1], width=window_size[0] + self.model, height=camera_size[1], width=camera_size[0] ) # Enable depth rendering only for depth renderer self._depth_renderer.enable_depth_rendering() # Create renderers for left and right depth cameras self._depth_left_renderer = mujoco.Renderer( - self.model, height=window_size[1], width=window_size[0] + self.model, height=camera_size[1], width=camera_size[0] ) self._depth_left_renderer.enable_depth_rendering() self._depth_right_renderer = mujoco.Renderer( - self.model, height=window_size[1], width=window_size[0] + self.model, height=camera_size[1], width=camera_size[0] ) self._depth_right_renderer.enable_depth_rendering() scene_option = mujoco.MjvOption() + # Timing control variables + last_video_time = 0 + last_lidar_time = 0 + video_interval = 1.0 / VIDEO_FPS + lidar_interval = 1.0 / LIDAR_FPS + while m_viewer.is_running() and self._is_running: - # Step multiple times per frame to speed up physics + step_start = time.time() + for _ in range(STEPS_PER_FRAME): mujoco.mj_step(self.model, self.data) + m_viewer.sync() + + # Odometry happens every loop with self.odom_lock: # base position pos = self.data.qpos[0:3] @@ -133,44 +142,56 @@ def run_simulation(self): quat = self.data.qpos[3:7] # (w, x, y, z) self.odom_data = (pos.copy(), quat.copy()) - # Render regular camera for video (RGB) - self._rgb_renderer.update_scene( - self.data, camera=camera_id, scene_option=scene_option - ) - pixels = self._rgb_renderer.render() + current_time = time.time() - with self.pixels_lock: - self.shared_pixels = pixels.copy() + # Video rendering + if current_time - last_video_time >= video_interval: + self._rgb_renderer.update_scene( + self.data, camera=camera_id, scene_option=scene_option + ) + pixels = self._rgb_renderer.render() - # Render fisheye camera for depth/lidar data - self._depth_renderer.update_scene( - self.data, camera=lidar_camera_id, scene_option=scene_option - ) - # When depth rendering is enabled, render() returns depth as float array in meters - depth = self._depth_renderer.render() + with self.pixels_lock: + self.shared_pixels = pixels.copy() - with self.depth_lock_front: - self.shared_depth_front = depth.copy() + last_video_time = current_time - # Render left depth camera - self._depth_left_renderer.update_scene( - self.data, camera=lidar_left_camera_id, scene_option=scene_option - ) - depth_left = self._depth_left_renderer.render() + # Lidar rendering + if current_time - last_lidar_time >= lidar_interval: + # Render fisheye camera for depth/lidar data + self._depth_renderer.update_scene( + self.data, camera=lidar_camera_id, scene_option=scene_option + ) + # When depth rendering is enabled, render() returns depth as float array in meters + depth = self._depth_renderer.render() - with self.depth_left_lock: - self.shared_depth_left = depth_left.copy() + with self.depth_lock_front: + self.shared_depth_front = depth.copy() - # Render right depth camera - self._depth_right_renderer.update_scene( - self.data, camera=lidar_right_camera_id, scene_option=scene_option - ) - depth_right = self._depth_right_renderer.render() + # Render left depth camera + self._depth_left_renderer.update_scene( + self.data, camera=lidar_left_camera_id, scene_option=scene_option + ) + depth_left = self._depth_left_renderer.render() - with self.depth_right_lock: - self.shared_depth_right = depth_right.copy() + with self.depth_left_lock: + self.shared_depth_left = depth_left.copy() - m_viewer.sync() + # Render right depth camera + self._depth_right_renderer.update_scene( + self.data, camera=lidar_right_camera_id, scene_option=scene_option + ) + depth_right = self._depth_right_renderer.render() + + with self.depth_right_lock: + self.shared_depth_right = depth_right.copy() + + last_lidar_time = current_time + + # Control the simulation speed + time_until_next_step = self.model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) def _process_depth_camera(self, camera_name: str, depth_data, depth_lock) -> np.ndarray | None: """Process a single depth camera and return point cloud points.""" @@ -190,7 +211,6 @@ def _process_depth_camera(self, camera_name: str, depth_data, depth_lock) -> np. camera_pos, camera_mat, fov_degrees=DEPTH_CAMERA_FOV, - min_range=RANGE_FINDER_MIN_RANGE, ) return points if points.size > 0 else None