From 6cd48bfa765fd034fe1aa0f1fdb05f9a8ab06670 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 27 May 2025 22:02:58 +0300 Subject: [PATCH 01/72] json cleanup, tests reformat --- .gitignore | 4 +- dimos/utils/test_testing.py | 9 +- dimos/utils/testing.py | 9 +- dimos/web/dimos_interface/tsconfig.json | 22 +- dimos/web/dimos_interface/tsconfig.node.json | 4 +- dimos/web/websocket_vis/deno.json | 42 ++-- tests/agent_manip_flow_fastapi_test.py | 31 ++- tests/agent_manip_flow_flask_test.py | 39 ++-- tests/agent_memory_test.py | 2 +- tests/genesissim/stream_camera.py | 31 +-- tests/isaacsim/stream_camera.py | 12 +- tests/run.py | 57 +++-- tests/run_go2_ros.py | 52 ++--- tests/simple_agent_test.py | 18 +- tests/test_agent.py | 4 + tests/test_agent_alibaba.py | 28 +-- tests/test_agent_ctransformers_gguf.py | 4 +- tests/test_agent_huggingface_local.py | 10 +- tests/test_agent_huggingface_local_jetson.py | 8 +- tests/test_agent_huggingface_remote.py | 24 +- tests/test_audio_agent.py | 1 - tests/test_claude_agent_query.py | 7 +- tests/test_claude_agent_skills_query.py | 25 ++- tests/test_command_pose_unitree.py | 13 +- tests/test_header.py | 12 +- tests/test_huggingface_llm_agent.py | 14 +- tests/test_move_vel_unitree.py | 9 +- tests/test_navigate_to_object_robot.py | 79 ++++--- tests/test_navigation_skills.py | 64 +++--- ...bject_detection_agent_data_query_stream.py | 78 ++++--- tests/test_object_detection_stream.py | 115 +++++----- tests/test_object_tracking_webcam.py | 110 ++++++---- tests/test_object_tracking_with_qwen.py | 86 +++++--- tests/test_observe_stream_skill.py | 71 +++--- tests/test_person_following_robot.py | 53 ++--- tests/test_person_following_webcam.py | 117 +++++----- tests/test_planning_agent_web_interface.py | 54 +++-- tests/test_planning_robot_agent.py | 32 +-- tests/test_qwen_image_query.py | 18 +- tests/test_robot.py | 36 ++- tests/test_rtsp_video_provider.py | 53 ++--- tests/test_semantic_seg_robot.py | 53 +++-- tests/test_semantic_seg_robot_agent.py | 68 +++--- tests/test_semantic_seg_webcam.py | 57 ++--- tests/test_skills.py | 61 +++--- tests/test_skills_rest.py | 15 +- tests/test_spatial_memory.py | 180 ++++++++------- tests/test_spatial_memory_query.py | 207 ++++++++++-------- tests/test_standalone_chromadb.py | 12 +- tests/test_standalone_fastapi.py | 28 ++- tests/test_standalone_hugging_face.py | 16 +- tests/test_standalone_openai_json.py | 26 ++- tests/test_standalone_openai_json_struct.py | 14 +- ...test_standalone_openai_json_struct_func.py | 69 +++--- ...lone_openai_json_struct_func_playground.py | 24 +- tests/test_standalone_project_out.py | 138 ++++++------ tests/test_standalone_rxpy_01.py | 19 +- tests/test_unitree_agent.py | 38 ++-- tests/test_unitree_agent_queries_fastapi.py | 27 ++- tests/test_webrtc_queue.py | 72 +++--- tests/test_websocketvis.py | 28 +-- 61 files changed, 1408 insertions(+), 1201 deletions(-) diff --git a/.gitignore b/.gitignore index 47d636fc28..2439af14ed 100644 --- a/.gitignore +++ b/.gitignore @@ -22,5 +22,7 @@ assets/agent/memory.txt tests/data/* !tests/data/.lfs/ -# node modules (for dev tooling) +# node env (used by devcontainers cli) node_modules +package.json +package-lock.json diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index e134492952..8952782168 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -43,11 +43,16 @@ def test_pull_file(): # validate hashes with test_file_compressed.open("rb") as f: compressed_sha256 = hashlib.sha256(f.read()).hexdigest() - assert compressed_sha256 == "cdfd708d66e6dd5072ed7636fc10fb97754f8d14e3acd6c3553663e27fc96065" + assert ( + compressed_sha256 == "cdfd708d66e6dd5072ed7636fc10fb97754f8d14e3acd6c3553663e27fc96065" + ) with test_file_decompressed.open("rb") as f: decompressed_sha256 = hashlib.sha256(f.read()).hexdigest() - assert decompressed_sha256 == "55d451dde49b05e3ad386fdd4ae9e9378884b8905bff1ca8aaea7d039ff42ddd" + assert ( + decompressed_sha256 + == "55d451dde49b05e3ad386fdd4ae9e9378884b8905bff1ca8aaea7d039ff42ddd" + ) def test_pull_dir(): diff --git a/dimos/utils/testing.py b/dimos/utils/testing.py index a37e1804b9..53b9849718 100644 --- a/dimos/utils/testing.py +++ b/dimos/utils/testing.py @@ -19,7 +19,9 @@ def _check_git_lfs_available() -> None: @cache def _get_repo_root() -> Path: try: - result = subprocess.run(["git", "rev-parse", "--show-toplevel"], capture_output=True, check=True, text=True) + result = subprocess.run( + ["git", "rev-parse", "--show-toplevel"], capture_output=True, check=True, text=True + ) return Path(result.stdout.strip()) except subprocess.CalledProcessError: raise RuntimeError("Not in a Git repository") @@ -54,7 +56,10 @@ def _lfs_pull(file_path: Path, repo_root: Path) -> None: relative_path = file_path.relative_to(repo_root) subprocess.run( - ["git", "lfs", "pull", "--include", str(relative_path)], cwd=repo_root, check=True, capture_output=True + ["git", "lfs", "pull", "--include", str(relative_path)], + cwd=repo_root, + check=True, + capture_output=True, ) except subprocess.CalledProcessError as e: raise RuntimeError(f"Failed to pull LFS file {file_path}: {e}") diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 772ce46b79..4bf29f39d2 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -5,17 +5,21 @@ "useDefineForClassFields": true, "module": "ESNext", "resolveJsonModule": true, - /** - * Typecheck JS in `.svelte` and `.js` files by default. - * Disable checkJs if you'd like to use dynamic types in JS. - * Note that setting allowJs false does not prevent the use - * of JS in `.svelte` files. - */ "allowJs": true, "checkJs": true, "isolatedModules": true, - "types": ["node"] + "types": [ + "node" + ] }, - "include": ["src/**/*.ts", "src/**/*.js", "src/**/*.svelte"], - "references": [{ "path": "./tsconfig.node.json" }] + "include": [ + "src/**/*.ts", + "src/**/*.js", + "src/**/*.svelte" + ], + "references": [ + { + "path": "./tsconfig.node.json" + } + ] } diff --git a/dimos/web/dimos_interface/tsconfig.node.json b/dimos/web/dimos_interface/tsconfig.node.json index 494bfe0835..ad883d0eb4 100644 --- a/dimos/web/dimos_interface/tsconfig.node.json +++ b/dimos/web/dimos_interface/tsconfig.node.json @@ -5,5 +5,7 @@ "module": "ESNext", "moduleResolution": "bundler" }, - "include": ["vite.config.ts"] + "include": [ + "vite.config.ts" + ] } diff --git a/dimos/web/websocket_vis/deno.json b/dimos/web/websocket_vis/deno.json index 401e578474..453c7a4a80 100644 --- a/dimos/web/websocket_vis/deno.json +++ b/dimos/web/websocket_vis/deno.json @@ -1,22 +1,26 @@ { - "nodeModulesDir": "auto", - "tasks": { - "build": "deno run -A build.ts", - "watch": "deno run -A build.ts --watch" - }, - "lint": { - "rules": { - "exclude": ["require-await", "ban-ts-comment"] - } - }, - "fmt": { - "indentWidth": 4, - "useTabs": false, - "semiColons": false - }, - - "compilerOptions": { - "lib": ["dom", "deno.ns"] + "nodeModulesDir": "auto", + "tasks": { + "build": "deno run -A build.ts", + "watch": "deno run -A build.ts --watch" + }, + "lint": { + "rules": { + "exclude": [ + "require-await", + "ban-ts-comment" + ] } + }, + "fmt": { + "indentWidth": 4, + "useTabs": false, + "semiColons": false + }, + "compilerOptions": { + "lib": [ + "dom", + "deno.ns" + ] + } } - diff --git a/tests/agent_manip_flow_fastapi_test.py b/tests/agent_manip_flow_fastapi_test.py index d9bc9ef2f9..d802dd5663 100644 --- a/tests/agent_manip_flow_fastapi_test.py +++ b/tests/agent_manip_flow_fastapi_test.py @@ -19,7 +19,7 @@ from reactivex.scheduler import ThreadPoolScheduler, CurrentThreadScheduler, ImmediateScheduler # Local application imports -from dimos.agents.agent import OpenAIAgent +from dimos.agents.agent import OpenAIAgent from dimos.stream.frame_processor import FrameProcessor from dimos.stream.video_operators import VideoOperators as vops from dimos.stream.video_provider import VideoProvider @@ -28,6 +28,7 @@ # Load environment variables load_dotenv() + def main(): """ Initializes and runs the video processing pipeline with web server output. @@ -42,7 +43,9 @@ def main(): """ disposables = CompositeDisposable() - processor = FrameProcessor(output_dir=f"{os.getcwd()}/assets/output/frames", delete_on_init=True) + processor = FrameProcessor( + output_dir=f"{os.getcwd()}/assets/output/frames", delete_on_init=True + ) optimal_thread_count = multiprocessing.cpu_count() # Gets number of CPU cores thread_pool_scheduler = ThreadPoolScheduler(optimal_thread_count) @@ -53,14 +56,16 @@ def main(): f"{os.getcwd()}/assets/trimmed_video_480p.mov", f"{os.getcwd()}/assets/video-f30-480p.mp4", "rtsp://192.168.50.207:8080/h264.sdp", - "rtsp://10.0.0.106:8080/h264.sdp" + "rtsp://10.0.0.106:8080/h264.sdp", ] VIDEO_SOURCE_INDEX = 3 VIDEO_SOURCE_INDEX_2 = 2 my_video_provider = VideoProvider("Video File", video_source=VIDEO_SOURCES[VIDEO_SOURCE_INDEX]) - my_video_provider_2 = VideoProvider("Video File 2", video_source=VIDEO_SOURCES[VIDEO_SOURCE_INDEX_2]) + my_video_provider_2 = VideoProvider( + "Video File 2", video_source=VIDEO_SOURCES[VIDEO_SOURCE_INDEX_2] + ) video_stream_obs = my_video_provider.capture_video_as_observable(fps=120).pipe( ops.subscribe_on(thread_pool_scheduler), @@ -86,37 +91,39 @@ def main(): vops.with_jpeg_export(processor, suffix="edge"), ) - optical_flow_relevancy_stream_obs = processor.process_stream_optical_flow_with_relevancy(video_stream_obs) + optical_flow_relevancy_stream_obs = processor.process_stream_optical_flow_with_relevancy( + video_stream_obs + ) optical_flow_stream_obs = optical_flow_relevancy_stream_obs.pipe( ops.do_action(lambda result: print(f"Optical Flow Relevancy Score: {result[1]}")), vops.with_optical_flow_filtering(threshold=2.0), ops.do_action(lambda _: print(f"Optical Flow Passed Threshold.")), - vops.with_jpeg_export(processor, suffix="optical") + vops.with_jpeg_export(processor, suffix="optical"), ) # - # ====== Agent Orchastrator (Qu.s Awareness, Temporality, Routing) ====== + # ====== Agent Orchastrator (Qu.s Awareness, Temporality, Routing) ====== # # Agent 1 # my_agent = OpenAIAgent( - # "Agent 1", + # "Agent 1", # query="You are a robot. What do you see? Put a JSON with objects of what you see in the format {object, description}.") # my_agent.subscribe_to_image_processing(slowed_video_stream_obs) # disposables.add(my_agent.disposables) # # Agent 2 # my_agent_two = OpenAIAgent( - # "Agent 2", + # "Agent 2", # query="This is a visualization of dense optical flow. What movement(s) have occured? Put a JSON with mapped directions you see in the format {direction, probability, english_description}.") # my_agent_two.subscribe_to_image_processing(optical_flow_stream_obs) # disposables.add(my_agent_two.disposables) # - # ====== Create and start the FastAPI server ====== + # ====== Create and start the FastAPI server ====== # - + # Will be visible at http://[host]:[port]/video_feed/[key] streams = { "video_one": video_stream_obs, @@ -127,6 +134,6 @@ def main(): fast_api_server = FastAPIServer(port=5555, **streams) fast_api_server.run() + if __name__ == "__main__": main() - diff --git a/tests/agent_manip_flow_flask_test.py b/tests/agent_manip_flow_flask_test.py index e7cf0cfb61..aecf4049a5 100644 --- a/tests/agent_manip_flow_flask_test.py +++ b/tests/agent_manip_flow_flask_test.py @@ -20,7 +20,7 @@ from reactivex.scheduler import ThreadPoolScheduler, CurrentThreadScheduler, ImmediateScheduler # Local application imports -from dimos.agents.agent import PromptBuilder, OpenAIAgent +from dimos.agents.agent import PromptBuilder, OpenAIAgent from dimos.stream.frame_processor import FrameProcessor from dimos.stream.video_operators import VideoOperators as vops from dimos.stream.video_provider import VideoProvider @@ -31,6 +31,7 @@ app = Flask(__name__) + def main(): """ Initializes and runs the video processing pipeline with web server output. @@ -45,7 +46,9 @@ def main(): """ disposables = CompositeDisposable() - processor = FrameProcessor(output_dir=f"{os.getcwd()}/assets/output/frames", delete_on_init=True) + processor = FrameProcessor( + output_dir=f"{os.getcwd()}/assets/output/frames", delete_on_init=True + ) optimal_thread_count = multiprocessing.cpu_count() # Gets number of CPU cores thread_pool_scheduler = ThreadPoolScheduler(optimal_thread_count) @@ -58,7 +61,7 @@ def main(): f"{os.getcwd()}/assets/video.mov", "rtsp://192.168.50.207:8080/h264.sdp", "rtsp://10.0.0.106:8080/h264.sdp", - f"{os.getcwd()}/assets/people_1080p_24fps.mp4" + f"{os.getcwd()}/assets/people_1080p_24fps.mp4", ] VIDEO_SOURCE_INDEX = 4 @@ -85,28 +88,28 @@ def main(): # ops.do_action(lambda result: print(f"Optical Flow Relevancy Score: {result[1]}")), # vops.with_optical_flow_filtering(threshold=2.0), # ops.do_action(lambda _: print(f"Optical Flow Passed Threshold.")), - #vops.with_jpeg_export(processor, suffix="optical") + # vops.with_jpeg_export(processor, suffix="optical") ) # - # ====== Agent Orchastrator (Qu.s Awareness, Temporality, Routing) ====== + # ====== Agent Orchastrator (Qu.s Awareness, Temporality, Routing) ====== # # Observable that emits every 2 seconds secondly_emission = interval(2, scheduler=thread_pool_scheduler).pipe( - ops.map(lambda x: f"Second {x+1}"), + ops.map(lambda x: f"Second {x + 1}"), # ops.take(30) ) # Agent 1 my_agent = OpenAIAgent( - "Agent 1", + "Agent 1", query="You are a robot. What do you see? Put a JSON with objects of what you see in the format {object, description}.", - json_mode=False + json_mode=False, ) - - # Create an agent for each subset of questions that it would be theroized to handle. - # Set std. template/blueprints, and devs will add to that likely. + + # Create an agent for each subset of questions that it would be theroized to handle. + # Set std. template/blueprints, and devs will add to that likely. ai_1_obs = video_stream_obs.pipe( # vops.with_fps_sampling(fps=30), @@ -118,20 +121,20 @@ def main(): ai_1_obs.connect() ai_1_repeat_obs = ai_1_obs.pipe(ops.repeat()) - + my_agent.subscribe_to_image_processing(ai_1_obs) disposables.add(my_agent.disposables) # Agent 2 my_agent_two = OpenAIAgent( - "Agent 2", + "Agent 2", query="This is a visualization of dense optical flow. What movement(s) have occured? Put a JSON with mapped directions you see in the format {direction, probability, english_description}.", max_input_tokens_per_request=1000, max_output_tokens_per_request=300, json_mode=False, model_name="gpt-4o-2024-08-06", ) - + ai_2_obs = optical_flow_stream_obs.pipe( # vops.with_fps_sampling(fps=30), # ops.throttle_first(1), @@ -143,7 +146,6 @@ def main(): ai_2_repeat_obs = ai_2_obs.pipe(ops.repeat()) - # Combine emissions using zip ai_1_secondly_repeating_obs = zip(secondly_emission, ai_1_repeat_obs).pipe( # ops.do_action(lambda s: print(f"AI 1 - Emission Count: {s[0]}")), @@ -156,12 +158,11 @@ def main(): ops.map(lambda r: r[1]), ) - my_agent_two.subscribe_to_image_processing(ai_2_obs) disposables.add(my_agent_two.disposables) # - # ====== Create and start the Flask server ====== + # ====== Create and start the Flask server ====== # # Will be visible at http://[host]:[port]/video_feed/[key] @@ -172,9 +173,9 @@ def main(): OpenAIAgent_1=ai_1_secondly_repeating_obs, OpenAIAgent_2=ai_2_secondly_repeating_obs, ) - + flask_server.run(threaded=True) + if __name__ == "__main__": main() - diff --git a/tests/agent_memory_test.py b/tests/agent_memory_test.py index 5ebe40e5d7..e77cbc2821 100644 --- a/tests/agent_memory_test.py +++ b/tests/agent_memory_test.py @@ -44,4 +44,4 @@ results = agent_memory.query("Colors", n_results=19, similarity_threshold=0.45) print(results) -print("Done querying agent memory (n_results=19, similarity_threshold=0.45).") \ No newline at end of file +print("Done querying agent memory (n_results=19, similarity_threshold=0.45).") diff --git a/tests/genesissim/stream_camera.py b/tests/genesissim/stream_camera.py index c7a439232f..4a820851cf 100644 --- a/tests/genesissim/stream_camera.py +++ b/tests/genesissim/stream_camera.py @@ -1,27 +1,19 @@ import os from dimos.simulation.genesis import GenesisSimulator, GenesisStream + def main(): # Add multiple entities at once entities = [ - { - 'type': 'primitive', - 'params': {'shape': 'plane'} - }, - { - 'type': 'mjcf', - 'path': 'xml/franka_emika_panda/panda.xml' - } + {"type": "primitive", "params": {"shape": "plane"}}, + {"type": "mjcf", "path": "xml/franka_emika_panda/panda.xml"}, ] # Initialize simulator - sim = GenesisSimulator( - headless=True, - entities=entities - ) + sim = GenesisSimulator(headless=True, entities=entities) # You can also add entity individually - sim.add_entity('primitive', shape='box', size=[0.5, 0.5, 0.5], pos=[0, 1, 0.5]) - + sim.add_entity("primitive", shape="box", size=[0.5, 0.5, 0.5], pos=[0, 1, 0.5]) + # Create stream with custom settings stream = GenesisStream( simulator=sim, @@ -29,11 +21,11 @@ def main(): height=960, fps=60, camera_path="/camera", # Genesis uses simpler camera paths - annotator_type='rgb', # Can be 'rgb' or 'normals' - transport='tcp', - rtsp_url="rtsp://mediamtx:8554/stream" + annotator_type="rgb", # Can be 'rgb' or 'normals' + transport="tcp", + rtsp_url="rtsp://mediamtx:8554/stream", ) - + # Start streaming try: stream.stream() @@ -45,5 +37,6 @@ def main(): finally: sim.close() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/isaacsim/stream_camera.py b/tests/isaacsim/stream_camera.py index 31952dc671..a6eecd93d9 100644 --- a/tests/isaacsim/stream_camera.py +++ b/tests/isaacsim/stream_camera.py @@ -2,10 +2,11 @@ from dimos.simulation.isaac import IsaacSimulator from dimos.simulation.isaac import IsaacStream + def main(): # Initialize simulator sim = IsaacSimulator(headless=True) - + # Create stream with custom settings stream = IsaacStream( simulator=sim, @@ -13,14 +14,15 @@ def main(): height=1080, fps=60, camera_path="/World/alfred_parent_prim/alfred_base_descr/chest_cam_rgb_camera_frame/chest_cam", - annotator_type='rgb', - transport='tcp', + annotator_type="rgb", + transport="tcp", rtsp_url="rtsp://mediamtx:8554/stream", - usd_path=f"{os.getcwd()}/assets/TestSim3.usda" + usd_path=f"{os.getcwd()}/assets/TestSim3.usda", ) - + # Start streaming stream.stream() + if __name__ == "__main__": main() diff --git a/tests/run.py b/tests/run.py index d62c3a1103..4c4bfc036e 100644 --- a/tests/run.py +++ b/tests/run.py @@ -43,21 +43,31 @@ # Allow command line arguments to control spatial memory parameters import argparse + def parse_arguments(): - parser = argparse.ArgumentParser(description='Run the robot with optional spatial memory parameters') - parser.add_argument('--new-memory', action='store_true', help='Create a new spatial memory from scratch') - parser.add_argument('--spatial-memory-dir', type=str, help='Directory for storing spatial memory data') + parser = argparse.ArgumentParser( + description="Run the robot with optional spatial memory parameters" + ) + parser.add_argument( + "--new-memory", action="store_true", help="Create a new spatial memory from scratch" + ) + parser.add_argument( + "--spatial-memory-dir", type=str, help="Directory for storing spatial memory data" + ) return parser.parse_args() + args = parse_arguments() # Initialize robot with spatial memory parameters -robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - skills=MyUnitreeSkills(), - mock_connection=False, - spatial_memory_dir=args.spatial_memory_dir, # Will use default if None - new_memory=args.new_memory, # Create a new memory if specified - mode = "ai") +robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), + skills=MyUnitreeSkills(), + mock_connection=False, + spatial_memory_dir=args.spatial_memory_dir, # Will use default if None + new_memory=args.new_memory, # Create a new memory if specified + mode="ai", +) # Create a subject for agent responses agent_response_subject = rx.subject.Subject() @@ -79,7 +89,7 @@ def parse_arguments(): class_filter=class_filter, transform_to_map=robot.ros_control.transform_pose, detector=detector, - video_stream=video_stream + video_stream=video_stream, ) # Create visualization stream for web interface @@ -94,12 +104,13 @@ def parse_arguments(): ops.filter(lambda x: x is not None) ) + # Create a direct mapping that combines detection data with locations def combine_with_locations(object_detections): # Get locations from spatial memory try: locations = robot.get_spatial_memory().get_robot_locations() - + # Format the locations section locations_text = "\n\nSaved Robot Locations:\n" if locations: @@ -108,22 +119,22 @@ def combine_with_locations(object_detections): locations_text += f"Rotation ({loc.rotation[0]:.2f}, {loc.rotation[1]:.2f}, {loc.rotation[2]:.2f})\n" else: locations_text += "None\n" - + # Simply concatenate the strings return object_detections + locations_text except Exception as e: print(f"Error adding locations: {e}") return object_detections + # Create the combined stream with a simple pipe operation -enhanced_data_stream = formatted_detection_stream.pipe( - ops.map(combine_with_locations), - ops.share() -) +enhanced_data_stream = formatted_detection_stream.pipe(ops.map(combine_with_locations), ops.share()) -streams = {"unitree_video": robot.get_ros_video_stream(), - "local_planner_viz": local_planner_viz_stream, - "object_detection": viz_stream} +streams = { + "unitree_video": robot.get_ros_video_stream(), + "local_planner_viz": local_planner_viz_stream, + "object_detection": viz_stream, +} text_streams = { "agent_responses": agent_response_stream, } @@ -145,7 +156,7 @@ def combine_with_locations(object_detections): skills=robot.get_skills(), system_query="What do you see", model_name="claude-3-7-sonnet-latest", - thinking_budget_tokens=0 + thinking_budget_tokens=0, ) # tts_node = tts() @@ -168,11 +179,9 @@ def combine_with_locations(object_detections): robot_skills.create_instance("Speak", tts_node=tts_node) # Subscribe to agent responses and send them to the subject -agent.get_response_observable().subscribe( - lambda x: agent_response_subject.on_next(x) -) +agent.get_response_observable().subscribe(lambda x: agent_response_subject.on_next(x)) print("ObserveStream and Kill skills registered and ready for use") print("Created memory.txt file") -web_interface.run() \ No newline at end of file +web_interface.run() diff --git a/tests/run_go2_ros.py b/tests/run_go2_ros.py index 459fa54b0c..2fcc74dcb1 100644 --- a/tests/run_go2_ros.py +++ b/tests/run_go2_ros.py @@ -6,10 +6,11 @@ from dimos.robot.unitree.unitree_go2 import UnitreeGo2, WebRTCConnectionMethod from dimos.robot.unitree.unitree_ros_control import UnitreeROSControl + def get_env_var(var_name, default=None, required=False): """Get environment variable with validation.""" value = os.getenv(var_name, default) - if value == '': + if value == "": value = default if required and not value: raise ValueError(f"{var_name} environment variable is required") @@ -21,8 +22,7 @@ def get_env_var(var_name, default=None, required=False): robot_ip = get_env_var("ROBOT_IP") connection_method = get_env_var("CONNECTION_METHOD", "LocalSTA") serial_number = get_env_var("SERIAL_NUMBER", None) - output_dir = get_env_var("ROS_OUTPUT_DIR", - os.path.join(os.getcwd(), "assets/output/ros")) + output_dir = get_env_var("ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros")) # Ensure output directory exists os.makedirs(output_dir, exist_ok=True) @@ -45,16 +45,17 @@ def get_env_var(var_name, default=None, required=False): else: ros_control = None - robot = UnitreeGo2(ip=robot_ip, - connection_method=connection_method, - serial_number=serial_number, - output_dir=output_dir, - ros_control=ros_control, - use_ros=use_ros, - use_webrtc=use_webrtc) + robot = UnitreeGo2( + ip=robot_ip, + connection_method=connection_method, + serial_number=serial_number, + output_dir=output_dir, + ros_control=ros_control, + use_ros=use_ros, + use_webrtc=use_webrtc, + ) time.sleep(5) try: - # Start perception print("\nStarting perception system...") @@ -72,14 +73,15 @@ def handle_frame(frame): try: # Save frame to output directory if desired for debugging frame streaming # MAKE SURE TO CHANGE OUTPUT DIR depending on if running in ROS or local - #frame_path = os.path.join(output_dir, f"frame_{frame_count:04d}.jpg") - #success = cv2.imwrite(frame_path, frame) - #print(f"Frame #{frame_count} {'saved successfully' if success else 'failed to save'} to {frame_path}") + # frame_path = os.path.join(output_dir, f"frame_{frame_count:04d}.jpg") + # success = cv2.imwrite(frame_path, frame) + # print(f"Frame #{frame_count} {'saved successfully' if success else 'failed to save'} to {frame_path}") pass except Exception as e: print(f"Error in handle_frame: {e}") import traceback + print(traceback.format_exc()) def handle_error(error): @@ -94,7 +96,8 @@ def handle_completion(): subscription = processed_stream.subscribe( on_next=handle_frame, on_error=lambda e: print(f"Subscription error: {e}"), - on_completed=lambda: print("Subscription completed")) + on_completed=lambda: print("Subscription completed"), + ) print("Subscription created successfully") except Exception as e: print(f"Error creating subscription: {e}") @@ -109,38 +112,37 @@ def handle_completion(): print("\n🤖 QUEUEING WEBRTC COMMANDS BACK-TO-BACK FOR TESTING UnitreeGo2🤖\n") # Dance 1 - robot.webrtc_req(api_id=1033) + robot.webrtc_req(api_id=1033) print("Queued: WiggleHips (1033)") robot.reverse(distance=0.2, speed=0.5) print("Queued: Reverse 0.5m at 0.5m/s") # Wiggle Hips - robot.webrtc_req(api_id=1033) + robot.webrtc_req(api_id=1033) print("Queued: WiggleHips (1033)") robot.move(distance=0.2, speed=0.5) print("Queued: Move forward 1.0m at 0.5m/s") - robot.webrtc_req(api_id=1017) + robot.webrtc_req(api_id=1017) print("Queued: Stretch (1017)") robot.move(distance=0.2, speed=0.5) print("Queued: Move forward 1.0m at 0.5m/s") - robot.webrtc_req(api_id=1017) + robot.webrtc_req(api_id=1017) print("Queued: Stretch (1017)") robot.reverse(distance=0.2, speed=0.5) print("Queued: Reverse 0.5m at 0.5m/s") - robot.webrtc_req(api_id=1017) - print("Queued: Stretch (1017)")\ - + robot.webrtc_req(api_id=1017) + print("Queued: Stretch (1017)") robot.spin(degrees=-90.0, speed=45.0) print("Queued: Spin right 90 degrees at 45 degrees/s") - robot.spin(degrees=90.0, speed=45.0) + robot.spin(degrees=90.0, speed=45.0) print("Queued: Spin left 90 degrees at 45 degrees/s") # To prevent termination @@ -149,14 +151,14 @@ def handle_completion(): except KeyboardInterrupt: print("\nStopping perception...") - if 'subscription' in locals(): + if "subscription" in locals(): subscription.dispose() except Exception as e: print(f"Error in main loop: {e}") finally: # Cleanup print("Cleaning up resources...") - if 'subscription' in locals(): + if "subscription" in locals(): subscription.dispose() del robot print("Cleanup complete.") diff --git a/tests/simple_agent_test.py b/tests/simple_agent_test.py index 0b7f64bac9..a5506f820a 100644 --- a/tests/simple_agent_test.py +++ b/tests/simple_agent_test.py @@ -7,19 +7,19 @@ import os # Initialize robot -robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - ros_control=UnitreeROSControl(), - skills=MyUnitreeSkills()) +robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), ros_control=UnitreeROSControl(), skills=MyUnitreeSkills() +) # Initialize agent agent = OpenAIAgent( - dev_name="UnitreeExecutionAgent", - input_video_stream=robot.get_ros_video_stream(), - skills=robot.get_skills(), - system_query="Wiggle when you see a person! Jump when you see a person waving!" - ) + dev_name="UnitreeExecutionAgent", + input_video_stream=robot.get_ros_video_stream(), + skills=robot.get_skills(), + system_query="Wiggle when you see a person! Jump when you see a person waving!", +) try: input("Press ESC to exit...") except KeyboardInterrupt: - print("\nExiting...") \ No newline at end of file + print("\nExiting...") diff --git a/tests/test_agent.py b/tests/test_agent.py index d05b18bb34..1319533d78 100644 --- a/tests/test_agent.py +++ b/tests/test_agent.py @@ -6,6 +6,7 @@ from dotenv import load_dotenv + # Sanity check for dotenv def test_dotenv(): print("test_dotenv:") @@ -13,9 +14,11 @@ def test_dotenv(): openai_api_key = os.getenv("OPENAI_API_KEY") print("\t\tOPENAI_API_KEY: ", openai_api_key) + # Sanity check for openai connection def test_openai_connection(): from openai import OpenAI + client = OpenAI() print("test_openai_connection:") response = client.chat.completions.create( @@ -38,5 +41,6 @@ def test_openai_connection(): ) print("\t\tOpenAI Response: ", response.choices[0]) + test_dotenv() test_openai_connection() diff --git a/tests/test_agent_alibaba.py b/tests/test_agent_alibaba.py index 6d39efe85a..9519387b7b 100644 --- a/tests/test_agent_alibaba.py +++ b/tests/test_agent_alibaba.py @@ -32,9 +32,9 @@ # Specify the OpenAI client for Alibaba qwen_client = OpenAI( - base_url='https://dashscope-intl.aliyuncs.com/compatible-mode/v1', - api_key=os.getenv('ALIBABA_API_KEY'), - ) + base_url="https://dashscope-intl.aliyuncs.com/compatible-mode/v1", + api_key=os.getenv("ALIBABA_API_KEY"), +) # Initialize Unitree skills myUnitreeSkills = MyUnitreeSkills() @@ -42,18 +42,18 @@ # Initialize agent agent = OpenAIAgent( - dev_name="AlibabaExecutionAgent", - openai_client=qwen_client, - model_name="qwen2.5-vl-72b-instruct", - tokenizer=HuggingFaceTokenizer(model_name="Qwen/Qwen2.5-VL-72B-Instruct"), - max_output_tokens_per_request=8192, - input_video_stream=video_stream, - # system_query="Tell me the number in the video. Find me the center of the number spotted, and print the coordinates to the console using an appropriate function call. Then provide me a deep history of the number in question and its significance in history. Additionally, tell me what model and version of language model you are.", - system_query="Tell me about any objects seen. Print the coordinates for center of the objects seen to the console using an appropriate function call. Then provide me a deep history of the number in question and its significance in history. Additionally, tell me what model and version of language model you are.", - skills=myUnitreeSkills, - ) + dev_name="AlibabaExecutionAgent", + openai_client=qwen_client, + model_name="qwen2.5-vl-72b-instruct", + tokenizer=HuggingFaceTokenizer(model_name="Qwen/Qwen2.5-VL-72B-Instruct"), + max_output_tokens_per_request=8192, + input_video_stream=video_stream, + # system_query="Tell me the number in the video. Find me the center of the number spotted, and print the coordinates to the console using an appropriate function call. Then provide me a deep history of the number in question and its significance in history. Additionally, tell me what model and version of language model you are.", + system_query="Tell me about any objects seen. Print the coordinates for center of the objects seen to the console using an appropriate function call. Then provide me a deep history of the number in question and its significance in history. Additionally, tell me what model and version of language model you are.", + skills=myUnitreeSkills, +) try: input("Press ESC to exit...") except KeyboardInterrupt: - print("\nExiting...") \ No newline at end of file + print("\nExiting...") diff --git a/tests/test_agent_ctransformers_gguf.py b/tests/test_agent_ctransformers_gguf.py index a48fa3c68e..6cd3405239 100644 --- a/tests/test_agent_ctransformers_gguf.py +++ b/tests/test_agent_ctransformers_gguf.py @@ -35,10 +35,10 @@ agent.run_observable_query(test_query).subscribe( on_next=lambda response: print(f"One-off query response: {response}"), on_error=lambda error: print(f"Error: {error}"), - on_completed=lambda: print("Query completed") + on_completed=lambda: print("Query completed"), ) try: input("Press ESC to exit...") except KeyboardInterrupt: - print("\nExiting...") \ No newline at end of file + print("\nExiting...") diff --git a/tests/test_agent_huggingface_local.py b/tests/test_agent_huggingface_local.py index d94b6a0470..4c4536a197 100644 --- a/tests/test_agent_huggingface_local.py +++ b/tests/test_agent_huggingface_local.py @@ -42,7 +42,7 @@ # Initialize agent agent = HuggingFaceLocalAgent( dev_name="HuggingFaceLLMAgent", - model_name= "Qwen/Qwen2.5-3B", + model_name="Qwen/Qwen2.5-3B", agent_type="HF-LLM", system_query=system_query, input_query_stream=query_provider.data_stream, @@ -59,14 +59,14 @@ # This will cause listening agents to consume the queries and respond # to them via skill execution and provide 1-shot responses. query_provider.start_query_stream( - query_template= - "{query}; User: travel forward by 10 meters", + query_template="{query}; User: travel forward by 10 meters", frequency=10, start_count=1, end_count=10000, - step=1) + step=1, +) try: input("Press ESC to exit...") except KeyboardInterrupt: - print("\nExiting...") \ No newline at end of file + print("\nExiting...") diff --git a/tests/test_agent_huggingface_local_jetson.py b/tests/test_agent_huggingface_local_jetson.py index eb260dcc90..6d29b3903f 100644 --- a/tests/test_agent_huggingface_local_jetson.py +++ b/tests/test_agent_huggingface_local_jetson.py @@ -43,7 +43,7 @@ agent = HuggingFaceLocalAgent( dev_name="HuggingFaceLLMAgent", model_name="Qwen/Qwen2.5-0.5B", - #model_name="HuggingFaceTB/SmolLM2-135M", + # model_name="HuggingFaceTB/SmolLM2-135M", agent_type="HF-LLM", system_query=system_query, input_query_stream=query_provider.data_stream, @@ -60,12 +60,12 @@ # This will cause listening agents to consume the queries and respond # to them via skill execution and provide 1-shot responses. query_provider.start_query_stream( - query_template= - "{query}; User: Hello how are you!", + query_template="{query}; User: Hello how are you!", frequency=30, start_count=1, end_count=10000, - step=1) + step=1, +) try: input("Press ESC to exit...") diff --git a/tests/test_agent_huggingface_remote.py b/tests/test_agent_huggingface_remote.py index dd533b2e78..7129523bf0 100644 --- a/tests/test_agent_huggingface_remote.py +++ b/tests/test_agent_huggingface_remote.py @@ -39,26 +39,26 @@ # Initialize agent agent = HuggingFaceRemoteAgent( - dev_name="HuggingFaceRemoteAgent", - model_name="meta-llama/Meta-Llama-3-8B-Instruct", - tokenizer=HuggingFaceTokenizer(model_name="meta-llama/Meta-Llama-3-8B-Instruct"), - max_output_tokens_per_request=8192, - input_query_stream=query_provider.data_stream, - # input_video_stream=video_stream, - system_query="You are a helpful assistant that can answer questions and help with tasks.", - ) + dev_name="HuggingFaceRemoteAgent", + model_name="meta-llama/Meta-Llama-3-8B-Instruct", + tokenizer=HuggingFaceTokenizer(model_name="meta-llama/Meta-Llama-3-8B-Instruct"), + max_output_tokens_per_request=8192, + input_query_stream=query_provider.data_stream, + # input_video_stream=video_stream, + system_query="You are a helpful assistant that can answer questions and help with tasks.", +) # Start the query stream. # Queries will be pushed every 1 second, in a count from 100 to 5000. query_provider.start_query_stream( - query_template= - "{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response.", + query_template="{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response.", frequency=5, start_count=1, end_count=10000, - step=1) + step=1, +) try: input("Press ESC to exit...") except KeyboardInterrupt: - print("\nExiting...") \ No newline at end of file + print("\nExiting...") diff --git a/tests/test_audio_agent.py b/tests/test_audio_agent.py index debdee11a6..61c30031fb 100644 --- a/tests/test_audio_agent.py +++ b/tests/test_audio_agent.py @@ -5,7 +5,6 @@ def main(): - stt_node = stt() agent = OpenAIAgent( diff --git a/tests/test_claude_agent_query.py b/tests/test_claude_agent_query.py index 96319d760b..aabd85bc12 100644 --- a/tests/test_claude_agent_query.py +++ b/tests/test_claude_agent_query.py @@ -21,12 +21,9 @@ load_dotenv() # Create a ClaudeAgent instance -agent = ClaudeAgent( - dev_name="test_agent", - query="What is the capital of France?" -) +agent = ClaudeAgent(dev_name="test_agent", query="What is the capital of France?") # Use the stream_query method to get a response response = agent.run_observable_query("What is the capital of France?").run() -print(f"Response from Claude Agent: {response}") \ No newline at end of file +print(f"Response from Claude Agent: {response}") diff --git a/tests/test_claude_agent_skills_query.py b/tests/test_claude_agent_skills_query.py index 2f1e34ec73..1aaeb795f1 100644 --- a/tests/test_claude_agent_skills_query.py +++ b/tests/test_claude_agent_skills_query.py @@ -37,18 +37,22 @@ # Load API key from environment load_dotenv() -robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - ros_control=UnitreeROSControl(), - skills=MyUnitreeSkills(), - mock_connection=False) +robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), + ros_control=UnitreeROSControl(), + skills=MyUnitreeSkills(), + mock_connection=False, +) # Create a subject for agent responses agent_response_subject = rx.subject.Subject() agent_response_stream = agent_response_subject.pipe(ops.share()) local_planner_viz_stream = robot.local_planner_viz_stream.pipe(ops.share()) -streams = {"unitree_video": robot.get_ros_video_stream(), - "local_planner_viz": local_planner_viz_stream} +streams = { + "unitree_video": robot.get_ros_video_stream(), + "local_planner_viz": local_planner_viz_stream, +} text_streams = { "agent_responses": agent_response_stream, } @@ -73,7 +77,7 @@ Example: If the user asks to move forward 1 meter, call the Move function with distance=1""", model_name="claude-3-7-sonnet-latest", - thinking_budget_tokens=2000 + thinking_budget_tokens=2000, ) tts_node = tts() @@ -100,9 +104,7 @@ robot_skills.create_instance("Speak", tts_node=tts_node) # Subscribe to agent responses and send them to the subject -agent.get_response_observable().subscribe( - lambda x: agent_response_subject.on_next(x) -) +agent.get_response_observable().subscribe(lambda x: agent_response_subject.on_next(x)) print("ObserveStream and Kill skills registered and ready for use") print("Created memory.txt file") @@ -120,11 +122,14 @@ def msg_handler(msgtype, data): except Exception as e: print(f"Error setting goal: {e}") return + + def threaded_msg_handler(msgtype, data): thread = threading.Thread(target=msg_handler, args=(msgtype, data)) thread.daemon = True thread.start() + websocket_vis.msg_handler = threaded_msg_handler web_interface.run() diff --git a/tests/test_command_pose_unitree.py b/tests/test_command_pose_unitree.py index 2311593a28..0537f5c446 100644 --- a/tests/test_command_pose_unitree.py +++ b/tests/test_command_pose_unitree.py @@ -1,5 +1,6 @@ import os import sys + # Add the parent directory to the Python path sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -11,9 +12,10 @@ import math # Initialize robot -robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - ros_control=UnitreeROSControl(), - skills=MyUnitreeSkills()) +robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), ros_control=UnitreeROSControl(), skills=MyUnitreeSkills() +) + # Helper function to send pose commands continuously for a duration def send_pose_for_duration(roll, pitch, yaw, duration, hz=10): @@ -21,7 +23,8 @@ def send_pose_for_duration(roll, pitch, yaw, duration, hz=10): start_time = time.time() while time.time() - start_time < duration: robot.pose_command(roll=roll, pitch=pitch, yaw=yaw) - time.sleep(1.0/hz) # Sleep to achieve the desired frequency + time.sleep(1.0 / hz) # Sleep to achieve the desired frequency + # Test pose commands @@ -62,4 +65,4 @@ def send_pose_for_duration(roll, pitch, yaw, duration, hz=10): while True: time.sleep(1) except KeyboardInterrupt: - print("Test terminated by user") \ No newline at end of file + print("Test terminated by user") diff --git a/tests/test_header.py b/tests/test_header.py index 8ae057208d..48ea6dd509 100644 --- a/tests/test_header.py +++ b/tests/test_header.py @@ -26,31 +26,33 @@ # Add the parent directory of 'tests' to the Python path sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + def get_caller_info(): """Identify the filename of the caller in the stack. - + Examines the call stack to find the first non-internal file that called this module. Skips the current file and Python internal files. - + Returns: str: The basename of the caller's filename, or "unknown" if not found. """ current_file = os.path.abspath(__file__) - + # Look through the call stack to find the first file that's not this one for frame in inspect.stack()[1:]: filename = os.path.abspath(frame.filename) # Skip this file and Python internals if filename != current_file and "= self.print_interval: self.last_print_time = current_time - + if not objects: print("\n[No objects detected]") return - - print("\n" + "="*50) + + print("\n" + "=" * 50) print(f"Detected {len(objects)} objects at {time.strftime('%H:%M:%S')}:") - print("="*50) - + print("=" * 50) + for i, obj in enumerate(objects): pos = obj["position"] rot = obj["rotation"] size = obj["size"] - - print(f"{i+1}. {obj['label']} (ID: {obj['object_id']}, Conf: {obj['confidence']:.2f})") + + print( + f"{i + 1}. {obj['label']} (ID: {obj['object_id']}, Conf: {obj['confidence']:.2f})" + ) print(f" Position: x={pos['x']:.2f}, y={pos['y']:.2f}, z={pos['z']:.2f} m") print(f" Rotation: yaw={rot['yaw']:.2f} rad") print(f" Size: width={size['width']:.2f}, height={size['height']:.2f} m") @@ -70,7 +82,7 @@ def print_results(self, objects: List[Dict[str, Any]]): def main(): # Get command line arguments args = parse_args() - + # Set up the result printer for console output result_printer = ResultPrinter(print_interval=1.0) @@ -78,20 +90,20 @@ def main(): min_confidence = 0.6 class_filter = None # No class filtering web_port = 5555 - + # Initialize detector detector = Detic2DDetector(vocabulary=None, threshold=min_confidence) - + # Initialize based on mode if args.mode == "robot": print("Initializing in robot mode...") - + # Get robot IP from environment - robot_ip = os.getenv('ROBOT_IP') + robot_ip = os.getenv("ROBOT_IP") if not robot_ip: print("Error: ROBOT_IP environment variable not set.") sys.exit(1) - + # Initialize robot robot = UnitreeGo2( ip=robot_ip, @@ -108,99 +120,92 @@ def main(): class_filter=class_filter, transform_to_map=robot.ros_control.transform_pose, detector=detector, - video_stream=video_stream + video_stream=video_stream, ) - - else: # webcam mode print("Initializing in webcam mode...") - + # Define camera intrinsics for the webcam # These are approximate values for a typical 640x480 webcam width, height = 640, 480 focal_length_mm = 3.67 # mm (typical webcam) - sensor_width_mm = 4.8 # mm (1/4" sensor) - + sensor_width_mm = 4.8 # mm (1/4" sensor) + # Calculate focal length in pixels focal_length_x_px = width * focal_length_mm / sensor_width_mm focal_length_y_px = height * focal_length_mm / sensor_width_mm - + # Principal point (center of image) cx, cy = width / 2, height / 2 - + # Camera intrinsics in [fx, fy, cx, cy] format camera_intrinsics = [focal_length_x_px, focal_length_y_px, cx, cy] - + # Initialize video provider and ObjectDetectionStream video_provider = VideoProvider("test_camera", video_source=0) # Default camera # Create video stream - video_stream = backpressure(video_provider.capture_video_as_observable(realtime=True, fps=30)) + video_stream = backpressure( + video_provider.capture_video_as_observable(realtime=True, fps=30) + ) object_detector = ObjectDetectionStream( camera_intrinsics=camera_intrinsics, min_confidence=min_confidence, class_filter=class_filter, detector=detector, - video_stream=video_stream + video_stream=video_stream, ) - - - + # Set placeholder robot for cleanup robot = None - + # Create visualization stream for web interface viz_stream = object_detector.get_stream().pipe( ops.share(), ops.map(lambda x: x["viz_frame"] if x is not None else None), ops.filter(lambda x: x is not None), ) - + # Create stop event for clean shutdown stop_event = threading.Event() - + # Define subscription callback to print results def on_next(result): if stop_event.is_set(): return - + # Print detected objects to console if "objects" in result: result_printer.print_results(result["objects"]) - + def on_error(error): print(f"Error in detection stream: {error}") stop_event.set() - + def on_completed(): print("Detection stream completed") stop_event.set() - + try: # Subscribe to the detection stream subscription = object_detector.get_stream().subscribe( - on_next=on_next, - on_error=on_error, - on_completed=on_completed + on_next=on_next, on_error=on_error, on_completed=on_completed ) - + # Set up web interface print("Initializing web interface...") - web_interface = RobotWebInterface( - port=web_port, - object_detection=viz_stream - ) - + web_interface = RobotWebInterface(port=web_port, object_detection=viz_stream) + # Print configuration information print("\nObjectDetectionStream Test Running:") print(f"Mode: {args.mode}") print(f"Web Interface: http://localhost:{web_port}") print("\nPress Ctrl+C to stop the test\n") - + # Start web server (blocking call) web_interface.run() - + except KeyboardInterrupt: print("\nTest interrupted by user") except Exception as e: @@ -209,16 +214,16 @@ def on_completed(): # Clean up resources print("Cleaning up resources...") stop_event.set() - + if subscription: subscription.dispose() - + if args.mode == "robot" and robot: robot.cleanup() elif args.mode == "webcam": - if 'video_provider' in locals(): + if "video_provider" in locals(): video_provider.dispose_all() - + print("Test completed") diff --git a/tests/test_object_tracking_webcam.py b/tests/test_object_tracking_webcam.py index e72912ed93..ed72d76e34 100644 --- a/tests/test_object_tracking_webcam.py +++ b/tests/test_object_tracking_webcam.py @@ -16,20 +16,21 @@ tracker_initialized = False object_size = 0.30 # Hardcoded object size in meters (adjust based on your tracking target) + def mouse_callback(event, x, y, flags, param): global selecting_bbox, bbox_points, current_bbox, tracker_initialized, tracker_stream - + if event == cv2.EVENT_LBUTTONDOWN: # Start bbox selection selecting_bbox = True bbox_points = [(x, y)] current_bbox = None tracker_initialized = False - + elif event == cv2.EVENT_MOUSEMOVE and selecting_bbox: # Update current selection for visualization current_bbox = [bbox_points[0][0], bbox_points[0][1], x, y] - + elif event == cv2.EVENT_LBUTTONUP: # End bbox selection selecting_bbox = False @@ -40,49 +41,49 @@ def mouse_callback(event, x, y, flags, param): # Ensure x1,y1 is top-left and x2,y2 is bottom-right current_bbox = [min(x1, x2), min(y1, y2), max(x1, x2), max(y1, y2)] # Add the bbox to the tracking queue - if param.get('bbox_queue') and not tracker_initialized: - param['bbox_queue'].put((current_bbox, object_size)) + if param.get("bbox_queue") and not tracker_initialized: + param["bbox_queue"].put((current_bbox, object_size)) tracker_initialized = True def main(): global tracker_initialized - + # Create queues for thread communication frame_queue = queue.Queue(maxsize=5) bbox_queue = queue.Queue() stop_event = threading.Event() - + # Logitech C920e camera parameters at 480p # Convert physical parameters to pixel-based intrinsics width, height = 640, 480 focal_length_mm = 3.67 # mm - sensor_width_mm = 4.8 # mm (1/4" sensor) + sensor_width_mm = 4.8 # mm (1/4" sensor) sensor_height_mm = 3.6 # mm - + # Calculate focal length in pixels focal_length_x_px = width * focal_length_mm / sensor_width_mm focal_length_y_px = height * focal_length_mm / sensor_height_mm - + # Principal point (assuming center of image) cx = width / 2 cy = height / 2 - + # Final camera intrinsics in [fx, fy, cx, cy] format camera_intrinsics = [focal_length_x_px, focal_length_y_px, cx, cy] - + # Initialize video provider and object tracking stream video_provider = VideoProvider("test_camera", video_source=0) tracker_stream = ObjectTrackingStream( camera_intrinsics=camera_intrinsics, camera_pitch=0.0, # Adjust if your camera is tilted - camera_height=0.5 # Height of camera from ground in meters (adjust as needed) + camera_height=0.5, # Height of camera from ground in meters (adjust as needed) ) - + # Create video stream video_stream = video_provider.capture_video_as_observable(realtime=True, fps=30) tracking_stream = tracker_stream.create_stream(video_stream) - + # Define callbacks for the tracking stream def on_next(result): if stop_event.is_set(): @@ -90,55 +91,74 @@ def on_next(result): # Get the visualization frame viz_frame = result["viz_frame"] - + # If we're selecting a bbox, draw the current selection if selecting_bbox and current_bbox is not None: x1, y1, x2, y2 = current_bbox cv2.rectangle(viz_frame, (x1, y1), (x2, y2), (0, 255, 255), 2) - + # Add instructions - cv2.putText(viz_frame, "Click and drag to select object", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(viz_frame, f"Object size: {object_size:.2f}m", (10, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - + cv2.putText( + viz_frame, + "Click and drag to select object", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + ) + cv2.putText( + viz_frame, + f"Object size: {object_size:.2f}m", + (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + ) + # Show tracking status status = "Tracking" if tracker_initialized else "Not tracking" - cv2.putText(viz_frame, f"Status: {status}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0) if tracker_initialized else (0, 0, 255), 2) - + cv2.putText( + viz_frame, + f"Status: {status}", + (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 0) if tracker_initialized else (0, 0, 255), + 2, + ) + # Put frame in queue for main thread to display try: frame_queue.put_nowait(viz_frame) except queue.Full: # Skip frame if queue is full pass - + def on_error(error): print(f"Error: {error}") stop_event.set() - + def on_completed(): print("Stream completed") stop_event.set() - + # Start the subscription subscription = None - + try: # Subscribe to start processing in background thread subscription = tracking_stream.subscribe( - on_next=on_next, - on_error=on_error, - on_completed=on_completed + on_next=on_next, on_error=on_error, on_completed=on_completed ) - + print("Object tracking started. Click and drag to select an object. Press 'q' to exit.") - + # Create window and set mouse callback cv2.namedWindow("Object Tracker") - cv2.setMouseCallback("Object Tracker", mouse_callback, {'bbox_queue': bbox_queue}) - + cv2.setMouseCallback("Object Tracker", mouse_callback, {"bbox_queue": bbox_queue}) + # Main thread loop for displaying frames and handling bbox selection while not stop_event.is_set(): # Check if there's a new bbox to track @@ -149,35 +169,35 @@ def on_completed(): tracker_stream.track(new_bbox, size=size) except queue.Empty: pass - + try: # Get frame with timeout viz_frame = frame_queue.get(timeout=1.0) - + # Display the frame cv2.imshow("Object Tracker", viz_frame) # Check for exit key - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break - + except queue.Empty: # No frame available, check if we should continue - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break continue - + except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") finally: # Signal threads to stop stop_event.set() - + # Clean up resources if subscription: subscription.dispose() - + video_provider.dispose_all() tracker_stream.cleanup() cv2.destroyAllWindows() @@ -185,4 +205,4 @@ def on_completed(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/test_object_tracking_with_qwen.py b/tests/test_object_tracking_with_qwen.py index 547ee7e139..c2cb004f90 100644 --- a/tests/test_object_tracking_with_qwen.py +++ b/tests/test_object_tracking_with_qwen.py @@ -29,7 +29,7 @@ # Logitech C920e camera parameters at 480p width, height = 640, 480 focal_length_mm = 3.67 # mm -sensor_width_mm = 4.8 # mm (1/4" sensor) +sensor_width_mm = 4.8 # mm (1/4" sensor) sensor_height_mm = 3.6 # mm # Calculate focal length in pixels @@ -43,9 +43,7 @@ # Initialize video provider and object tracking stream video_provider = VideoProvider("webcam", video_source=0) tracker_stream = ObjectTrackingStream( - camera_intrinsics=camera_intrinsics, - camera_pitch=0.0, - camera_height=0.5 + camera_intrinsics=camera_intrinsics, camera_pitch=0.0, camera_height=0.5 ) # Create video streams @@ -53,8 +51,11 @@ tracking_stream = tracker_stream.create_stream(video_stream) # Check if display is available -if 'DISPLAY' not in os.environ: - raise RuntimeError("No display available. Please set DISPLAY environment variable or run in headless mode.") +if "DISPLAY" not in os.environ: + raise RuntimeError( + "No display available. Please set DISPLAY environment variable or run in headless mode." + ) + # Define callbacks for the tracking stream def on_next(result): @@ -64,38 +65,55 @@ def on_next(result): # Get the visualization frame viz_frame = result["viz_frame"] - + # Add information to the visualization - cv2.putText(viz_frame, f"Tracking {tracking_object_name}", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - cv2.putText(viz_frame, f"Object size: {object_size:.2f}m", (10, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2) - + cv2.putText( + viz_frame, + f"Tracking {tracking_object_name}", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + ) + cv2.putText( + viz_frame, + f"Object size: {object_size:.2f}m", + (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (255, 255, 255), + 2, + ) + # Show tracking status status = "Tracking" if tracker_initialized else "Waiting for detection" color = (0, 255, 0) if tracker_initialized else (0, 0, 255) - cv2.putText(viz_frame, f"Status: {status}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2) - + cv2.putText(viz_frame, f"Status: {status}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2) + # If detection is in progress, show a message if detection_in_progress: - cv2.putText(viz_frame, "Querying Qwen...", (10, 120), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) - + cv2.putText( + viz_frame, "Querying Qwen...", (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2 + ) + # Put frame in queue for main thread to display try: frame_queue.put_nowait(viz_frame) except queue.Full: pass + def on_error(error): print(f"Error: {error}") stop_event.set() + def on_completed(): print("Stream completed") stop_event.set() + # Start the subscription subscription = None @@ -105,14 +123,12 @@ def on_completed(): detection_in_progress = False # Subscribe to start processing in background thread subscription = tracking_stream.subscribe( - on_next=on_next, - on_error=on_error, - on_completed=on_completed + on_next=on_next, on_error=on_error, on_completed=on_completed ) - + print("Object tracking with Qwen started. Press 'q' to exit.") print("Waiting for initial object detection...") - + # Main thread loop for displaying frames and updating tracking while not stop_event.is_set(): # Check if we need to update tracking @@ -129,14 +145,14 @@ def detection_task(): try: result = get_bbox_from_qwen(video_stream, object_name=object_name) print(f"Got result from Qwen: {result}") - + if result: bbox, size = result print(f"Detected object at {bbox} with size {size}") tracker_stream.track(bbox, size=size) tracker_initialized = True return - + print("No object detected by Qwen") tracker_initialized = False tracker_stream.stop_track() @@ -150,37 +166,37 @@ def detection_task(): # Run detection task in a separate thread threading.Thread(target=detection_task, daemon=True).start() - + try: # Get frame with timeout viz_frame = frame_queue.get(timeout=0.1) - + # Display the frame cv2.imshow("Object Tracking with Qwen", viz_frame) - + # Check for exit key - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break - + except queue.Empty: # No frame available, check if we should continue - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break continue - + except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") finally: # Signal threads to stop stop_event.set() - + # Clean up resources if subscription: subscription.dispose() - + video_provider.dispose_all() tracker_stream.cleanup() cv2.destroyAllWindows() - print("Cleanup complete") \ No newline at end of file + print("Cleanup complete") diff --git a/tests/test_observe_stream_skill.py b/tests/test_observe_stream_skill.py index d0b87d4ff7..10532c8d2e 100644 --- a/tests/test_observe_stream_skill.py +++ b/tests/test_observe_stream_skill.py @@ -23,32 +23,28 @@ from dimos.web.robot_web_interface import RobotWebInterface from dimos.utils.logging_config import setup_logger import tests.test_header + logger = setup_logger("tests.test_observe_stream_skill") load_dotenv() + def main(): # Initialize the robot with mock connection for testing robot = UnitreeGo2( - ip=os.getenv('ROBOT_IP', '192.168.123.161'), - skills=MyUnitreeSkills(), - mock_connection=True + ip=os.getenv("ROBOT_IP", "192.168.123.161"), skills=MyUnitreeSkills(), mock_connection=True ) - + agent_response_subject = rx.subject.Subject() agent_response_stream = agent_response_subject.pipe(ops.share()) - + streams = {"unitree_video": robot.get_ros_video_stream()} text_streams = { "agent_responses": agent_response_stream, } - - web_interface = RobotWebInterface( - port=5555, - text_streams=text_streams, - **streams - ) - + + web_interface = RobotWebInterface(port=5555, text_streams=text_streams, **streams) + agent = ClaudeAgent( dev_name="test_agent", input_query_stream=web_interface.query_stream, @@ -57,62 +53,65 @@ def main(): When you see an image, describe what you see and alert if you notice any people or important changes. Be concise but thorough in your observations.""", model_name="claude-3-7-sonnet-latest", - thinking_budget_tokens=10000 - ) - - agent.get_response_observable().subscribe( - lambda x: agent_response_subject.on_next(x) + thinking_budget_tokens=10000, ) - + + agent.get_response_observable().subscribe(lambda x: agent_response_subject.on_next(x)) + robot_skills = robot.get_skills() - + robot_skills.add(ObserveStream) robot_skills.add(KillSkill) - + robot_skills.create_instance("ObserveStream", robot=robot, agent=agent) robot_skills.create_instance("KillSkill", skill_library=robot_skills) - + web_interface_thread = threading.Thread(target=web_interface.run) web_interface_thread.daemon = True web_interface_thread.start() - + logger.info("Starting monitor skill...") - + memory_file = os.path.join(agent.output_dir, "memory.txt") with open(memory_file, "a") as f: - f.write("SKILL CALL: ObserveStream(timestep=10.0, query_text='What do you see in this image? Alert me if you see any people.', max_duration=120.0)") - - result = robot_skills.call("ObserveStream", - timestep=10.0, # 20 seconds between monitoring queries - query_text="What do you see in this image? Alert me if you see any people.", - max_duration=120.0) # Run for 120 seconds + f.write( + "SKILL CALL: ObserveStream(timestep=10.0, query_text='What do you see in this image? Alert me if you see any people.', max_duration=120.0)" + ) + + result = robot_skills.call( + "ObserveStream", + timestep=10.0, # 20 seconds between monitoring queries + query_text="What do you see in this image? Alert me if you see any people.", + max_duration=120.0, + ) # Run for 120 seconds logger.info(f"Monitor skill result: {result}") - + logger.info(f"Running skills: {robot_skills.get_running_skills().keys()}") - + try: logger.info("Observer running. Will stop after 35 seconds...") time.sleep(20.0) logger.info(f"Running skills before kill: {robot_skills.get_running_skills().keys()}") logger.info("Killing the observer skill...") - + memory_file = os.path.join(agent.output_dir, "memory.txt") with open(memory_file, "a") as f: f.write("\n\nSKILL CALL: KillSkill(skill_name='observer')\n\n") - + kill_result = robot_skills.call("KillSkill", skill_name="observer") logger.info(f"Kill skill result: {kill_result}") - + logger.info(f"Running skills after kill: {robot_skills.get_running_skills().keys()}") - + # Keep test running until user interrupts while True: time.sleep(1.0) except KeyboardInterrupt: logger.info("Test interrupted by user") - + logger.info("Test completed") + if __name__ == "__main__": main() diff --git a/tests/test_person_following_robot.py b/tests/test_person_following_robot.py index 22a51e0670..c082cb1b57 100644 --- a/tests/test_person_following_robot.py +++ b/tests/test_person_following_robot.py @@ -16,12 +16,12 @@ def main(): # Hardcoded parameters timeout = 60.0 # Maximum time to follow a person (seconds) distance = 0.5 # Desired distance to maintain from target (meters) - + print("Initializing Unitree Go2 robot...") - + # Initialize the robot with ROS control and skills robot = UnitreeGo2( - ip=os.getenv('ROBOT_IP'), + ip=os.getenv("ROBOT_IP"), ros_control=UnitreeROSControl(), skills=MyUnitreeSkills(), ) @@ -33,58 +33,59 @@ def main(): RxOps.filter(lambda x: x is not None), ) video_stream = robot.get_ros_video_stream() - + try: # Set up web interface logger.info("Initializing web interface") - streams = { - "unitree_video": video_stream, - "person_tracking": viz_stream - } - - web_interface = RobotWebInterface( - port=5555, - **streams - ) - + streams = {"unitree_video": video_stream, "person_tracking": viz_stream} + + web_interface = RobotWebInterface(port=5555, **streams) + # Wait for camera and tracking to initialize print("Waiting for camera and tracking to initialize...") time.sleep(5) # Get initial point from Qwen max_retries = 5 - delay = 3 - + delay = 3 + for attempt in range(max_retries): try: - qwen_point = eval(query_single_frame_observable( - video_stream, - "Look at this frame and point to the person shirt. Return ONLY their center coordinates as a tuple (x,y)." - ).pipe(RxOps.take(1)).run()) # Get first response and convert string tuple to actual tuple + qwen_point = eval( + query_single_frame_observable( + video_stream, + "Look at this frame and point to the person shirt. Return ONLY their center coordinates as a tuple (x,y).", + ) + .pipe(RxOps.take(1)) + .run() + ) # Get first response and convert string tuple to actual tuple logger.info(f"Found person at coordinates {qwen_point}") break # If successful, break out of retry loop except Exception as e: if attempt < max_retries - 1: - logger.error(f"Person not found. Attempt {attempt + 1}/{max_retries} failed. Retrying in {delay}s... Error: {e}") + logger.error( + f"Person not found. Attempt {attempt + 1}/{max_retries} failed. Retrying in {delay}s... Error: {e}" + ) time.sleep(delay) else: logger.error(f"Person not found after {max_retries} attempts. Last error: {e}") return - + # Start following human in a separate thread import threading + follow_thread = threading.Thread( target=lambda: robot.follow_human(timeout=timeout, distance=distance, point=qwen_point), - daemon=True + daemon=True, ) follow_thread.start() - + print(f"Following human at point {qwen_point} for {timeout} seconds...") print("Web interface available at http://localhost:5555") - + # Start web server (blocking call) web_interface.run() - + except KeyboardInterrupt: print("\nInterrupted by user") except Exception as e: diff --git a/tests/test_person_following_webcam.py b/tests/test_person_following_webcam.py index 09ca7f9319..11d2739504 100644 --- a/tests/test_person_following_webcam.py +++ b/tests/test_person_following_webcam.py @@ -17,52 +17,50 @@ def main(): frame_queue = queue.Queue(maxsize=5) result_queue = queue.Queue(maxsize=5) # For tracking results stop_event = threading.Event() - + # Logitech C920e camera parameters at 480p # Convert physical parameters to intrinsics [fx, fy, cx, cy] resolution = (640, 480) # 480p resolution focal_length_mm = 3.67 # mm sensor_size_mm = (4.8, 3.6) # mm (1/4" sensor) - + # Calculate focal length in pixels fx = (resolution[0] * focal_length_mm) / sensor_size_mm[0] fy = (resolution[1] * focal_length_mm) / sensor_size_mm[1] - + # Principal point (typically at image center) cx = resolution[0] / 2 cy = resolution[1] / 2 - + # Camera intrinsics in [fx, fy, cx, cy] format camera_intrinsics = [fx, fy, cx, cy] - + # Camera mounted parameters camera_pitch = np.deg2rad(-5) # negative for downward pitch camera_height = 1.4 # meters - + # Initialize video provider and person tracking stream video_provider = VideoProvider("test_camera", video_source=0) person_tracker = PersonTrackingStream( - camera_intrinsics=camera_intrinsics, - camera_pitch=camera_pitch, - camera_height=camera_height + camera_intrinsics=camera_intrinsics, camera_pitch=camera_pitch, camera_height=camera_height ) - + # Create streams video_stream = video_provider.capture_video_as_observable(realtime=False, fps=20) person_tracking_stream = person_tracker.create_stream(video_stream) - + # Create visual servoing object visual_servoing = VisualServoing( tracking_stream=person_tracking_stream, max_linear_speed=0.5, max_angular_speed=0.75, - desired_distance=2.5 + desired_distance=2.5, ) - + # Track if we have selected a person to follow selected_point = None tracking_active = False - + # Define callbacks for the tracking stream def on_next(result): if stop_event.is_set(): @@ -71,63 +69,61 @@ def on_next(result): # Get the visualization frame which already includes person detections # with bounding boxes, tracking IDs, and distance/angle information viz_frame = result["viz_frame"] - + # Store the result for the main thread to use with visual servoing try: result_queue.put_nowait(result) except queue.Full: # Skip if queue is full pass - + # Put frame in queue for main thread to display (non-blocking) try: frame_queue.put_nowait(viz_frame) except queue.Full: # Skip frame if queue is full pass - + def on_error(error): print(f"Error: {error}") stop_event.set() - + def on_completed(): print("Stream completed") stop_event.set() - + # Mouse callback for selecting a person to track def mouse_callback(event, x, y, flags, param): nonlocal selected_point, tracking_active - + if event == cv2.EVENT_LBUTTONDOWN: # Store the clicked point selected_point = (x, y) tracking_active = False # Will be set to True if start_tracking succeeds print(f"Selected point: {selected_point}") - + # Start the subscription subscription = None - + try: # Subscribe to start processing in background thread subscription = person_tracking_stream.subscribe( - on_next=on_next, - on_error=on_error, - on_completed=on_completed + on_next=on_next, on_error=on_error, on_completed=on_completed ) - + print("Person tracking visualization started.") print("Click on a person to start visual servoing. Press 'q' to exit.") - + # Set up mouse callback cv2.namedWindow("Person Tracking") cv2.setMouseCallback("Person Tracking", mouse_callback) - + # Main thread loop for displaying frames while not stop_event.is_set(): try: # Get frame with timeout (allows checking stop_event periodically) frame = frame_queue.get(timeout=1.0) - + # Call the visual servoing if we have a selected point if selected_point is not None: # If not actively tracking, try to start tracking @@ -136,56 +132,79 @@ def mouse_callback(event, x, y, flags, param): if not tracking_active: print("Failed to start tracking") selected_point = None - + # If tracking is active, update tracking if tracking_active: servoing_result = visual_servoing.updateTracking() - + # Display visual servoing output on the frame linear_vel = servoing_result.get("linear_vel", 0.0) angular_vel = servoing_result.get("angular_vel", 0.0) running = visual_servoing.running - - status_color = (0, 255, 0) if running else (0, 0, 255) # Green if running, red if not - + + status_color = ( + (0, 255, 0) if running else (0, 0, 255) + ) # Green if running, red if not + # Add velocity text to frame - cv2.putText(frame, f"Linear: {linear_vel:.2f} m/s", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, status_color, 2) - cv2.putText(frame, f"Angular: {angular_vel:.2f} rad/s", (10, 60), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, status_color, 2) - cv2.putText(frame, f"Tracking: {'ON' if running else 'OFF'}", (10, 90), - cv2.FONT_HERSHEY_SIMPLEX, 0.7, status_color, 2) - + cv2.putText( + frame, + f"Linear: {linear_vel:.2f} m/s", + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + status_color, + 2, + ) + cv2.putText( + frame, + f"Angular: {angular_vel:.2f} rad/s", + (10, 60), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + status_color, + 2, + ) + cv2.putText( + frame, + f"Tracking: {'ON' if running else 'OFF'}", + (10, 90), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + status_color, + 2, + ) + # If tracking is lost, reset selected_point and tracking_active if not running: selected_point = None tracking_active = False - + # Display the frame in main thread cv2.imshow("Person Tracking", frame) - + # Check for exit key - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break - + except queue.Empty: # No frame available, check if we should continue - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break continue - + except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") finally: # Signal threads to stop stop_event.set() - + # Clean up resources if subscription: subscription.dispose() - + visual_servoing.cleanup() video_provider.dispose_all() person_tracker.cleanup() diff --git a/tests/test_planning_agent_web_interface.py b/tests/test_planning_agent_web_interface.py index 46f58c9e4e..68bc711075 100644 --- a/tests/test_planning_agent_web_interface.py +++ b/tests/test_planning_agent_web_interface.py @@ -27,18 +27,19 @@ from dimos.robot.unitree.unitree_go2 import UnitreeGo2 from dimos.robot.unitree.unitree_skills import MyUnitreeSkills from dimos.utils.logging_config import logger + # from dimos.web.fastapi_server import FastAPIServer from dimos.web.robot_web_interface import RobotWebInterface from dimos.utils.threadpool import make_single_thread_scheduler + def main(): # Get environment variables robot_ip = os.getenv("ROBOT_IP") if not robot_ip: raise ValueError("ROBOT_IP environment variable is required") - connection_method = os.getenv("CONN_TYPE") or 'webrtc' - output_dir = os.getenv("ROS_OUTPUT_DIR", - os.path.join(os.getcwd(), "assets/output/ros")) + connection_method = os.getenv("CONN_TYPE") or "webrtc" + output_dir = os.getenv("ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros")) # Initialize components as None for proper cleanup robot = None @@ -49,11 +50,13 @@ def main(): try: # Initialize robot logger.info("Initializing Unitree Robot") - robot = UnitreeGo2(ip=robot_ip, - connection_method=connection_method, - output_dir=output_dir, - mock_connection=False, - skills=MyUnitreeSkills()) + robot = UnitreeGo2( + ip=robot_ip, + connection_method=connection_method, + output_dir=output_dir, + mock_connection=False, + skills=MyUnitreeSkills(), + ) # Set up video stream logger.info("Starting video stream") video_stream = robot.get_ros_video_stream() @@ -65,10 +68,10 @@ def main(): logger.info("Creating response streams") planner_response_subject = rx.subject.Subject() planner_response_stream = planner_response_subject.pipe(ops.share()) - + executor_response_subject = rx.subject.Subject() executor_response_stream = executor_response_subject.pipe(ops.share()) - + # Web interface mode with FastAPI server logger.info("Initializing FastAPI server") streams = {"unitree_video": video_stream} @@ -76,36 +79,33 @@ def main(): "planner_responses": planner_response_stream, "executor_responses": executor_response_stream, } - - web_interface = RobotWebInterface( - port=5555, text_streams=text_streams, **streams) + + web_interface = RobotWebInterface(port=5555, text_streams=text_streams, **streams) logger.info("Starting planning agent with web interface") planner = PlanningAgent( dev_name="TaskPlanner", model_name="gpt-4o", input_query_stream=web_interface.query_stream, - skills=robot.get_skills() + skills=robot.get_skills(), ) - + # Get planner's response observable logger.info("Setting up agent response streams") planner_responses = planner.get_response_observable() - + # Connect planner to its subject - planner_responses.subscribe( - lambda x: planner_response_subject.on_next(x) - ) + planner_responses.subscribe(lambda x: planner_response_subject.on_next(x)) planner_responses.subscribe( on_next=lambda x: logger.info(f"Planner response: {x}"), on_error=lambda e: logger.error(f"Planner error: {e}"), - on_completed=lambda: logger.info("Planner completed") + on_completed=lambda: logger.info("Planner completed"), ) - + # Initialize execution agent with robot skills logger.info("Starting execution agent") - system_query=dedent( + system_query = dedent( """ You are a robot execution agent that can execute tasks on a virtual robot. The sole text you will be given is the task to execute. @@ -119,7 +119,7 @@ def main(): output_dir=output_dir, skills=robot.get_skills(), system_query=system_query, - pool_scheduler=make_single_thread_scheduler() + pool_scheduler=make_single_thread_scheduler(), ) # Get executor's response observable @@ -129,13 +129,11 @@ def main(): executor_responses.subscribe( on_next=lambda x: logger.info(f"Executor response: {x}"), on_error=lambda e: logger.error(f"Executor error: {e}"), - on_completed=lambda: logger.info("Executor completed") + on_completed=lambda: logger.info("Executor completed"), ) - + # Connect executor to its subject - executor_responses.subscribe( - lambda x: executor_response_subject.on_next(x) - ) + executor_responses.subscribe(lambda x: executor_response_subject.on_next(x)) # Start web server (blocking call) logger.info("Starting FastAPI server") diff --git a/tests/test_planning_robot_agent.py b/tests/test_planning_robot_agent.py index 3b7b95cea2..b2d27ba3a5 100644 --- a/tests/test_planning_robot_agent.py +++ b/tests/test_planning_robot_agent.py @@ -29,14 +29,14 @@ from dimos.web.robot_web_interface import RobotWebInterface from dimos.utils.threadpool import make_single_thread_scheduler + def main(): # Get environment variables robot_ip = os.getenv("ROBOT_IP") if not robot_ip: raise ValueError("ROBOT_IP environment variable is required") - connection_method = os.getenv("CONN_TYPE") or 'webrtc' - output_dir = os.getenv("ROS_OUTPUT_DIR", - os.path.join(os.getcwd(), "assets/output/ros")) + connection_method = os.getenv("CONN_TYPE") or "webrtc" + output_dir = os.getenv("ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros")) use_terminal = os.getenv("USE_TERMINAL", "").lower() == "true" use_terminal = True @@ -49,10 +49,12 @@ def main(): try: # Initialize robot logger.info("Initializing Unitree Robot") - robot = UnitreeGo2(ip=robot_ip, - connection_method=connection_method, - output_dir=output_dir, - mock_connection=True) + robot = UnitreeGo2( + ip=robot_ip, + connection_method=connection_method, + output_dir=output_dir, + mock_connection=True, + ) # Set up video stream logger.info("Starting video stream") @@ -69,7 +71,7 @@ def main(): dev_name="TaskPlanner", model_name="gpt-4o", use_terminal=True, - skills=skills_instance + skills=skills_instance, ) else: # Web interface mode @@ -82,16 +84,16 @@ def main(): dev_name="TaskPlanner", model_name="gpt-4o", input_query_stream=web_interface.query_stream, - skills=skills_instance + skills=skills_instance, ) - + # Get planner's response observable logger.info("Setting up agent response streams") planner_responses = planner.get_response_observable() - + # Initialize execution agent with robot skills logger.info("Starting execution agent") - system_query=dedent( + system_query = dedent( """ You are a robot execution agent that can execute tasks on a virtual robot. You are given a task to execute and a list of skills that @@ -105,7 +107,7 @@ def main(): output_dir=output_dir, skills=skills_instance, system_query=system_query, - pool_scheduler=make_single_thread_scheduler() + pool_scheduler=make_single_thread_scheduler(), ) # Get executor's response observable @@ -115,7 +117,7 @@ def main(): executor_responses.subscribe( on_next=lambda x: logger.info(f"Executor response: {x}"), on_error=lambda e: logger.error(f"Executor error: {e}"), - on_completed=lambda: logger.info("Executor completed") + on_completed=lambda: logger.info("Executor completed"), ) if use_terminal: @@ -158,4 +160,4 @@ def main(): if __name__ == "__main__": sys.exit(main()) -# Example Task: Move the robot forward by 1 meter, then turn 90 degrees clockwise, then move backward by 1 meter, then turn a random angle counterclockwise, then repeat this sequence 5 times. \ No newline at end of file +# Example Task: Move the robot forward by 1 meter, then turn 90 degrees clockwise, then move backward by 1 meter, then turn a random angle counterclockwise, then repeat this sequence 5 times. diff --git a/tests/test_qwen_image_query.py b/tests/test_qwen_image_query.py index 6032430f2d..feaa8c0096 100644 --- a/tests/test_qwen_image_query.py +++ b/tests/test_qwen_image_query.py @@ -4,30 +4,32 @@ from PIL import Image from dimos.models.qwen.video_query import query_single_frame + def test_qwen_image_query(): """Test querying Qwen with a single image.""" # Skip if no API key - if not os.getenv('ALIBABA_API_KEY'): + if not os.getenv("ALIBABA_API_KEY"): print("ALIBABA_API_KEY not set") return - + # Load test image image_path = os.path.join(os.getcwd(), "assets", "test_spatial_memory", "frame_038.jpg") image = Image.open(image_path) - + # Test basic object detection query response = query_single_frame( image=image, - query="What objects do you see in this image? Return as a comma-separated list." + query="What objects do you see in this image? Return as a comma-separated list.", ) print(response) - + # Test coordinate query response = query_single_frame( image=image, - query="Return the center coordinates of any person in the image as a tuple (x,y)" + query="Return the center coordinates of any person in the image as a tuple (x,y)", ) print(response) - + + if __name__ == "__main__": - test_qwen_image_query() \ No newline at end of file + test_qwen_image_query() diff --git a/tests/test_robot.py b/tests/test_robot.py index b452100c85..011850b04e 100644 --- a/tests/test_robot.py +++ b/tests/test_robot.py @@ -9,19 +9,20 @@ from reactivex import operators as RxOps import tests.test_header + def main(): print("Initializing Unitree Go2 robot with local planner visualization...") - + # Initialize the robot with ROS control and skills robot = UnitreeGo2( - ip=os.getenv('ROBOT_IP'), + ip=os.getenv("ROBOT_IP"), ros_control=UnitreeROSControl(), skills=MyUnitreeSkills(), ) # Get the camera stream video_stream = robot.get_ros_video_stream() - + # The local planner visualization stream is created during robot initialization local_planner_stream = robot.local_planner_viz_stream @@ -30,21 +31,15 @@ def main(): RxOps.map(lambda x: x if x is not None else None), RxOps.filter(lambda x: x is not None), ) - + goal_following_thread = None try: # Set up web interface with both streams - streams = { - "camera": video_stream, - "local_planner": local_planner_stream - } - + streams = {"camera": video_stream, "local_planner": local_planner_stream} + # Create and start the web interface - web_interface = RobotWebInterface( - port=5555, - **streams - ) - + web_interface = RobotWebInterface(port=5555, **streams) + # Wait for initialization print("Waiting for camera and systems to initialize...") time.sleep(2) @@ -53,23 +48,18 @@ def main(): print("Starting navigation to local goal (2m ahead) in a separate thread...") goal_following_thread = threading.Thread( target=navigate_to_goal_local, - kwargs={ - 'robot': robot, - 'goal_xy_robot': (3.0, 0.0), - 'distance': 0.0, - 'timeout': 300 - }, - daemon=True + kwargs={"robot": robot, "goal_xy_robot": (3.0, 0.0), "distance": 0.0, "timeout": 300}, + daemon=True, ) goal_following_thread.start() print("Robot streams running") print("Web interface available at http://localhost:5555") print("Press Ctrl+C to exit") - + # Start web server (blocking call) web_interface.run() - + except KeyboardInterrupt: print("\nInterrupted by user") except Exception as e: diff --git a/tests/test_rtsp_video_provider.py b/tests/test_rtsp_video_provider.py index 0afa7b95cf..e3824740a6 100644 --- a/tests/test_rtsp_video_provider.py +++ b/tests/test_rtsp_video_provider.py @@ -36,17 +36,20 @@ # Load environment variables from .env file from dotenv import load_dotenv + load_dotenv() # RTSP URL must be provided as a command-line argument or environment variable RTSP_URL = os.environ.get("TEST_RTSP_URL", "") if len(sys.argv) > 1: - RTSP_URL = sys.argv[1] # Allow overriding with command-line argument + RTSP_URL = sys.argv[1] # Allow overriding with command-line argument elif RTSP_URL == "": - print("Please provide an RTSP URL for testing.") - print("You can set the TEST_RTSP_URL environment variable or pass it as a command-line argument.") - print("Example: python -m dimos.stream.rtsp_video_provider rtsp://...") - sys.exit(1) + print("Please provide an RTSP URL for testing.") + print( + "You can set the TEST_RTSP_URL environment variable or pass it as a command-line argument." + ) + print("Example: python -m dimos.stream.rtsp_video_provider rtsp://...") + sys.exit(1) logger.info(f"Attempting to connect to provided RTSP URL.") provider = RtspVideoProvider(dev_name="TestRtspCam", rtsp_url=RTSP_URL) @@ -56,34 +59,40 @@ logger.info("Subscribing to observable...") frame_counter = 0 -start_time = time.monotonic() # Re-initialize start_time -last_log_time = start_time # Keep this for interval timing +start_time = time.monotonic() # Re-initialize start_time +last_log_time = start_time # Keep this for interval timing # Create a subject for ffmpeg responses ffmpeg_response_subject = rx.subject.Subject() ffmpeg_response_stream = ffmpeg_response_subject.pipe(ops.observe_on(get_scheduler()), ops.share()) + def process_frame(frame: np.ndarray): """Callback function executed for each received frame.""" - global frame_counter, last_log_time, start_time # Add start_time to global + global frame_counter, last_log_time, start_time # Add start_time to global frame_counter += 1 current_time = time.monotonic() # Log stats periodically (e.g., every 5 seconds) if current_time - last_log_time >= 5.0: - total_elapsed_time = current_time - start_time # Calculate total elapsed time + total_elapsed_time = current_time - start_time # Calculate total elapsed time avg_fps = frame_counter / total_elapsed_time if total_elapsed_time > 0 else 0 logger.info(f"Received frame {frame_counter}. Shape: {frame.shape}. Avg FPS: {avg_fps:.2f}") - ffmpeg_response_subject.on_next(f"Received frame {frame_counter}. Shape: {frame.shape}. Avg FPS: {avg_fps:.2f}") - last_log_time = current_time # Update log time for the next interval + ffmpeg_response_subject.on_next( + f"Received frame {frame_counter}. Shape: {frame.shape}. Avg FPS: {avg_fps:.2f}" + ) + last_log_time = current_time # Update log time for the next interval + def handle_error(error: Exception): """Callback function executed if the observable stream errors.""" - logger.error(f"Stream error: {error}", exc_info=True) # Log with traceback + logger.error(f"Stream error: {error}", exc_info=True) # Log with traceback + def handle_completion(): """Callback function executed when the observable stream completes.""" logger.info("Stream completed.") + # Subscribe to the observable stream processor = FrameProcessor() subscription = video_stream_observable.pipe( @@ -91,22 +100,16 @@ def handle_completion(): ops.observe_on(get_scheduler()), ops.share(), vops.with_jpeg_export(processor, suffix="reolink_", save_limit=30, loop=True), -).subscribe( - on_next=process_frame, - on_error=handle_error, - on_completed=handle_completion -) - -streams = { - "reolink_video": video_stream_observable -} +).subscribe(on_next=process_frame, on_error=handle_error, on_completed=handle_completion) + +streams = {"reolink_video": video_stream_observable} text_streams = { "ffmpeg_responses": ffmpeg_response_stream, } web_interface = RobotWebInterface(port=5555, text_streams=text_streams, **streams) -web_interface.run() # This may block the main thread +web_interface.run() # This may block the main thread # TODO: Redo disposal / keep-alive loop @@ -135,9 +138,9 @@ def handle_completion(): print("Cleanup finished.") # Final check (optional, for debugging) -time.sleep(1) # Give background threads a moment +time.sleep(1) # Give background threads a moment final_process = provider._ffmpeg_process if final_process and final_process.poll() is None: - print(f"WARNING: ffmpeg process (PID: {final_process.pid}) may still be running after cleanup!") + print(f"WARNING: ffmpeg process (PID: {final_process.pid}) may still be running after cleanup!") else: - print("ffmpeg process appears terminated.") \ No newline at end of file + print("ffmpeg process appears terminated.") diff --git a/tests/test_semantic_seg_robot.py b/tests/test_semantic_seg_robot.py index 88f40b6755..eccc5dc84e 100644 --- a/tests/test_semantic_seg_robot.py +++ b/tests/test_semantic_seg_robot.py @@ -23,25 +23,29 @@ def main(): # Create a queue for thread communication (limit to prevent memory issues) frame_queue = queue.Queue(maxsize=5) stop_event = threading.Event() - + # Unitree Go2 camera parameters at 1080p camera_params = { - 'resolution': (1920, 1080), # 1080p resolution - 'focal_length': 3.2, # mm - 'sensor_size': (4.8, 3.6) # mm (1/4" sensor) + "resolution": (1920, 1080), # 1080p resolution + "focal_length": 3.2, # mm + "sensor_size": (4.8, 3.6), # mm (1/4" sensor) } - + # Initialize video provider and segmentation stream - #video_provider = VideoProvider("test_camera", video_source=0) - robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - ros_control=UnitreeROSControl(),) - - seg_stream = SemanticSegmentationStream(enable_mono_depth=False, camera_params=camera_params, gt_depth_scale=512.0) - + # video_provider = VideoProvider("test_camera", video_source=0) + robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), + ros_control=UnitreeROSControl(), + ) + + seg_stream = SemanticSegmentationStream( + enable_mono_depth=False, camera_params=camera_params, gt_depth_scale=512.0 + ) + # Create streams video_stream = robot.get_ros_video_stream(fps=5) segmentation_stream = seg_stream.create_stream(video_stream) - + # Define callbacks for the segmentation stream def on_next(segmentation): if stop_event.is_set(): @@ -53,7 +57,7 @@ def on_next(segmentation): height, width = vis_frame.shape[:2] depth_height, depth_width = depth_viz.shape[:2] - # Resize depth visualization to match segmentation height + # Resize depth visualization to match segmentation height # (maintaining aspect ratio if needed) depth_resized = cv2.resize(depth_viz, (int(depth_width * height / depth_height), height)) @@ -63,7 +67,9 @@ def on_next(segmentation): # Add labels font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(combined_viz, "Semantic Segmentation", (10, 30), font, 0.8, (255, 255, 255), 2) - cv2.putText(combined_viz, "Depth Estimation", (width + 10, 30), font, 0.8, (255, 255, 255), 2) + cv2.putText( + combined_viz, "Depth Estimation", (width + 10, 30), font, 0.8, (255, 255, 255), 2 + ) # Put frame in queue for main thread to display (non-blocking) try: @@ -71,18 +77,18 @@ def on_next(segmentation): except queue.Full: # Skip frame if queue is full pass - + def on_error(error): print(f"Error: {error}") stop_event.set() - + def on_completed(): print("Stream completed") stop_event.set() - + # Start the subscription subscription = None - + try: # Subscribe to start processing in background thread print_emission_args = { @@ -91,7 +97,6 @@ def on_completed(): "counts": {}, } - frame_processor = FrameProcessor(delete_on_init=True) subscription = segmentation_stream.pipe( MyOps.print_emission(id="A", **print_emission_args), @@ -101,7 +106,7 @@ def on_completed(): MyOps.print_emission(id="C", **print_emission_args), RxOps.filter(lambda x: x is not None), MyOps.print_emission(id="D", **print_emission_args), - # MyVideoOps.with_jpeg_export(frame_processor=frame_processor, suffix="_frame_"), + # MyVideoOps.with_jpeg_export(frame_processor=frame_processor, suffix="_frame_"), MyOps.print_emission(id="E", **print_emission_args), ) @@ -112,21 +117,21 @@ def on_completed(): } fast_api_server = RobotWebInterface(port=5555, **streams) fast_api_server.run() - + except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") finally: # Signal threads to stop stop_event.set() - + # Clean up resources if subscription: subscription.dispose() - + seg_stream.cleanup() cv2.destroyAllWindows() print("Cleanup complete") if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/test_semantic_seg_robot_agent.py b/tests/test_semantic_seg_robot_agent.py index 527d1286a3..8aa102e16a 100644 --- a/tests/test_semantic_seg_robot_agent.py +++ b/tests/test_semantic_seg_robot_agent.py @@ -15,28 +15,29 @@ from dimos.agents.agent import OpenAIAgent from dimos.utils.threadpool import get_scheduler + def main(): # Unitree Go2 camera parameters at 1080p camera_params = { - 'resolution': (1920, 1080), # 1080p resolution - 'focal_length': 3.2, # mm - 'sensor_size': (4.8, 3.6) # mm (1/4" sensor) + "resolution": (1920, 1080), # 1080p resolution + "focal_length": 3.2, # mm + "sensor_size": (4.8, 3.6), # mm (1/4" sensor) } - - robot = UnitreeGo2(ip=os.getenv('ROBOT_IP'), - ros_control=UnitreeROSControl(), - skills=MyUnitreeSkills()) - - seg_stream = SemanticSegmentationStream(enable_mono_depth=True, camera_params=camera_params, gt_depth_scale=512.0) - + + robot = UnitreeGo2( + ip=os.getenv("ROBOT_IP"), ros_control=UnitreeROSControl(), skills=MyUnitreeSkills() + ) + + seg_stream = SemanticSegmentationStream( + enable_mono_depth=True, camera_params=camera_params, gt_depth_scale=512.0 + ) + # Create streams video_stream = robot.get_ros_video_stream(fps=5) segmentation_stream = seg_stream.create_stream( - video_stream.pipe( - MyVideoOps.with_fps_sampling(fps=.5) - ) + video_stream.pipe(MyVideoOps.with_fps_sampling(fps=0.5)) ) - # Throttling to slowdown SegmentationAgent calls + # Throttling to slowdown SegmentationAgent calls # TODO: add Agent parameter to handle this called api_call_interval frame_processor = FrameProcessor(delete_on_init=True) @@ -57,25 +58,33 @@ def main(): RxOps.share(), RxOps.map(lambda x: x.metadata["objects"] if x is not None else None), RxOps.filter(lambda x: x is not None), - RxOps.map(lambda objects: "\n".join( - f"Object {obj['object_id']}: {obj['label']} (confidence: {obj['prob']:.2f})" + - (f", depth: {obj['depth']:.2f}m" if 'depth' in obj else "") - for obj in objects - ) if objects else "No objects detected."), + RxOps.map( + lambda objects: "\n".join( + f"Object {obj['object_id']}: {obj['label']} (confidence: {obj['prob']:.2f})" + + (f", depth: {obj['depth']:.2f}m" if "depth" in obj else "") + for obj in objects + ) + if objects + else "No objects detected." + ), ) text_query_stream = Subject() - + # Combine text query with latest object data when a new text query arrives enriched_query_stream = text_query_stream.pipe( RxOps.with_latest_from(object_stream), - RxOps.map(lambda combined: { - "query": combined[0], - "objects": combined[1] if len(combined) > 1 else "No object data available" - }), + RxOps.map( + lambda combined: { + "query": combined[0], + "objects": combined[1] if len(combined) > 1 else "No object data available", + } + ), RxOps.map(lambda data: f"{data['query']}\n\nCurrent objects detected:\n{data['objects']}"), - RxOps.do_action(lambda x: print(f"\033[34mEnriched query: {x.split(chr(10))[0]}\033[0m") or - [print(f"\033[34m{line}\033[0m") for line in x.split(chr(10))[1:]]), + RxOps.do_action( + lambda x: print(f"\033[34mEnriched query: {x.split(chr(10))[0]}\033[0m") + or [print(f"\033[34m{line}\033[0m") for line in x.split(chr(10))[1:]] + ), ) segmentation_agent = OpenAIAgent( @@ -85,7 +94,7 @@ def main(): input_query_stream=enriched_query_stream, process_all_inputs=False, pool_scheduler=get_scheduler(), - skills=robot.get_skills() + skills=robot.get_skills(), ) agent_response_stream = segmentation_agent.get_response_observable() @@ -104,7 +113,7 @@ def main(): try: fast_api_server = RobotWebInterface(port=5555, text_streams=text_streams, **streams) - fast_api_server.query_stream.subscribe(lambda x: text_query_stream.on_next(x)) + fast_api_server.query_stream.subscribe(lambda x: text_query_stream.on_next(x)) fast_api_server.run() except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") @@ -113,5 +122,6 @@ def main(): cv2.destroyAllWindows() print("Cleanup complete") + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/test_semantic_seg_webcam.py b/tests/test_semantic_seg_webcam.py index 7e387ae597..444e4cd629 100644 --- a/tests/test_semantic_seg_webcam.py +++ b/tests/test_semantic_seg_webcam.py @@ -11,26 +11,29 @@ from dimos.stream.video_provider import VideoProvider from dimos.perception.semantic_seg import SemanticSegmentationStream + def main(): # Create a queue for thread communication (limit to prevent memory issues) frame_queue = queue.Queue(maxsize=5) stop_event = threading.Event() - + # Logitech C920e camera parameters at 480p camera_params = { - 'resolution': (640, 480), # 480p resolution - 'focal_length': 3.67, # mm - 'sensor_size': (4.8, 3.6) # mm (1/4" sensor) + "resolution": (640, 480), # 480p resolution + "focal_length": 3.67, # mm + "sensor_size": (4.8, 3.6), # mm (1/4" sensor) } - + # Initialize video provider and segmentation stream video_provider = VideoProvider("test_camera", video_source=0) - seg_stream = SemanticSegmentationStream(enable_mono_depth=True, camera_params=camera_params, gt_depth_scale=512.0) - + seg_stream = SemanticSegmentationStream( + enable_mono_depth=True, camera_params=camera_params, gt_depth_scale=512.0 + ) + # Create streams video_stream = video_provider.capture_video_as_observable(realtime=False, fps=5) segmentation_stream = seg_stream.create_stream(video_stream) - + # Define callbacks for the segmentation stream def on_next(segmentation): if stop_event.is_set(): @@ -43,7 +46,7 @@ def on_next(segmentation): height, width = vis_frame.shape[:2] depth_height, depth_width = depth_viz.shape[:2] - # Resize depth visualization to match segmentation height + # Resize depth visualization to match segmentation height # (maintaining aspect ratio if needed) depth_resized = cv2.resize(depth_viz, (int(depth_width * height / depth_height), height)) @@ -53,7 +56,9 @@ def on_next(segmentation): # Add labels font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(combined_viz, "Semantic Segmentation", (10, 30), font, 0.8, (255, 255, 255), 2) - cv2.putText(combined_viz, "Depth Estimation", (width + 10, 30), font, 0.8, (255, 255, 255), 2) + cv2.putText( + combined_viz, "Depth Estimation", (width + 10, 30), font, 0.8, (255, 255, 255), 2 + ) # Put frame in queue for main thread to display (non-blocking) try: @@ -61,58 +66,56 @@ def on_next(segmentation): except queue.Full: # Skip frame if queue is full pass - + def on_error(error): print(f"Error: {error}") stop_event.set() - + def on_completed(): print("Stream completed") stop_event.set() - + # Start the subscription subscription = None - + try: # Subscribe to start processing in background thread subscription = segmentation_stream.subscribe( - on_next=on_next, - on_error=on_error, - on_completed=on_completed + on_next=on_next, on_error=on_error, on_completed=on_completed ) - + print("Semantic segmentation visualization started. Press 'q' to exit.") - + # Main thread loop for displaying frames while not stop_event.is_set(): try: # Get frame with timeout (allows checking stop_event periodically) combined_viz = frame_queue.get(timeout=1.0) - + # Display the frame in main thread cv2.imshow("Semantic Segmentation", combined_viz) # Check for exit key - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break - + except queue.Empty: # No frame available, check if we should continue - if cv2.waitKey(1) & 0xFF == ord('q'): + if cv2.waitKey(1) & 0xFF == ord("q"): print("Exit key pressed") break continue - + except KeyboardInterrupt: print("\nKeyboard interrupt received. Stopping...") finally: # Signal threads to stop stop_event.set() - + # Clean up resources if subscription: subscription.dispose() - + video_provider.dispose_all() seg_stream.cleanup() cv2.destroyAllWindows() @@ -120,4 +123,4 @@ def on_completed(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/test_skills.py b/tests/test_skills.py index ae77facc38..0d4b7f2ff8 100644 --- a/tests/test_skills.py +++ b/tests/test_skills.py @@ -31,10 +31,10 @@ class TestSkill(AbstractSkill): _called: bool = False - def __init__(self, *args, **kwargs): + def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self._called = False - + def __call__(self): self._called = True return "TestSkill executed successfully" @@ -48,31 +48,31 @@ def setUp(self): self.robot = MockRobot() self.skill_library = MyUnitreeSkills(robot=self.robot) self.skill_library.initialize_skills() - + def test_skill_iteration(self): """Test that skills can be properly iterated in the skill library.""" skills_count = 0 for skill in self.skill_library: skills_count += 1 - self.assertTrue(hasattr(skill, '__name__')) + self.assertTrue(hasattr(skill, "__name__")) self.assertTrue(issubclass(skill, AbstractSkill)) - + self.assertGreater(skills_count, 0, "Skill library should contain at least one skill") - + def test_skill_registration(self): """Test that skills can be properly registered in the skill library.""" # Clear existing skills for isolated test self.skill_library = MyUnitreeSkills(robot=self.robot) original_count = len(list(self.skill_library)) - + # Add a custom test skill test_skill = TestSkill self.skill_library.add(test_skill) - + # Verify the skill was added new_count = len(list(self.skill_library)) self.assertEqual(new_count, original_count + 1) - + # Check if the skill can be found by name found = False for skill in self.skill_library: @@ -80,7 +80,7 @@ def test_skill_registration(self): found = True break self.assertTrue(found, "Added skill should be found in skill library") - + def test_skill_direct_execution(self): """Test that a skill can be executed directly.""" test_skill = TestSkill() @@ -88,19 +88,19 @@ def test_skill_direct_execution(self): result = test_skill() self.assertTrue(test_skill._called) self.assertEqual(result, "TestSkill executed successfully") - + def test_skill_library_execution(self): """Test that a skill can be executed through the skill library.""" # Add our test skill to the library test_skill = TestSkill self.skill_library.add(test_skill) - + # Create an instance to confirm it was executed - with mock.patch.object(TestSkill, '__call__', return_value="Success") as mock_call: + with mock.patch.object(TestSkill, "__call__", return_value="Success") as mock_call: result = self.skill_library.call("TestSkill") mock_call.assert_called_once() self.assertEqual(result, "Success") - + def test_skill_not_found(self): """Test that calling a non-existent skill raises an appropriate error.""" with self.assertRaises(ValueError): @@ -115,34 +115,34 @@ def setUp(self): self.robot = MockRobot() self.skill_library = MyUnitreeSkills(robot=self.robot) self.skill_library.initialize_skills() - + # Add a test skill self.skill_library.add(TestSkill) - + # Create the agent self.agent = OpenAIAgent( dev_name="SkillTestAgent", system_query="You are a skill testing agent. When prompted to perform an action, use the appropriate skill.", - skills=self.skill_library + skills=self.skill_library, ) - - @mock.patch('dimos.agents.agent.OpenAIAgent.run_observable_query') + + @mock.patch("dimos.agents.agent.OpenAIAgent.run_observable_query") def test_agent_skill_identification(self, mock_query): """Test that the agent can identify skills based on natural language.""" # Mock the agent response mock_response = mock.MagicMock() mock_response.run.return_value = "I found the TestSkill and executed it." mock_query.return_value = mock_response - + # Run the test response = self.agent.run_observable_query("Please run the test skill").run() - + # Assertions mock_query.assert_called_once_with("Please run the test skill") self.assertEqual(response, "I found the TestSkill and executed it.") - - @mock.patch.object(TestSkill, '__call__') - @mock.patch('dimos.agents.agent.OpenAIAgent.run_observable_query') + + @mock.patch.object(TestSkill, "__call__") + @mock.patch("dimos.agents.agent.OpenAIAgent.run_observable_query") def test_agent_skill_execution(self, mock_query, mock_skill_call): """Test that the agent can execute skills properly.""" # Mock the agent and skill call @@ -150,30 +150,31 @@ def test_agent_skill_execution(self, mock_query, mock_skill_call): mock_response = mock.MagicMock() mock_response.run.return_value = "Executed TestSkill successfully." mock_query.return_value = mock_response - + # Run the test response = self.agent.run_observable_query("Execute the TestSkill skill").run() - + # We can't directly verify the skill was called since our mocking setup # doesn't capture the internal skill execution of the agent, but we can # verify the agent was properly called mock_query.assert_called_once_with("Execute the TestSkill skill") self.assertEqual(response, "Executed TestSkill successfully.") - + def test_agent_multi_skill_registration(self): """Test that multiple skills can be registered with an agent.""" + # Create a new skill class AnotherTestSkill(AbstractSkill): def __call__(self): return "Another test skill executed" - + # Register the new skill initial_count = len(list(self.skill_library)) self.skill_library.add(AnotherTestSkill) - + # Verify two distinct skills now exist self.assertEqual(len(list(self.skill_library)), initial_count + 1) - + # Verify both skills are found by name skill_names = [skill.__name__ for skill in self.skill_library] self.assertIn("TestSkill", skill_names) diff --git a/tests/test_skills_rest.py b/tests/test_skills_rest.py index 36ca9dc366..70a15fcfd5 100644 --- a/tests/test_skills_rest.py +++ b/tests/test_skills_rest.py @@ -54,21 +54,20 @@ IMPORTANT: Only return the response directly asked of the user. E.G. if the user asks for the time, only return the time. If the user asks for the weather, only return the weather. - """), + """ + ), model_name="claude-3-7-sonnet-latest", - thinking_budget_tokens=2000 + thinking_budget_tokens=2000, ) # Subscribe to agent responses and send them to the subject -agent.get_response_observable().subscribe( - lambda x: agent_response_subject.on_next(x) -) +agent.get_response_observable().subscribe(lambda x: agent_response_subject.on_next(x)) # Start the web interface web_interface.run() # Run this query in the web interface: -# -# Make a web request to nist to get the current time. +# +# Make a web request to nist to get the current time. # You should use http://worldclockapi.com/api/json/utc/now -# \ No newline at end of file +# diff --git a/tests/test_spatial_memory.py b/tests/test_spatial_memory.py index c520674b9f..b400749cb4 100644 --- a/tests/test_spatial_memory.py +++ b/tests/test_spatial_memory.py @@ -32,70 +32,67 @@ from dimos.robot.unitree.unitree_ros_control import UnitreeROSControl from dimos.perception.spatial_perception import SpatialMemory + def extract_position(transform): """Extract position coordinates from a transform message""" if transform is None: return (0, 0, 0) - + pos = transform.transform.translation return (pos.x, pos.y, pos.z) + def setup_persistent_chroma_db(db_path="chromadb_data"): """ Set up a persistent ChromaDB database at the specified path. - + Args: db_path: Path to store the ChromaDB database - + Returns: The ChromaDB client instance """ # Create a persistent ChromaDB client full_db_path = os.path.join("/home/stash/dimensional/dimos/assets/test_spatial_memory", db_path) print(f"Setting up persistent ChromaDB at: {full_db_path}") - + # Ensure the directory exists os.makedirs(full_db_path, exist_ok=True) - + return chromadb.PersistentClient(path=full_db_path) + def main(): print("Starting spatial memory test...") - + # Initialize ROS control and robot - ros_control = UnitreeROSControl( - node_name="spatial_memory_test", - mock_connection=False - ) - - robot = UnitreeGo2( - ros_control=ros_control, - ip=os.getenv('ROBOT_IP') - ) - + ros_control = UnitreeROSControl(node_name="spatial_memory_test", mock_connection=False) + + robot = UnitreeGo2(ros_control=ros_control, ip=os.getenv("ROBOT_IP")) + # Create counters for tracking frame_count = 0 transform_count = 0 stored_count = 0 - + print("Setting up video stream...") - video_stream = robot.get_ros_video_stream() - + video_stream = robot.get_ros_video_stream() + # Create transform stream at 1 Hz print("Setting up transform stream...") transform_stream = ros_control.get_transform_stream( child_frame="map", parent_frame="base_link", - rate_hz=1.0 # 1 transform per second + rate_hz=1.0, # 1 transform per second ) - + # Setup output directory for visual memory visual_memory_dir = "/home/stash/dimensional/dimos/assets/test_spatial_memory" os.makedirs(visual_memory_dir, exist_ok=True) - + # Setup persistent storage path for visual memory visual_memory_path = os.path.join(visual_memory_dir, "visual_memory.pkl") - + # Try to load existing visual memory if it exists if os.path.exists(visual_memory_path): try: @@ -108,10 +105,10 @@ def main(): else: print("No existing visual memory found. Starting with empty visual memory.") visual_memory = VisualMemory(output_dir=visual_memory_dir) - + # Setup a persistent database for ChromaDB db_client = setup_persistent_chroma_db() - + # Create spatial perception instance with persistent storage print("Creating SpatialMemory with persistent vector database...") spatial_memory = SpatialMemory( @@ -119,167 +116,182 @@ def main(): min_distance_threshold=1, # Store frames every 1 meter min_time_threshold=1, # Store frames at least every 1 second chroma_client=db_client, # Use the persistent client - visual_memory=visual_memory # Use the visual memory we loaded or created + visual_memory=visual_memory, # Use the visual memory we loaded or created ) - + # Combine streams using combine_latest # This will pair up items properly without buffering combined_stream = reactivex.combine_latest(video_stream, transform_stream).pipe( - ops.map(lambda pair: { - "frame": pair[0], # First element is the frame - "position": extract_position(pair[1]) # Second element is the transform - }) + ops.map( + lambda pair: { + "frame": pair[0], # First element is the frame + "position": extract_position(pair[1]), # Second element is the transform + } + ) ) - + # Process with spatial memory result_stream = spatial_memory.process_stream(combined_stream) - + # Simple callback to track stored frames and save them to the assets directory def on_stored_frame(result): nonlocal stored_count # Only count actually stored frames (not debug frames) - if not result.get('stored', True) == False: + if not result.get("stored", True) == False: stored_count += 1 - pos = result['position'] + pos = result["position"] print(f"\nStored frame #{stored_count} at ({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})") - + # Save the frame to the assets directory - if 'frame' in result: + if "frame" in result: frame_filename = f"/home/stash/dimensional/dimos/assets/test_spatial_memory/frame_{stored_count:03d}.jpg" - cv2.imwrite(frame_filename, result['frame']) + cv2.imwrite(frame_filename, result["frame"]) print(f"Saved frame to {frame_filename}") - + # Subscribe to results print("Subscribing to spatial perception results...") result_subscription = result_stream.subscribe(on_stored_frame) - + print("\nRunning until interrupted...") try: while True: - time.sleep(1.0) + time.sleep(1.0) print(f"Running: {stored_count} frames stored so far", end="\r") except KeyboardInterrupt: print("\nTest interrupted by user") finally: # Clean up resources print("\nCleaning up...") - if 'result_subscription' in locals(): + if "result_subscription" in locals(): result_subscription.dispose() - + # Visualize spatial memory with multiple object queries visualize_spatial_memory_with_objects( - spatial_memory, - objects=["kitchen", "conference room", "vacuum", "office", "bathroom", "boxes", "telephone booth"], - output_filename="spatial_memory_map.png" + spatial_memory, + objects=[ + "kitchen", + "conference room", + "vacuum", + "office", + "bathroom", + "boxes", + "telephone booth", + ], + output_filename="spatial_memory_map.png", ) - + # Save visual memory to disk for later use saved_path = spatial_memory.vector_db.visual_memory.save("visual_memory.pkl") print(f"Saved {spatial_memory.vector_db.visual_memory.count()} images to disk at {saved_path}") - -def visualize_spatial_memory_with_objects(spatial_memory, objects, output_filename="spatial_memory_map.png"): + + +def visualize_spatial_memory_with_objects( + spatial_memory, objects, output_filename="spatial_memory_map.png" +): """ Visualize a spatial memory map with multiple labeled objects. - + Args: spatial_memory: SpatialMemory instance objects: List of object names to query and visualize (e.g. ["kitchen", "office"]) output_filename: Filename to save the visualization """ # Define colors for different objects - will cycle through these - colors = ['red', 'green', 'orange', 'purple', 'brown', 'cyan', 'magenta', 'yellow'] - + colors = ["red", "green", "orange", "purple", "brown", "cyan", "magenta", "yellow"] + # Get all stored locations for background locations = spatial_memory.vector_db.get_all_locations() if not locations: print("No locations stored in spatial memory.") return - + # Extract coordinates from all stored locations if len(locations[0]) >= 3: x_coords = [loc[0] for loc in locations] y_coords = [loc[1] for loc in locations] else: x_coords, y_coords = zip(*locations) - + # Create figure plt.figure(figsize=(12, 10)) - + # Plot all points in blue - plt.scatter(x_coords, y_coords, c='blue', s=50, alpha=0.5, label='All Frames') - + plt.scatter(x_coords, y_coords, c="blue", s=50, alpha=0.5, label="All Frames") + # Container for all object coordinates object_coords = {} - + # Query for each object and store the result for i, obj in enumerate(objects): color = colors[i % len(colors)] # Cycle through colors print(f"\nProcessing {obj} query for visualization...") - + # Get best match for this object results = spatial_memory.query_by_text(obj, limit=1) if not results: print(f"No results found for '{obj}'") continue - + # Get the first (best) result result = results[0] - metadata = result['metadata'] - + metadata = result["metadata"] + # Extract coordinates from the first metadata item if isinstance(metadata, list) and metadata: metadata = metadata[0] - - if isinstance(metadata, dict) and 'x' in metadata and 'y' in metadata: - x = metadata.get('x', 0) - y = metadata.get('y', 0) - + + if isinstance(metadata, dict) and "x" in metadata and "y" in metadata: + x = metadata.get("x", 0) + y = metadata.get("y", 0) + # Store coordinates for this object object_coords[obj] = (x, y) - + # Plot this object's position plt.scatter([x], [y], c=color, s=100, alpha=0.8, label=obj.title()) - + # Add annotation - obj_abbrev = obj[0].upper() if len(obj) > 0 else 'X' - plt.annotate(f"{obj_abbrev}", (x, y), textcoords="offset points", - xytext=(0,10), ha='center') - + obj_abbrev = obj[0].upper() if len(obj) > 0 else "X" + plt.annotate( + f"{obj_abbrev}", (x, y), textcoords="offset points", xytext=(0, 10), ha="center" + ) + # Save the image to a file using the object name - if 'image' in result and result['image'] is not None: + if "image" in result and result["image"] is not None: # Clean the object name to make it suitable for a filename - clean_name = obj.replace(' ', '_').lower() + clean_name = obj.replace(" ", "_").lower() output_img_filename = f"{clean_name}_result.jpg" cv2.imwrite(output_img_filename, result["image"]) print(f"Saved {obj} image to {output_img_filename}") - + # Finalize the plot plt.title("Spatial Memory Map with Query Results") plt.xlabel("X Position (m)") plt.ylabel("Y Position (m)") plt.grid(True) - plt.axis('equal') + plt.axis("equal") plt.legend() - + # Add origin circle - plt.gca().add_patch(Circle((0, 0), 1.0, fill=False, color='blue', linestyle='--')) - + plt.gca().add_patch(Circle((0, 0), 1.0, fill=False, color="blue", linestyle="--")) + # Save the visualization plt.savefig(output_filename, dpi=300) print(f"Saved enhanced map visualization to {output_filename}") - + return object_coords - + # Final cleanup print("Performing final cleanup...") spatial_memory.cleanup() - + try: robot.cleanup() except Exception as e: print(f"Error during robot cleanup: {e}") - + print("Test completed successfully") + if __name__ == "__main__": main() diff --git a/tests/test_spatial_memory_query.py b/tests/test_spatial_memory_query.py index 2919575428..a0e77e9444 100644 --- a/tests/test_spatial_memory_query.py +++ b/tests/test_spatial_memory_query.py @@ -19,6 +19,7 @@ python test_spatial_memory_query.py --query "kitchen table" --limit 5 --threshold 0.7 --save-all python test_spatial_memory_query.py --query "robot" --limit 3 --save-one """ + import os import sys import argparse @@ -32,45 +33,59 @@ from dimos.perception.spatial_perception import SpatialMemory from dimos.agents.memory.visual_memory import VisualMemory + def setup_persistent_chroma_db(db_path): """Set up a persistent ChromaDB client at the specified path.""" print(f"Setting up persistent ChromaDB at: {db_path}") os.makedirs(db_path, exist_ok=True) return chromadb.PersistentClient(path=db_path) + def parse_args(): """Parse command-line arguments.""" parser = argparse.ArgumentParser(description="Query spatial memory database.") - parser.add_argument("--query", type=str, default=None, - help="Text query to search for (e.g., 'kitchen table')") - parser.add_argument("--limit", type=int, default=3, - help="Maximum number of results to return") - parser.add_argument("--threshold", type=float, default=None, - help="Similarity threshold (0.0-1.0). Only return results above this threshold.") - parser.add_argument("--save-all", action="store_true", - help="Save all result images") - parser.add_argument("--save-one", action="store_true", - help="Save only the best matching image") - parser.add_argument("--visualize", action="store_true", - help="Create a visualization of all stored memory locations") - parser.add_argument("--db-path", type=str, - default="/home/stash/dimensional/dimos/assets/test_spatial_memory/chromadb_data", - help="Path to ChromaDB database") - parser.add_argument("--visual-memory-path", type=str, - default="/home/stash/dimensional/dimos/assets/test_spatial_memory/visual_memory.pkl", - help="Path to visual memory file") + parser.add_argument( + "--query", type=str, default=None, help="Text query to search for (e.g., 'kitchen table')" + ) + parser.add_argument("--limit", type=int, default=3, help="Maximum number of results to return") + parser.add_argument( + "--threshold", + type=float, + default=None, + help="Similarity threshold (0.0-1.0). Only return results above this threshold.", + ) + parser.add_argument("--save-all", action="store_true", help="Save all result images") + parser.add_argument("--save-one", action="store_true", help="Save only the best matching image") + parser.add_argument( + "--visualize", + action="store_true", + help="Create a visualization of all stored memory locations", + ) + parser.add_argument( + "--db-path", + type=str, + default="/home/stash/dimensional/dimos/assets/test_spatial_memory/chromadb_data", + help="Path to ChromaDB database", + ) + parser.add_argument( + "--visual-memory-path", + type=str, + default="/home/stash/dimensional/dimos/assets/test_spatial_memory/visual_memory.pkl", + help="Path to visual memory file", + ) return parser.parse_args() + def main(): args = parse_args() print("Loading existing spatial memory database for querying...") - + # Setup the persistent ChromaDB client db_client = setup_persistent_chroma_db(args.db_path) - + # Setup output directory for any saved results output_dir = os.path.dirname(args.visual_memory_path) - + # Load the visual memory print(f"Loading visual memory from {args.visual_memory_path}...") if os.path.exists(args.visual_memory_path): @@ -79,40 +94,41 @@ def main(): else: visual_memory = VisualMemory(output_dir=output_dir) print("No existing visual memory found. Query results won't include images.") - + # Create SpatialMemory with the existing database and visual memory spatial_memory = SpatialMemory( - collection_name="test_spatial_memory", - chroma_client=db_client, - visual_memory=visual_memory + collection_name="test_spatial_memory", chroma_client=db_client, visual_memory=visual_memory ) - + # Create a visualization if requested if args.visualize: print("\nCreating visualization of spatial memory...") common_objects = [ - "kitchen", "conference room", "vacuum", "office", - "bathroom", "boxes", "telephone booth" + "kitchen", + "conference room", + "vacuum", + "office", + "bathroom", + "boxes", + "telephone booth", ] visualize_spatial_memory_with_objects( - spatial_memory, - objects=common_objects, - output_filename="spatial_memory_map.png" + spatial_memory, objects=common_objects, output_filename="spatial_memory_map.png" ) - + # Handle query if provided if args.query: query = args.query limit = args.limit print(f"\nQuerying for: '{query}' (limit: {limit})...") - + # Run the query results = spatial_memory.query_by_text(query, limit=limit) - + if not results: print(f"No results found for query: '{query}'") return - + # Filter by threshold if specified if args.threshold is not None: print(f"Filtering results with similarity threshold: {args.threshold}") @@ -120,153 +136,162 @@ def main(): for result in results: # Distance is inverse of similarity (0 is perfect match) # Convert to similarity score (1.0 is perfect match) - similarity = 1.0 - (result.get('distance', 0) if result.get('distance') is not None else 0) + similarity = 1.0 - ( + result.get("distance", 0) if result.get("distance") is not None else 0 + ) if similarity >= args.threshold: filtered_results.append((result, similarity)) - + # Sort by similarity (highest first) filtered_results.sort(key=lambda x: x[1], reverse=True) - + if not filtered_results: print(f"No results met the similarity threshold of {args.threshold}") return - + print(f"Found {len(filtered_results)} results above threshold") results_with_scores = filtered_results else: # Add similarity scores for all results results_with_scores = [] for result in results: - similarity = 1.0 - (result.get('distance', 0) if result.get('distance') is not None else 0) + similarity = 1.0 - ( + result.get("distance", 0) if result.get("distance") is not None else 0 + ) results_with_scores.append((result, similarity)) - + # Process and display results timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") - + for i, (result, similarity) in enumerate(results_with_scores): - metadata = result.get('metadata', {}) + metadata = result.get("metadata", {}) if isinstance(metadata, list) and metadata: metadata = metadata[0] - + # Display result information - print(f"\nResult {i+1} for '{query}':") + print(f"\nResult {i + 1} for '{query}':") print(f"Similarity: {similarity:.4f} (distance: {1.0 - similarity:.4f})") - + # Extract and display position information if isinstance(metadata, dict): - x = metadata.get('x', 0) - y = metadata.get('y', 0) - z = metadata.get('z', 0) + x = metadata.get("x", 0) + y = metadata.get("y", 0) + z = metadata.get("z", 0) print(f"Position: ({x:.2f}, {y:.2f}, {z:.2f})") - if 'timestamp' in metadata: + if "timestamp" in metadata: print(f"Timestamp: {metadata['timestamp']}") - if 'frame_id' in metadata: + if "frame_id" in metadata: print(f"Frame ID: {metadata['frame_id']}") - + # Save image if requested and available - if 'image' in result and result['image'] is not None: + if "image" in result and result["image"] is not None: # Only save first image, or all images based on flags if args.save_one and i > 0: continue if not (args.save_all or args.save_one): continue - + # Create a descriptive filename - clean_query = query.replace(' ', '_').replace('/', '_').lower() - output_filename = f"{clean_query}_result_{i+1}_{timestamp}.jpg" - + clean_query = query.replace(" ", "_").replace("/", "_").lower() + output_filename = f"{clean_query}_result_{i + 1}_{timestamp}.jpg" + # Save the image cv2.imwrite(output_filename, result["image"]) print(f"Saved image to {output_filename}") - elif 'image' in result and result['image'] is None: + elif "image" in result and result["image"] is None: print("Image data not available for this result") else: - print("No query specified. Use --query \"text to search for\" to run a query.") + print('No query specified. Use --query "text to search for" to run a query.') print("Use --help to see all available options.") - + print("\nQuery completed successfully!") -def visualize_spatial_memory_with_objects(spatial_memory, objects, output_filename="spatial_memory_map.png"): + +def visualize_spatial_memory_with_objects( + spatial_memory, objects, output_filename="spatial_memory_map.png" +): """Visualize spatial memory with labeled objects.""" # Define colors for different objects - colors = ['red', 'green', 'orange', 'purple', 'brown', 'cyan', 'magenta', 'yellow'] - + colors = ["red", "green", "orange", "purple", "brown", "cyan", "magenta", "yellow"] + # Get all stored locations for background locations = spatial_memory.vector_db.get_all_locations() if not locations: print("No locations stored in spatial memory.") return - + # Extract coordinates if len(locations[0]) >= 3: x_coords = [loc[0] for loc in locations] y_coords = [loc[1] for loc in locations] else: x_coords, y_coords = zip(*locations) - + # Create figure plt.figure(figsize=(12, 10)) - plt.scatter(x_coords, y_coords, c='blue', s=50, alpha=0.5, label='All Frames') - + plt.scatter(x_coords, y_coords, c="blue", s=50, alpha=0.5, label="All Frames") + # Container for object coordinates object_coords = {} - + # Query for each object for i, obj in enumerate(objects): color = colors[i % len(colors)] print(f"Processing {obj} query for visualization...") - + # Get best match results = spatial_memory.query_by_text(obj, limit=1) if not results: print(f"No results found for '{obj}'") continue - + # Process result result = results[0] - metadata = result['metadata'] - + metadata = result["metadata"] + if isinstance(metadata, list) and metadata: metadata = metadata[0] - - if isinstance(metadata, dict) and 'x' in metadata and 'y' in metadata: - x = metadata.get('x', 0) - y = metadata.get('y', 0) - + + if isinstance(metadata, dict) and "x" in metadata and "y" in metadata: + x = metadata.get("x", 0) + y = metadata.get("y", 0) + # Store coordinates object_coords[obj] = (x, y) - + # Plot position plt.scatter([x], [y], c=color, s=100, alpha=0.8, label=obj.title()) - + # Add annotation - obj_abbrev = obj[0].upper() if len(obj) > 0 else 'X' - plt.annotate(f"{obj_abbrev}", (x, y), textcoords="offset points", - xytext=(0,10), ha='center') - + obj_abbrev = obj[0].upper() if len(obj) > 0 else "X" + plt.annotate( + f"{obj_abbrev}", (x, y), textcoords="offset points", xytext=(0, 10), ha="center" + ) + # Save image if available - if 'image' in result and result['image'] is not None: - clean_name = obj.replace(' ', '_').lower() + if "image" in result and result["image"] is not None: + clean_name = obj.replace(" ", "_").lower() output_img_filename = f"{clean_name}_result.jpg" cv2.imwrite(output_img_filename, result["image"]) print(f"Saved {obj} image to {output_img_filename}") - + # Finalize plot plt.title("Spatial Memory Map with Query Results") plt.xlabel("X Position (m)") plt.ylabel("Y Position (m)") plt.grid(True) - plt.axis('equal') + plt.axis("equal") plt.legend() - + # Add origin marker - plt.gca().add_patch(plt.Circle((0, 0), 1.0, fill=False, color='blue', linestyle='--')) - + plt.gca().add_patch(plt.Circle((0, 0), 1.0, fill=False, color="blue", linestyle="--")) + # Save visualization plt.savefig(output_filename, dpi=300) print(f"Saved visualization to {output_filename}") - + return object_coords + if __name__ == "__main__": main() diff --git a/tests/test_standalone_chromadb.py b/tests/test_standalone_chromadb.py index da9ef9e691..067303b572 100644 --- a/tests/test_standalone_chromadb.py +++ b/tests/test_standalone_chromadb.py @@ -24,6 +24,7 @@ embedding_function=embeddings, ) + def add_vector(vector_id, vector_data): """Add a vector to the ChromaDB collection.""" if not db_connection: @@ -34,6 +35,7 @@ def add_vector(vector_id, vector_data): metadatas=[{"name": vector_id}], ) + add_vector("id0", "Food") add_vector("id1", "Cat") add_vector("id2", "Mouse") @@ -50,22 +52,22 @@ def add_vector(vector_id, vector_data): def get_vector(vector_id): """Retrieve a vector from the ChromaDB by its identifier.""" - result = db_connection.get(include=['embeddings'], ids=[vector_id]) + result = db_connection.get(include=["embeddings"], ids=[vector_id]) return result + print(get_vector("id1")) # print(get_vector("id3")) # print(get_vector("id0")) # print(get_vector("id2")) + def query(query_texts, n_results=2): """Query the collection with a specific text and return up to n results.""" if not db_connection: raise Exception("Collection not initialized. Call connect() first.") - return db_connection.similarity_search( - query=query_texts, - k=n_results - ) + return db_connection.similarity_search(query=query_texts, k=n_results) + results = query("Colors") print(results) diff --git a/tests/test_standalone_fastapi.py b/tests/test_standalone_fastapi.py index 86775930a1..8cfec64ae0 100644 --- a/tests/test_standalone_fastapi.py +++ b/tests/test_standalone_fastapi.py @@ -2,6 +2,7 @@ import os import logging + logging.basicConfig(level=logging.DEBUG) from fastapi import FastAPI, Response @@ -11,25 +12,29 @@ app = FastAPI() -# Note: Chrome does not allow for loading more than 6 simultaneous -# video streams. Use Safari or another browser for utilizing +# Note: Chrome does not allow for loading more than 6 simultaneous +# video streams. Use Safari or another browser for utilizing # multiple simultaneous streams. Possibly build out functionality # that will stop live streams. + @app.get("/") async def root(): pid = os.getpid() # Get the current process ID return {"message": f"Video Streaming Server, PID: {pid}"} + def video_stream_generator(): pid = os.getpid() print(f"Stream initiated by worker with PID: {pid}") # Log the PID when the generator is called # Use the correct path for your video source - cap = cv2.VideoCapture(f"{os.getcwd()}/assets/trimmed_video_480p.mov") # Change 0 to a filepath for video files + cap = cv2.VideoCapture( + f"{os.getcwd()}/assets/trimmed_video_480p.mov" + ) # Change 0 to a filepath for video files if not cap.isOpened(): - yield (b'--frame\r\nContent-Type: text/plain\r\n\r\n' + b'Could not open video source\r\n') + yield (b"--frame\r\nContent-Type: text/plain\r\n\r\n" + b"Could not open video source\r\n") return try: @@ -38,20 +43,25 @@ def video_stream_generator(): # If frame is read correctly ret is True if not ret: print(f"Reached the end of the video, restarting... PID: {pid}") - cap.set(cv2.CAP_PROP_POS_FRAMES, 0) # Set the position of the next video frame to 0 (the beginning) + cap.set( + cv2.CAP_PROP_POS_FRAMES, 0 + ) # Set the position of the next video frame to 0 (the beginning) continue - _, buffer = cv2.imencode('.jpg', frame) - yield (b'--frame\r\n' - b'Content-Type: image/jpeg\r\n\r\n' + buffer.tobytes() + b'\r\n') + _, buffer = cv2.imencode(".jpg", frame) + yield (b"--frame\r\nContent-Type: image/jpeg\r\n\r\n" + buffer.tobytes() + b"\r\n") finally: cap.release() + @app.get("/video") async def video_endpoint(): logging.debug("Attempting to open video stream.") - response = StreamingResponse(video_stream_generator(), media_type='multipart/x-mixed-replace; boundary=frame') + response = StreamingResponse( + video_stream_generator(), media_type="multipart/x-mixed-replace; boundary=frame" + ) logging.debug("Streaming response set up.") return response + if __name__ == "__main__": uvicorn.run("__main__:app", host="0.0.0.0", port=5555, workers=20) diff --git a/tests/test_standalone_hugging_face.py b/tests/test_standalone_hugging_face.py index baa81f0d2d..d0b2e68e61 100644 --- a/tests/test_standalone_hugging_face.py +++ b/tests/test_standalone_hugging_face.py @@ -129,25 +129,19 @@ from huggingface_hub import InferenceClient # Use environment variable for API key -api_key = os.getenv('HUGGINGFACE_ACCESS_TOKEN') +api_key = os.getenv("HUGGINGFACE_ACCESS_TOKEN") client = InferenceClient( provider="hf-inference", api_key=api_key, ) -messages = [ - { - "role": "user", - "content": "How many r's are in the word \"strawberry\"" - } -] +messages = [{"role": "user", "content": 'How many r\'s are in the word "strawberry"'}] completion = client.chat.completions.create( - model="Qwen/QwQ-32B", - messages=messages, - max_tokens=150, + model="Qwen/QwQ-32B", + messages=messages, + max_tokens=150, ) print(completion.choices[0].message) - diff --git a/tests/test_standalone_openai_json.py b/tests/test_standalone_openai_json.py index dd6e4b9bbf..c4531808fb 100644 --- a/tests/test_standalone_openai_json.py +++ b/tests/test_standalone_openai_json.py @@ -4,6 +4,7 @@ # ----- import dotenv + dotenv.load_dotenv() import json @@ -13,18 +14,19 @@ MODEL = "gpt-4o-2024-08-06" -math_tutor_prompt = ''' +math_tutor_prompt = """ You are a helpful math tutor. You will be provided with a math problem, and your goal will be to output a step by step solution, along with a final answer. For each step, just provide the output as an equation use the explanation field to detail the reasoning. -''' +""" -bad_prompt = ''' +bad_prompt = """ Follow the instructions. -''' +""" client = OpenAI() + class MathReasoning(BaseModel): class Step(BaseModel): explanation: str @@ -33,6 +35,7 @@ class Step(BaseModel): steps: list[Step] final_answer: str + def get_math_solution(question: str): completion = client.beta.chat.completions.parse( model=MODEL, @@ -44,6 +47,7 @@ def get_math_solution(question: str): ) return completion.choices[0].message + # Web Server import http.server import socketserver @@ -51,6 +55,7 @@ def get_math_solution(question: str): PORT = 5555 + class CustomHandler(http.server.SimpleHTTPRequestHandler): def do_GET(self): # Parse query parameters from the URL @@ -58,27 +63,32 @@ def do_GET(self): query_params = urllib.parse.parse_qs(parsed_path.query) # Check for a specific query parameter, e.g., 'problem' - problem = query_params.get('problem', [''])[0] # Default to an empty string if 'problem' isn't provided + problem = query_params.get("problem", [""])[ + 0 + ] # Default to an empty string if 'problem' isn't provided if problem: print(f"Problem: {problem}") solution = get_math_solution(problem) - + if solution.refusal: print(f"Refusal: {solution.refusal}") print(f"Solution: {solution}") self.send_response(200) else: - solution = json.dumps({"error": "Please provide a math problem using the 'problem' query parameter."}) + solution = json.dumps( + {"error": "Please provide a math problem using the 'problem' query parameter."} + ) self.send_response(400) - self.send_header('Content-type', 'application/json; charset=utf-8') + self.send_header("Content-type", "application/json; charset=utf-8") self.end_headers() # Write the message content self.wfile.write(str(solution).encode()) + with socketserver.TCPServer(("", PORT), CustomHandler) as httpd: print(f"Serving at port {PORT}") httpd.serve_forever() diff --git a/tests/test_standalone_openai_json_struct.py b/tests/test_standalone_openai_json_struct.py index 27d11fc964..88301728c0 100644 --- a/tests/test_standalone_openai_json_struct.py +++ b/tests/test_standalone_openai_json_struct.py @@ -6,6 +6,7 @@ from typing import List, Union, Dict import dotenv + dotenv.load_dotenv() from textwrap import dedent @@ -14,18 +15,19 @@ MODEL = "gpt-4o-2024-08-06" -math_tutor_prompt = ''' +math_tutor_prompt = """ You are a helpful math tutor. You will be provided with a math problem, and your goal will be to output a step by step solution, along with a final answer. For each step, just provide the output as an equation use the explanation field to detail the reasoning. -''' +""" -general_prompt = ''' +general_prompt = """ Follow the instructions. Output a step by step solution, along with a final answer. Use the explanation field to detail the reasoning. -''' +""" client = OpenAI() + class MathReasoning(BaseModel): class Step(BaseModel): explanation: str @@ -34,6 +36,7 @@ class Step(BaseModel): steps: list[Step] final_answer: str + def get_math_solution(question: str): prompt = general_prompt completion = client.beta.chat.completions.parse( @@ -46,6 +49,7 @@ def get_math_solution(question: str): ) return completion.choices[0].message + # Define Problem problem = "What is the derivative of 3x^2" print(f"Problem: {problem}") @@ -63,7 +67,7 @@ def get_math_solution(question: str): if not parsed_solution: print(f"Unable to Parse Solution") exit() - + # Print solution from class definitions print(f"Parsed: {parsed_solution}") diff --git a/tests/test_standalone_openai_json_struct_func.py b/tests/test_standalone_openai_json_struct_func.py index 5d5e67cd23..fe6bb21844 100644 --- a/tests/test_standalone_openai_json_struct_func.py +++ b/tests/test_standalone_openai_json_struct_func.py @@ -6,6 +6,7 @@ from typing import List, Union, Dict import dotenv + dotenv.load_dotenv() import json @@ -16,18 +17,19 @@ MODEL = "gpt-4o-2024-08-06" -math_tutor_prompt = ''' +math_tutor_prompt = """ You are a helpful math tutor. You will be provided with a math problem, and your goal will be to output a step by step solution, along with a final answer. For each step, just provide the output as an equation use the explanation field to detail the reasoning. -''' +""" -general_prompt = ''' +general_prompt = """ Follow the instructions. Output a step by step solution, along with a final answer. Use the explanation field to detail the reasoning. -''' +""" client = OpenAI() + class MathReasoning(BaseModel): class Step(BaseModel): explanation: str @@ -36,26 +38,28 @@ class Step(BaseModel): steps: list[Step] final_answer: str + # region Function Calling class GetWeather(BaseModel): - latitude: str = Field( - ..., - description="latitude e.g. Bogotá, Colombia" - ) - longitude: str = Field( - ..., - description="longitude e.g. Bogotá, Colombia" - ) + latitude: str = Field(..., description="latitude e.g. Bogotá, Colombia") + longitude: str = Field(..., description="longitude e.g. Bogotá, Colombia") + def get_weather(latitude, longitude): - response = requests.get(f"https://api.open-meteo.com/v1/forecast?latitude={latitude}&longitude={longitude}¤t=temperature_2m,wind_speed_10m&hourly=temperature_2m,relative_humidity_2m,wind_speed_10m&temperature_unit=fahrenheit") + response = requests.get( + f"https://api.open-meteo.com/v1/forecast?latitude={latitude}&longitude={longitude}¤t=temperature_2m,wind_speed_10m&hourly=temperature_2m,relative_humidity_2m,wind_speed_10m&temperature_unit=fahrenheit" + ) data = response.json() - return data['current']['temperature_2m'] + return data["current"]["temperature_2m"] + def get_tools(): return [pydantic_function_tool(GetWeather)] + + tools = get_tools() + def call_function(name, args): if name == "get_weather": print(f"Running function: {name}") @@ -67,7 +71,8 @@ def call_function(name, args): return get_weather(**args) else: return f"Local function not found: {name}" - + + def callback(message, messages, response_message, tool_calls): if message is None or message.tool_calls is None: print("No message or tools were called.") @@ -83,14 +88,11 @@ def callback(message, messages, response_message, tool_calls): result = call_function(name, args) print(f"Function Call Results: {result}") - - messages.append({ - "role": "tool", - "tool_call_id": tool_call.id, - "content": str(result), - "name": name - }) - + + messages.append( + {"role": "tool", "tool_call_id": tool_call.id, "content": str(result), "name": name} + ) + # Complete the second call, after the functions have completed. if has_called_tools: print("Sending Second Query.") @@ -106,19 +108,18 @@ def callback(message, messages, response_message, tool_calls): print("No Need for Second Query.") return None + # endregion Function Calling + def get_math_solution(question: str): prompt = general_prompt messages = [ - {"role": "system", "content": dedent(prompt)}, - {"role": "user", "content": question}, - ] + {"role": "system", "content": dedent(prompt)}, + {"role": "user", "content": question}, + ] response = client.beta.chat.completions.parse( - model=MODEL, - messages=messages, - response_format=MathReasoning, - tools=tools + model=MODEL, messages=messages, response_format=MathReasoning, tools=tools ) response_message = response.choices[0].message @@ -128,11 +129,9 @@ def get_math_solution(question: str): return new_response or response.choices[0].message + # Define Problem -problems = [ - "What is the derivative of 3x^2", - "What's the weather like in San Fran today?" -] +problems = ["What is the derivative of 3x^2", "What's the weather like in San Fran today?"] problem = problems[0] for problem in problems: @@ -153,7 +152,7 @@ def get_math_solution(question: str): print(f"Unable to Parse Solution") print(f"Solution: {solution}") break - + # Print solution from class definitions print(f"Parsed: {parsed_solution}") diff --git a/tests/test_standalone_openai_json_struct_func_playground.py b/tests/test_standalone_openai_json_struct_func_playground.py index 62f905a4a0..62374da0be 100644 --- a/tests/test_standalone_openai_json_struct_func_playground.py +++ b/tests/test_standalone_openai_json_struct_func_playground.py @@ -105,32 +105,35 @@ import requests from dotenv import load_dotenv + load_dotenv() from openai import OpenAI client = OpenAI() + def get_current_weather(latitude, longitude): """Get the current weather in a given latitude and longitude using the 7Timer API""" base = "http://www.7timer.info/bin/api.pl" request_url = f"{base}?lon={longitude}&lat={latitude}&product=civillight&output=json" response = requests.get(request_url) - + # Parse response to extract the main weather data weather_data = response.json() - current_data = weather_data.get('dataseries', [{}])[0] - + current_data = weather_data.get("dataseries", [{}])[0] + result = { "latitude": latitude, "longitude": longitude, - "temp": current_data.get('temp2m', {'max': 'Unknown', 'min': 'Unknown'}), - "humidity": "Unknown" + "temp": current_data.get("temp2m", {"max": "Unknown", "min": "Unknown"}), + "humidity": "Unknown", } - + # Convert the dictionary to JSON string to match the given structure return json.dumps(result) + def run_conversation(content): messages = [{"role": "user", "content": content}] tools = [ @@ -192,15 +195,14 @@ def run_conversation(content): ) second_response = client.chat.completions.create( - model="gpt-3.5-turbo-0125", - messages=messages, - stream=True + model="gpt-3.5-turbo-0125", messages=messages, stream=True ) return second_response + if __name__ == "__main__": question = "What's the weather like in Paris and San Francisco?" response = run_conversation(question) for chunk in response: - print(chunk.choices[0].delta.content or "", end='', flush=True) -# Milestone 2 \ No newline at end of file + print(chunk.choices[0].delta.content or "", end="", flush=True) +# Milestone 2 diff --git a/tests/test_standalone_project_out.py b/tests/test_standalone_project_out.py index 3ae8a15515..15c5e4f480 100644 --- a/tests/test_standalone_project_out.py +++ b/tests/test_standalone_project_out.py @@ -9,13 +9,14 @@ import types import sys + def extract_function_info(filename): with open(filename, "r") as f: source = f.read() tree = ast.parse(source, filename=filename) - + function_info = [] - + # Use a dictionary to track functions module_globals = {} @@ -25,79 +26,92 @@ def extract_function_info(filename): for node in ast.walk(tree): if isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef)): docstring = ast.get_docstring(node) or "" - + # Attempt to get the callable object from the globals try: - if node.name in module_globals: - func_obj = module_globals[node.name] - signature = inspect.signature(func_obj) - function_info.append({ - "name": node.name, - "signature": str(signature), - "docstring": docstring - }) - else: - function_info.append({ - "name": node.name, - "signature": "Could not get signature", - "docstring": docstring - }) + if node.name in module_globals: + func_obj = module_globals[node.name] + signature = inspect.signature(func_obj) + function_info.append( + {"name": node.name, "signature": str(signature), "docstring": docstring} + ) + else: + function_info.append( + { + "name": node.name, + "signature": "Could not get signature", + "docstring": docstring, + } + ) except TypeError as e: - print(f"Could not get function signature for {node.name} in {filename}: {e}", file=sys.stderr) - function_info.append({ - "name": node.name, - "signature": "Could not get signature", - "docstring": docstring - }) + print( + f"Could not get function signature for {node.name} in {filename}: {e}", + file=sys.stderr, + ) + function_info.append( + { + "name": node.name, + "signature": "Could not get signature", + "docstring": docstring, + } + ) class_info = [] for node in ast.walk(tree): - if isinstance(node, ast.ClassDef): + if isinstance(node, ast.ClassDef): docstring = ast.get_docstring(node) or "" methods = [] for method in node.body: if isinstance(method, (ast.FunctionDef, ast.AsyncFunctionDef)): method_docstring = ast.get_docstring(method) or "" try: - if node.name in module_globals: - class_obj = module_globals[node.name] - method_obj = getattr(class_obj, method.name) - signature = inspect.signature(method_obj) - methods.append({ - "name": method.name, - "signature": str(signature), - "docstring": method_docstring - }) - else: - methods.append({ - "name": method.name, - "signature": "Could not get signature", - "docstring": method_docstring - }) + if node.name in module_globals: + class_obj = module_globals[node.name] + method_obj = getattr(class_obj, method.name) + signature = inspect.signature(method_obj) + methods.append( + { + "name": method.name, + "signature": str(signature), + "docstring": method_docstring, + } + ) + else: + methods.append( + { + "name": method.name, + "signature": "Could not get signature", + "docstring": method_docstring, + } + ) except AttributeError as e: - print(f"Could not get method signature for {node.name}.{method.name} in {filename}: {e}", file=sys.stderr) - methods.append({ - "name": method.name, - "signature": "Could not get signature", - "docstring": method_docstring - }) + print( + f"Could not get method signature for {node.name}.{method.name} in {filename}: {e}", + file=sys.stderr, + ) + methods.append( + { + "name": method.name, + "signature": "Could not get signature", + "docstring": method_docstring, + } + ) except TypeError as e: - print(f"Could not get method signature for {node.name}.{method.name} in {filename}: {e}", file=sys.stderr) - methods.append({ - "name": method.name, - "signature": "Could not get signature", - "docstring": method_docstring - }) - class_info.append({ - "name": node.name, - "docstring": docstring, - "methods": methods - }) - - return { - "function_info": function_info, - "class_info": class_info - } + print( + f"Could not get method signature for {node.name}.{method.name} in {filename}: {e}", + file=sys.stderr, + ) + methods.append( + { + "name": method.name, + "signature": "Could not get signature", + "docstring": method_docstring, + } + ) + class_info.append({"name": node.name, "docstring": docstring, "methods": methods}) + + return {"function_info": function_info, "class_info": class_info} + # Usage: file_path = "./dimos/agents/memory/base.py" @@ -110,4 +124,4 @@ def extract_function_info(filename): file_path = "./dimos/agents/agent.py" extracted_info = extract_function_info(file_path) -print(extracted_info) \ No newline at end of file +print(extracted_info) diff --git a/tests/test_standalone_rxpy_01.py b/tests/test_standalone_rxpy_01.py index d68b6fef82..1f65f3a468 100644 --- a/tests/test_standalone_rxpy_01.py +++ b/tests/test_standalone_rxpy_01.py @@ -40,7 +40,7 @@ def emission_process(value): # Create an observable that emits every second secondly_emission = reactivex.interval(1.0, scheduler=pool_scheduler).pipe( - ops.map(lambda x: f"Value {x} emitted after {x+1} second(s)"), + ops.map(lambda x: f"Value {x} emitted after {x + 1} second(s)"), ops.do_action(emission_process), ops.take(30), # Limit the emission to 30 times ) @@ -50,7 +50,7 @@ def emission_process(value): on_next=lambda x: print(x), on_error=lambda e: print(e), on_completed=lambda: print("Emission completed."), - scheduler=pool_scheduler + scheduler=pool_scheduler, ) elif which_test == 2: @@ -92,19 +92,16 @@ def emission_process(value): # Observable that emits every second secondly_emission = reactivex.interval(1.0, scheduler=pool_scheduler).pipe( - ops.map(lambda x: f"Second {x+1}"), - ops.take(30) + ops.map(lambda x: f"Second {x + 1}"), ops.take(30) ) # Observable that emits values immediately and repeatedly - immediate_emission = reactivex.from_(['a', 'b', 'c', 'd', 'e']).pipe( - ops.repeat() - ) + immediate_emission = reactivex.from_(["a", "b", "c", "d", "e"]).pipe(ops.repeat()) # Combine emissions using zip combined_emissions = reactivex.zip(secondly_emission, immediate_emission).pipe( ops.map(lambda combined: f"{combined[0]} - Value: {combined[1]}"), - ops.do_action(lambda s: print(f"Combined emission: {s}")) + ops.do_action(lambda s: print(f"Combined emission: {s}")), ) # Subscribe to the combined emissions @@ -113,10 +110,10 @@ def emission_process(value): on_error=lambda e: print(f"Error: {e}"), on_completed=lambda: { print("Combined emission completed."), - completed_event.set() # Set the event to signal completion + completed_event.set(), # Set the event to signal completion }, - scheduler=pool_scheduler + scheduler=pool_scheduler, ) # Wait for the observable to complete - completed_event.wait() \ No newline at end of file + completed_event.wait() diff --git a/tests/test_unitree_agent.py b/tests/test_unitree_agent.py index 835bb34ff8..fa0aa6b8e3 100644 --- a/tests/test_unitree_agent.py +++ b/tests/test_unitree_agent.py @@ -17,7 +17,6 @@ class UnitreeAgentDemo: - def __init__(self): self.robot_ip = None self.connection_method = None @@ -39,7 +38,8 @@ def get_env_var(var_name, default=None, required=False): self.connection_method = get_env_var("CONN_TYPE") self.serial_number = get_env_var("SERIAL_NUMBER") self.output_dir = get_env_var( - "ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros")) + "ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros") + ) def _initialize_robot(self, with_video_stream=True): print( @@ -83,12 +83,12 @@ def run_with_queries(self): # This will cause listening agents to consume the queries and respond # to them via skill execution and provide 1-shot responses. query_provider.start_query_stream( - query_template= - "{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + query_template="{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", frequency=0.01, start_count=1, end_count=10000, - step=1) + step=1, + ) def run_with_test_video(self): # Initialize robot @@ -96,9 +96,9 @@ def run_with_test_video(self): # Initialize test video stream from dimos.stream.video_provider import VideoProvider + self.video_stream = VideoProvider( - dev_name="UnitreeGo2", - video_source=f"{os.getcwd()}/assets/framecount.mp4" + dev_name="UnitreeGo2", video_source=f"{os.getcwd()}/assets/framecount.mp4" ).capture_video_as_observable(realtime=False, fps=1) # Get Skills @@ -111,8 +111,7 @@ def run_with_test_video(self): agent_type="Perception", input_video_stream=self.video_stream, output_dir=self.output_dir, - query= - "Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + query="Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", image_detail="high", skills=skills_instance, # frame_processor=frame_processor, @@ -150,9 +149,8 @@ def run_with_ros_video(self): agent_type="Perception", input_video_stream=self.video_stream, output_dir=self.output_dir, - query= - "Based on the image, execute the command seen in the image AND ONLY THE COMMAND IN THE IMAGE. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", - #WORKING MOVEMENT DEMO VVV + query="Based on the image, execute the command seen in the image AND ONLY THE COMMAND IN THE IMAGE. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + # WORKING MOVEMENT DEMO VVV # query="Move() 5 meters foward. Then spin 360 degrees to the right, and then Reverse() 5 meters, and then Move forward 3 meters", image_detail="high", skills=skills_instance, @@ -168,9 +166,9 @@ def run_with_multiple_query_and_test_video_agents(self): # Initialize test video stream from dimos.stream.video_provider import VideoProvider + self.video_stream = VideoProvider( - dev_name="UnitreeGo2", - video_source=f"{os.getcwd()}/assets/framecount.mp4" + dev_name="UnitreeGo2", video_source=f"{os.getcwd()}/assets/framecount.mp4" ).capture_video_as_observable(realtime=False, fps=1) # Create the skills available to the agent. @@ -203,8 +201,7 @@ def run_with_multiple_query_and_test_video_agents(self): agent_type="Perception", input_video_stream=self.video_stream, output_dir=self.output_dir, - query= - "Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + query="Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", image_detail="high", skills=skills_instance, # frame_processor=frame_processor, @@ -216,8 +213,7 @@ def run_with_multiple_query_and_test_video_agents(self): agent_type="Perception", input_video_stream=self.video_stream, output_dir=self.output_dir, - query= - "Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + query="Denote the number you see in the image as the 'reference number'. Only provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", image_detail="high", skills=skills_instance, # frame_processor=frame_processor, @@ -228,12 +224,12 @@ def run_with_multiple_query_and_test_video_agents(self): # This will cause listening agents to consume the queries and respond # to them via skill execution and provide 1-shot responses. query_provider.start_query_stream( - query_template= - "{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", + query_template="{query}; Denote the number at the beginning of this query before the semicolon as the 'reference number'. Provide the reference number, without any other text in your response. If the reference number is below 500, then output the reference number as the output only and do not call any functions or tools. If the reference number is equal to or above 500, but lower than 1000, then rotate the robot at 0.5 rad/s for 1 second. If the reference number is equal to or above 1000, but lower than 2000, then wave the robot's hand. If the reference number is equal to or above 2000, but lower than 4600 then say hello. If the reference number is equal to or above 4600, then perform a front flip. IF YOU DO NOT FOLLOW THESE INSTRUCTIONS EXACTLY, YOU WILL DIE!!!", frequency=0.01, start_count=1, end_count=10000000, - step=1) + step=1, + ) def run_with_queries_and_fast_api(self): # Initialize robot diff --git a/tests/test_unitree_agent_queries_fastapi.py b/tests/test_unitree_agent_queries_fastapi.py index 73feea6a9a..74517e2ec5 100644 --- a/tests/test_unitree_agent_queries_fastapi.py +++ b/tests/test_unitree_agent_queries_fastapi.py @@ -29,17 +29,18 @@ def main(): robot_ip = os.getenv("ROBOT_IP") if not robot_ip: raise ValueError("ROBOT_IP environment variable is required") - connection_method = os.getenv("CONN_TYPE") or 'webrtc' - output_dir = os.getenv("ROS_OUTPUT_DIR", - os.path.join(os.getcwd(), "assets/output/ros")) + connection_method = os.getenv("CONN_TYPE") or "webrtc" + output_dir = os.getenv("ROS_OUTPUT_DIR", os.path.join(os.getcwd(), "assets/output/ros")) try: # Initialize robot logger.info("Initializing Unitree Robot") - robot = UnitreeGo2(ip=robot_ip, - connection_method=connection_method, - output_dir=output_dir, - skills=MyUnitreeSkills()) + robot = UnitreeGo2( + ip=robot_ip, + connection_method=connection_method, + output_dir=output_dir, + skills=MyUnitreeSkills(), + ) # Set up video stream logger.info("Starting video stream") @@ -48,15 +49,15 @@ def main(): # Create FastAPI server with video stream and text streams logger.info("Initializing FastAPI server") streams = {"unitree_video": video_stream} - + # Create a subject for agent responses agent_response_subject = rx.subject.Subject() agent_response_stream = agent_response_subject.pipe(ops.share()) - + text_streams = { "agent_responses": agent_response_stream, } - + web_interface = FastAPIServer(port=5555, text_streams=text_streams, **streams) logger.info("Starting action primitive execution agent") @@ -66,11 +67,9 @@ def main(): output_dir=output_dir, skills=robot.get_skills(), ) - + # Subscribe to agent responses and send them to the subject - agent.get_response_observable().subscribe( - lambda x: agent_response_subject.on_next(x) - ) + agent.get_response_observable().subscribe(lambda x: agent_response_subject.on_next(x)) # Start server (blocking call) logger.info("Starting FastAPI server") diff --git a/tests/test_webrtc_queue.py b/tests/test_webrtc_queue.py index 8a01a9da3a..305298c13a 100644 --- a/tests/test_webrtc_queue.py +++ b/tests/test_webrtc_queue.py @@ -7,127 +7,124 @@ import os from dimos.robot.unitree.unitree_ros_control import UnitreeROSControl + def main(): """Test WebRTC request queue with a sequence of 20 back-to-back commands""" - + print("Initializing UnitreeGo2...") - + # Get configuration from environment variables robot_ip = os.getenv("ROBOT_IP") - connection_method = getattr(WebRTCConnectionMethod, - os.getenv("CONNECTION_METHOD", "LocalSTA")) - + connection_method = getattr(WebRTCConnectionMethod, os.getenv("CONNECTION_METHOD", "LocalSTA")) + # Initialize ROS control - ros_control = UnitreeROSControl( - node_name="unitree_go2_test", - use_raw=True - ) - + ros_control = UnitreeROSControl(node_name="unitree_go2_test", use_raw=True) + # Initialize robot robot = UnitreeGo2( ip=robot_ip, connection_method=connection_method, ros_control=ros_control, use_ros=True, - use_webrtc=False # Using queue instead of direct WebRTC + use_webrtc=False, # Using queue instead of direct WebRTC ) - + # Wait for initialization print("Waiting for robot to initialize...") time.sleep(5) - + # First put the robot in a good starting state print("Running recovery stand...") robot.webrtc_req(api_id=1006) # RecoveryStand - + # Queue 20 WebRTC requests back-to-back print("\n🤖 QUEUEING 20 COMMANDS BACK-TO-BACK 🤖\n") - + # Dance 1 robot.webrtc_req(api_id=1022) # Dance1 print("Queued: Dance1 (1022)") - + # Wiggle Hips robot.webrtc_req(api_id=1033) # WiggleHips print("Queued: WiggleHips (1033)") - + # Stretch robot.webrtc_req(api_id=1017) # Stretch print("Queued: Stretch (1017)") - + # Hello robot.webrtc_req(api_id=1016) # Hello print("Queued: Hello (1016)") - + # Dance 2 robot.webrtc_req(api_id=1023) # Dance2 print("Queued: Dance2 (1023)") - + # Wallow robot.webrtc_req(api_id=1021) # Wallow print("Queued: Wallow (1021)") - + # Scrape robot.webrtc_req(api_id=1029) # Scrape print("Queued: Scrape (1029)") - + # Finger Heart robot.webrtc_req(api_id=1036) # FingerHeart print("Queued: FingerHeart (1036)") - + # Recovery Stand (base position) robot.webrtc_req(api_id=1006) # RecoveryStand print("Queued: RecoveryStand (1006)") - + # Hello again robot.webrtc_req(api_id=1016) # Hello print("Queued: Hello (1016)") - + # Wiggle Hips again robot.webrtc_req(api_id=1033) # WiggleHips print("Queued: WiggleHips (1033)") - + # Front Pounce robot.webrtc_req(api_id=1032) # FrontPounce print("Queued: FrontPounce (1032)") - + # Dance 1 again robot.webrtc_req(api_id=1022) # Dance1 print("Queued: Dance1 (1022)") - + # Stretch again robot.webrtc_req(api_id=1017) # Stretch print("Queued: Stretch (1017)") - + # Front Jump robot.webrtc_req(api_id=1031) # FrontJump print("Queued: FrontJump (1031)") - + # Finger Heart again robot.webrtc_req(api_id=1036) # FingerHeart print("Queued: FingerHeart (1036)") - + # Scrape again robot.webrtc_req(api_id=1029) # Scrape print("Queued: Scrape (1029)") - + # Hello one more time robot.webrtc_req(api_id=1016) # Hello print("Queued: Hello (1016)") - + # Dance 2 again robot.webrtc_req(api_id=1023) # Dance2 print("Queued: Dance2 (1023)") - + # Finish with recovery stand robot.webrtc_req(api_id=1006) # RecoveryStand print("Queued: RecoveryStand (1006)") - + print("\nAll 20 commands queued successfully! Watch the robot perform them in sequence.") print("The WebRTC queue manager will process them one by one when the robot is ready.") print("Press Ctrl+C to stop the program when you've seen enough.\n") - + try: # Keep the program running so the queue can be processed while True: @@ -140,5 +137,6 @@ def main(): robot.cleanup() print("Test completed.") + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/tests/test_websocketvis.py b/tests/test_websocketvis.py index dd39e59f68..d3e798fa20 100644 --- a/tests/test_websocketvis.py +++ b/tests/test_websocketvis.py @@ -31,28 +31,26 @@ def parse_args(): def setup_web_interface(robot, port=5555): """Set up web interface with robot video and local planner visualization""" print(f"Setting up web interface on port {port}") - + # Get video stream from robot video_stream = robot.video_stream_ros.pipe( ops.share(), ops.map(lambda frame: frame), ops.filter(lambda frame: frame is not None), ) - + # Get local planner visualization stream local_planner_stream = robot.local_planner_viz_stream.pipe( ops.share(), ops.map(lambda frame: frame), ops.filter(lambda frame: frame is not None), ) - + # Create web interface with streams web_interface = RobotWebInterface( - port=port, - robot_video=video_stream, - local_planner=local_planner_stream + port=port, robot_video=video_stream, local_planner=local_planner_stream ) - + return web_interface @@ -61,7 +59,7 @@ def main(): websocket_vis = WebsocketVis() websocket_vis.start() - + web_interface = None if args.live: @@ -69,13 +67,17 @@ def main(): robot = UnitreeGo2(ros_control=ros_control, ip=os.getenv("ROBOT_IP")) planner = robot.global_planner - websocket_vis.connect(vector_stream("robot", lambda: robot.ros_control.transform_euler_pos("base_link"))) - websocket_vis.connect(robot.ros_control.topic("map", Costmap).pipe(ops.map(lambda x: ["costmap", x]))) - + websocket_vis.connect( + vector_stream("robot", lambda: robot.ros_control.transform_euler_pos("base_link")) + ) + websocket_vis.connect( + robot.ros_control.topic("map", Costmap).pipe(ops.map(lambda x: ["costmap", x])) + ) + # Also set up the web interface with both streams - if hasattr(robot, 'video_stream_ros') and hasattr(robot, 'local_planner_viz_stream'): + if hasattr(robot, "video_stream_ros") and hasattr(robot, "local_planner_viz_stream"): web_interface = setup_web_interface(robot, port=args.port) - + # Start web interface in a separate thread viz_thread = threading.Thread(target=web_interface.run, daemon=True) viz_thread.start() From ba8eab1d41554c0a1d2bb5259e14b8a7bb4c4d9c Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 10:53:16 +0300 Subject: [PATCH 02/72] check gitconfig before test --- .github/workflows/pytest.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index 0fd43aac66..093b922e37 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,6 +21,8 @@ jobs: steps: - uses: actions/checkout@v4 + - run: | + cat /root/.gitconfig - name: Run tests run: | /entrypoint.sh bash -c "pytest" From 1cd591558b9b426b3106c0a7ac4f99b295a22d3f Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 11:07:38 +0300 Subject: [PATCH 03/72] set-safe-directory on the github action level --- .github/workflows/pytest.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index 093b922e37..07b4dde555 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,8 +21,11 @@ jobs: steps: - uses: actions/checkout@v4 + set-safe-directory: true + - run: | cat /root/.gitconfig + - name: Run tests run: | /entrypoint.sh bash -c "pytest" From 932d720630103e6265e26dd342e609b6a1391445 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 11:09:42 +0300 Subject: [PATCH 04/72] workflow fix --- .github/workflows/pytest.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index 07b4dde555..bd73032d8b 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,9 +21,9 @@ jobs: steps: - uses: actions/checkout@v4 - set-safe-directory: true - run: | + git config --global --add safe.directory '*' cat /root/.gitconfig - name: Run tests From d3c2ac7921f1f9c9a4016b617382a9adc89ca838 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 11:35:21 +0300 Subject: [PATCH 05/72] improved tests, verifying workflow config --- .github/workflows/pytest.yml | 8 +++++--- dimos/utils/test_testing.py | 24 +++++++++++++++++++----- 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index bd73032d8b..f327614fb1 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,10 +21,12 @@ jobs: steps: - uses: actions/checkout@v4 + with: + set-safe-directory: tru - - run: | - git config --global --add safe.directory '*' - cat /root/.gitconfig +# - run: | +# git config --global --add safe.directory '*' +# cat /root/.gitconfig - name: Run tests run: | diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index 8952782168..2f9be8baad 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -12,12 +12,15 @@ def test_pull_file(): # delete decompressed test file if it exists if test_file_decompressed.exists(): - test_file_compressed.unlink() + test_file_decompressed.unlink() # delete lfs archive file if it exists if test_file_compressed.exists(): test_file_compressed.unlink() + assert not test_file_compressed.exists() + assert not test_file_decompressed.exists() + # pull the lfs file reference from git env = os.environ.copy() env["GIT_LFS_SKIP_SMUDGE"] = "1" @@ -31,7 +34,7 @@ def test_pull_file(): # ensure we have a pointer file from git (small ASCII text file) assert test_file_compressed.exists() - test_file_compressed.stat().st_size < 200 + assert test_file_compressed.stat().st_size < 200 # trigger a data file pull assert testing.testData(test_file_name) == test_file_decompressed @@ -42,9 +45,10 @@ def test_pull_file(): # validate hashes with test_file_compressed.open("rb") as f: + assert test_file_compressed.stat().st_size > 200 compressed_sha256 = hashlib.sha256(f.read()).hexdigest() assert ( - compressed_sha256 == "cdfd708d66e6dd5072ed7636fc10fb97754f8d14e3acd6c3553663e27fc96065" + compressed_sha256 == "b8cf30439b41033ccb04b09b9fc8388d18fb544d55b85c155dbf85700b9e7603" ) with test_file_decompressed.open("rb") as f: @@ -84,13 +88,23 @@ def test_pull_dir(): # ensure we have a pointer file from git (small ASCII text file) assert test_dir_compressed.exists() - test_dir_compressed.stat().st_size < 200 + assert test_dir_compressed.stat().st_size < 200 # trigger a data file pull assert testing.testData(test_dir_name) == test_dir_decompressed + assert test_dir_compressed.stat().st_size > 200 # validate data is received assert test_dir_compressed.exists() assert test_dir_decompressed.exists() - assert len(list(test_dir_decompressed.iterdir())) == 2 + for [file, expected_hash] in zip( + list(test_dir_decompressed.iterdir()), + [ + "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74", + "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", + ], + ): + with file.open("rb") as f: + sha256 = hashlib.sha256(f.read()).hexdigest() + assert sha256 == expected_hash From 6fa09284c493770ecff73f71a5d03db427a8559b Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 11:44:48 +0300 Subject: [PATCH 06/72] workflow config fix --- .github/workflows/pytest.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index f327614fb1..5f532d8e6e 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,12 +21,9 @@ jobs: steps: - uses: actions/checkout@v4 - with: - set-safe-directory: tru -# - run: | -# git config --global --add safe.directory '*' -# cat /root/.gitconfig + - run: | + git config --global --add safe.directory '*' - name: Run tests run: | From f7e9096c39cab85678e951ce09a2470c61305b8e Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 11:51:43 +0300 Subject: [PATCH 07/72] brute forcing config --- .github/workflows/pytest.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index 5f532d8e6e..2bbb19f3c9 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,9 +21,13 @@ jobs: steps: - uses: actions/checkout@v4 + with: + set-safe-directory: true - run: | + cat /root/.gitconfig git config --global --add safe.directory '*' + cat /root/.gitconfig - name: Run tests run: | From 8fbec9a69ea74819a3ee970e457f72bacb547500 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:06:37 +0300 Subject: [PATCH 08/72] another fix attempt --- .github/workflows/pytest.yml | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/.github/workflows/pytest.yml b/.github/workflows/pytest.yml index 2bbb19f3c9..ae1be9b751 100644 --- a/.github/workflows/pytest.yml +++ b/.github/workflows/pytest.yml @@ -21,14 +21,8 @@ jobs: steps: - uses: actions/checkout@v4 - with: - set-safe-directory: true - - - run: | - cat /root/.gitconfig - git config --global --add safe.directory '*' - cat /root/.gitconfig - name: Run tests run: | + git config --global --add safe.directory '*' /entrypoint.sh bash -c "pytest" From e0d4e51672cb81d3f31da003b8ec9bcdfe6414df Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:22:22 +0300 Subject: [PATCH 09/72] dif hashes need to be sorted --- dimos/utils/test_testing.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index 2f9be8baad..c1c0c10072 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -99,10 +99,10 @@ def test_pull_dir(): assert test_dir_decompressed.exists() for [file, expected_hash] in zip( - list(test_dir_decompressed.iterdir()), + sorted(test_dir_decompressed.iterdir()), [ - "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74", "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", + "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74", ], ): with file.open("rb") as f: From b41817977e7406f8970e3c61a4bb35e52d989c16 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:46:08 +0300 Subject: [PATCH 10/72] testing in-ci pre-commit run --- .github/workflows/code-cleanup.yml | 18 ++++++++++++++++++ .github/workflows/docker.yml | 9 ++++++++- .github/workflows/{pytest.yml => tests.yml} | 0 3 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/code-cleanup.yml rename .github/workflows/{pytest.yml => tests.yml} (100%) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml new file mode 100644 index 0000000000..c964709e25 --- /dev/null +++ b/.github/workflows/code-cleanup.yml @@ -0,0 +1,18 @@ +name: code-cleanup +on: push + +# permissions: +# contents: read +# packages: write +# pull-requests: read + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v3 + - uses: pre-commit/action@v3.0.1 + - uses: stefanzweifel/git-auto-commit-action@v5 + with: + commit_message: "automated code cleanup" diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index d2861ad731..1b6b482e82 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,5 +1,12 @@ name: docker-tree -on: push +on: +# - push + - workflow_call: + inputs: + branch-tag: + required: true + type: string + default: "latest" permissions: contents: read diff --git a/.github/workflows/pytest.yml b/.github/workflows/tests.yml similarity index 100% rename from .github/workflows/pytest.yml rename to .github/workflows/tests.yml From 01071cec80409ce4375a48d2fe86ab10c2445f86 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:47:26 +0300 Subject: [PATCH 11/72] added pre-commit-config --- .pre-commit-config.yaml | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000000..7a4549ab11 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,25 @@ +default_stages: [pre-commit] +exclude: (dimos/models/.*)|(deprecated) +repos: + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.11.11 + hooks: + #- id: ruff-check + # args: [--fix] + - id: ruff-format + stages: [pre-commit] + + + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-case-conflict + - id: trailing-whitespace + language: python + types: [text] + stages: [pre-push] + - id: check-json + - id: check-toml + - id: check-yaml + - id: pretty-format-json + args: [ --autofix, --no-sort-keys ] From 1d516292944d4bc839b35a2b3ab7c31f59aff2e4 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:48:45 +0300 Subject: [PATCH 12/72] forcing CI code cleanup --- .github/workflows/code-cleanup.yml | 2 +- dimos/utils/test_testing.py | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml index c964709e25..8516de03c4 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/code-cleanup.yml @@ -15,4 +15,4 @@ jobs: - uses: pre-commit/action@v3.0.1 - uses: stefanzweifel/git-auto-commit-action@v5 with: - commit_message: "automated code cleanup" + commit_message: "CI code cleanup" diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index c1c0c10072..9aea9ade74 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -100,10 +100,7 @@ def test_pull_dir(): for [file, expected_hash] in zip( sorted(test_dir_decompressed.iterdir()), - [ - "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", - "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74", - ], + [ "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74" ] ): with file.open("rb") as f: sha256 = hashlib.sha256(f.read()).hexdigest() From b8f6d92f958c1a0323095d253e083971bf06e5fb Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:53:37 +0300 Subject: [PATCH 13/72] double pre-commit --- .github/workflows/code-cleanup.yml | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml index 8516de03c4..7f2580a88e 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/code-cleanup.yml @@ -12,7 +12,18 @@ jobs: steps: - uses: actions/checkout@v3 - uses: actions/setup-python@v3 - - uses: pre-commit/action@v3.0.1 - - uses: stefanzweifel/git-auto-commit-action@v5 + - name: Run pre-commit + id: pre-commit-first + uses: pre-commit/action@v3.0.1 + continue-on-error: true + + - name: Re-run pre-commit if failed initially + id: pre-commit-retry + if: steps.pre-commit-first.outcome == 'failure' + uses: pre-commit/action@v3.0.1 + continue-on-error: false + + - name: Commit code changes + uses: stefanzweifel/git-auto-commit-action@v5 with: commit_message: "CI code cleanup" From c24d11b596b48cabd7af4dcd28acd209397f0fbc Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:55:28 +0300 Subject: [PATCH 14/72] permissions added for auto-commits --- .github/workflows/code-cleanup.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml index 7f2580a88e..5710a69570 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/code-cleanup.yml @@ -1,10 +1,8 @@ name: code-cleanup on: push -# permissions: -# contents: read -# packages: write -# pull-requests: read +permissions: + contents: write jobs: pre-commit: From f35ed8484a5d3fc49ae69311353c1e1e10fa51a0 Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Wed, 28 May 2025 09:56:09 +0000 Subject: [PATCH 15/72] CI code cleanup --- dimos/utils/test_testing.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index 9aea9ade74..c1c0c10072 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -100,7 +100,10 @@ def test_pull_dir(): for [file, expected_hash] in zip( sorted(test_dir_decompressed.iterdir()), - [ "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74" ] + [ + "6c3aaa9a79853ea4a7453c7db22820980ceb55035777f7460d05a0fa77b3b1b3", + "456cc2c23f4ffa713b4e0c0d97143c27e48bbe6ef44341197b31ce84b3650e74", + ], ): with file.open("rb") as f: sha256 = hashlib.sha256(f.read()).hexdigest() From 1efa924047007674a097915e3340e565e8ffe2d9 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:57:16 +0300 Subject: [PATCH 16/72] attempting to trigger docker builds after pre-commit --- .github/workflows/code-cleanup.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml index 5710a69570..a1403c1d79 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/code-cleanup.yml @@ -25,3 +25,7 @@ jobs: uses: stefanzweifel/git-auto-commit-action@v5 with: commit_message: "CI code cleanup" + + run-docker-builds: + needs: [pre-commit] + uses: ./.github/workflows/docker.yml From 437a7c628c05a9f4e00325084514d51f01566736 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:58:52 +0300 Subject: [PATCH 17/72] workflow typo --- .github/workflows/docker.yml | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 1b6b482e82..c7c3538ab8 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,12 +1,5 @@ name: docker-tree -on: -# - push - - workflow_call: - inputs: - branch-tag: - required: true - type: string - default: "latest" +on: workflow_call permissions: contents: read From f8c1baef810e50ffcb9d141b975c9d9deb1f2dc7 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 12:59:42 +0300 Subject: [PATCH 18/72] workflow pytest reference fix --- .github/workflows/docker.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index c7c3538ab8..67a4f4987f 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -114,6 +114,6 @@ jobs: (needs.build-dev.result == 'skipped' && needs.check-changes.outputs.tests == 'true')) }} - uses: ./.github/workflows/pytest.yml + uses: ./.github/workflows/tests.yml with: branch-tag: ${{ needs.build-dev.result != 'success' && 'dev' || needs.check-changes.outputs.branch-tag }} From b850fba715f69235b4507c807e04b4aa01c3d4b2 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:00:22 +0300 Subject: [PATCH 19/72] code cleanup needs permissions to call docker build --- .github/workflows/code-cleanup.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/code-cleanup.yml index a1403c1d79..ec46f3e4cd 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/code-cleanup.yml @@ -3,6 +3,8 @@ on: push permissions: contents: write + packages: write + pull-requests: read jobs: pre-commit: From 759fe4f9ae6724d757d86ec09a7805fbf2c35633 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:14:39 +0300 Subject: [PATCH 20/72] pre-commit hooks in dev container --- .devcontainer/devcontainer.json | 2 +- docker/dev/Dockerfile | 1 - docker/dev/dev-requirements.txt | 1 + 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b54b64bd31..860fcd87f2 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -12,7 +12,7 @@ "containerEnv": { "PYTHONPATH": "${localEnv:PYTHONPATH}:/workspaces/dimos" }, - "postCreateCommand": "git config --global --add safe.directory /workspaces/dimos", + "postCreateCommand": "git config --global --add safe.directory /workspaces/dimos && cd /workspaces/dimos && pre-commit install", "settings": { "notebook.formatOnSave.enabled": true, "notebook.codeActionsOnSave": { diff --git a/docker/dev/Dockerfile b/docker/dev/Dockerfile index f5a3810bcf..a210fd4e15 100644 --- a/docker/dev/Dockerfile +++ b/docker/dev/Dockerfile @@ -14,7 +14,6 @@ RUN apt-get install -y \ htop \ python-is-python3 \ iputils-ping \ - pre-commit \ wget # Configure git to trust any directory (resolves dubious ownership issues in containers) diff --git a/docker/dev/dev-requirements.txt b/docker/dev/dev-requirements.txt index 965af4cc9d..9633816cf2 100644 --- a/docker/dev/dev-requirements.txt +++ b/docker/dev/dev-requirements.txt @@ -1,2 +1,3 @@ ruff==0.11.10 mypy==1.15.0 +pre_commit==4.2.0 From 95854ae8d1859db511382b580ae91d0cfebc7409 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:46:13 +0300 Subject: [PATCH 21/72] Auto-compress test data: test_file.txt --- .pre-commit-config.yaml | 12 ++++++++++-- tests/data/.lfs/test_file.txt.tar.gz | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7a4549ab11..b3b885eb78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -8,8 +8,6 @@ repos: # args: [--fix] - id: ruff-format stages: [pre-commit] - - - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.6.0 hooks: @@ -23,3 +21,13 @@ repos: - id: check-yaml - id: pretty-format-json args: [ --autofix, --no-sort-keys ] + + - repo: local + hooks: + - id: lfs_push + name: lfs_push + always_run: true + pass_filenames: false + verbose: true + entry: bin/lfs_push_all + language: script diff --git a/tests/data/.lfs/test_file.txt.tar.gz b/tests/data/.lfs/test_file.txt.tar.gz index 3f28e33601..774fd6b694 100644 --- a/tests/data/.lfs/test_file.txt.tar.gz +++ b/tests/data/.lfs/test_file.txt.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6849b15423b67bcb3a4b6e636612e8fe1d4deb2478f27d50c2ae44f318e66cd8 +oid sha256:41091b53cf0bd0bda58709c356666bab1852452ec680bdfac647df99438408ad size 137 From ef8fe73f83323851d3b6b82f198f174513242eb7 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:48:20 +0300 Subject: [PATCH 22/72] Auto-compress test data: test_file.txt --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b3b885eb78..d4dcae7784 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -25,7 +25,7 @@ repos: - repo: local hooks: - id: lfs_push - name: lfs_push + name: lfs data compress and push always_run: true pass_filenames: false verbose: true From 9224f745d29c9a7eda21b8a8d164916a1ec53425 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:56:07 +0300 Subject: [PATCH 23/72] lfs hook fixes --- .pre-commit-config.yaml | 2 +- bin/lfs_push | 105 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 106 insertions(+), 1 deletion(-) create mode 100755 bin/lfs_push diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d4dcae7784..e11a291598 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -29,5 +29,5 @@ repos: always_run: true pass_filenames: false verbose: true - entry: bin/lfs_push_all + entry: bin/lfs_push language: script diff --git a/bin/lfs_push b/bin/lfs_push new file mode 100755 index 0000000000..d6a11050ca --- /dev/null +++ b/bin/lfs_push @@ -0,0 +1,105 @@ +#!/bin/bash + +# Pre-push hook to compress testing data directories +# Compresses directories in tests/data/* into tests/data/.lfs/dirname.tar.gz + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +#echo -e "${GREEN}Running test data compression check...${NC}" + +ROOT=$(git rev-parse --show-toplevel) + + +cd $ROOT +# Ensure tests/data/.lfs directory exists +mkdir -p tests/data/.lfs + +# Check if tests/data exists +if [ ! -d "tests/data" ]; then + echo -e "${YELLOW}No tests/data directory found, skipping compression.${NC}" + exit 0 +fi + +# Track if any compression was performed +compressed_dirs=() + +# Iterate through all directories in tests/data +for dir_path in tests/data/*; do + # Skip if no directories found (glob didn't match) + [ ! "$dir_path" ] && continue + + # Extract directory name + dir_name=$(basename "$dir_path") + + # Skip .lfs directory if it exists + [ "$dir_name" = ".lfs" ] && continue + + # Define compressed file path + compressed_file="tests/data/.lfs/${dir_name}.tar.gz" + + # Check if compressed file already exists + if [ -f "$compressed_file" ]; then + continue + fi + + echo -e " ${YELLOW}Compressing${NC} $dir_path -> $compressed_file" + + # Show directory size before compression + dir_size=$(du -sh "$dir_path" | cut -f1) + echo -e " Data size: ${YELLOW}$dir_size${NC}" + + # Create compressed archive with progress bar + # Use tar with gzip compression, excluding hidden files and common temp files + tar -czf "$compressed_file" \ + --exclude='*.tmp' \ + --exclude='*.temp' \ + --exclude='.DS_Store' \ + --exclude='Thumbs.db' \ + --checkpoint=1000 \ + --checkpoint-action=dot \ + -C "tests/data" \ + "$dir_name" + + if [ $? -eq 0 ]; then + # Show compressed file size + compressed_size=$(du -sh "$compressed_file" | cut -f1) + echo -e " ${GREEN}✓${NC} Successfully compressed $dir_name (${GREEN}$dir_size${NC} → ${GREEN}$compressed_size${NC})" + compressed_dirs+=("$dir_name") + + # Add the compressed file to git LFS tracking + + git add "$compressed_file" + + echo -e "${GREEN}✓${NC} git-add $compressed_file" + + else + echo -e " ${RED}✗${NC} Failed to compress $dir_name" + exit 1 + fi +done + +if [ ${#compressed_dirs[@]} -gt 0 ]; then + # Create commit message with compressed directory names + if [ ${#compressed_dirs[@]} -eq 1 ]; then + commit_msg="Auto-compress test data: ${compressed_dirs[0]}" + else + # Join array elements with commas + dirs_list=$(IFS=', '; echo "${compressed_dirs[*]}") + commit_msg="Auto-compress test data: ${dirs_list}" + fi + + #git commit -m "$commit_msg" + echo -e "${GREEN}✓${NC} Compressed files committed automatically, uploading..." + git lfs push origin $(git branch --show-current) + echo -e "${GREEN}✓${NC} Uploaded to LFS" + echo -e "${GREEN}✓${NC} New commit has been added" +else + echo -e "${GREEN}✓${NC} No test data to compress" +fi + From 97319cf92d445463c30c631e7c57ce5793c7b469 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:58:32 +0300 Subject: [PATCH 24/72] lfs hook fixes 2 --- bin/lfs_push | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/bin/lfs_push b/bin/lfs_push index d6a11050ca..85a35275c0 100755 --- a/bin/lfs_push +++ b/bin/lfs_push @@ -76,7 +76,7 @@ for dir_path in tests/data/*; do git add "$compressed_file" - echo -e "${GREEN}✓${NC} git-add $compressed_file" + echo -e " ${GREEN}✓${NC} git-add $compressed_file" else echo -e " ${RED}✗${NC} Failed to compress $dir_name" @@ -95,10 +95,9 @@ if [ ${#compressed_dirs[@]} -gt 0 ]; then fi #git commit -m "$commit_msg" - echo -e "${GREEN}✓${NC} Compressed files committed automatically, uploading..." + echo -e "${GREEN}✓${NC} Compressed file references added. Uploading..." git lfs push origin $(git branch --show-current) echo -e "${GREEN}✓${NC} Uploaded to LFS" - echo -e "${GREEN}✓${NC} New commit has been added" else echo -e "${GREEN}✓${NC} No test data to compress" fi From 20866bdaef1d4cfbcf78cf94409e796e3c79ef26 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 13:58:59 +0300 Subject: [PATCH 25/72] triggering rebuild 2 --- dimos/utils/test_testing.py | 2 ++ tests/data/.lfs/test_file2.txt.tar.gz | 3 +++ 2 files changed, 5 insertions(+) create mode 100644 tests/data/.lfs/test_file2.txt.tar.gz diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index c1c0c10072..d729866d3c 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -3,6 +3,8 @@ import subprocess from dimos.utils import testing +# trigger rebuild1 + def test_pull_file(): repo_root = testing._get_repo_root() diff --git a/tests/data/.lfs/test_file2.txt.tar.gz b/tests/data/.lfs/test_file2.txt.tar.gz new file mode 100644 index 0000000000..56c614ebd1 --- /dev/null +++ b/tests/data/.lfs/test_file2.txt.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:57604ebe6c01c7bcd180f5d87eaff64a275f1e1b1955b44b3d500c23c93d3b7e +size 137 From 5d6a960ec0383616791c68ce24797ecfdf24387d Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:00:22 +0300 Subject: [PATCH 26/72] final cleanup of the lfs script --- bin/lfs_push | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/bin/lfs_push b/bin/lfs_push index 85a35275c0..e4c70bd633 100755 --- a/bin/lfs_push +++ b/bin/lfs_push @@ -1,7 +1,6 @@ #!/bin/bash - -# Pre-push hook to compress testing data directories # Compresses directories in tests/data/* into tests/data/.lfs/dirname.tar.gz +# Pushes to LFS set -e @@ -14,9 +13,8 @@ NC='\033[0m' # No Color #echo -e "${GREEN}Running test data compression check...${NC}" ROOT=$(git rev-parse --show-toplevel) - - cd $ROOT + # Ensure tests/data/.lfs directory exists mkdir -p tests/data/.lfs @@ -73,7 +71,6 @@ for dir_path in tests/data/*; do compressed_dirs+=("$dir_name") # Add the compressed file to git LFS tracking - git add "$compressed_file" echo -e " ${GREEN}✓${NC} git-add $compressed_file" From 1b4f155a79af57608a2c4fb201393c89c3bbbb40 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:00:57 +0300 Subject: [PATCH 27/72] removed temp test files --- tests/data/.lfs/test_file.txt.tar.gz | 3 --- tests/data/.lfs/test_file2.txt.tar.gz | 3 --- 2 files changed, 6 deletions(-) delete mode 100644 tests/data/.lfs/test_file.txt.tar.gz delete mode 100644 tests/data/.lfs/test_file2.txt.tar.gz diff --git a/tests/data/.lfs/test_file.txt.tar.gz b/tests/data/.lfs/test_file.txt.tar.gz deleted file mode 100644 index 774fd6b694..0000000000 --- a/tests/data/.lfs/test_file.txt.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:41091b53cf0bd0bda58709c356666bab1852452ec680bdfac647df99438408ad -size 137 diff --git a/tests/data/.lfs/test_file2.txt.tar.gz b/tests/data/.lfs/test_file2.txt.tar.gz deleted file mode 100644 index 56c614ebd1..0000000000 --- a/tests/data/.lfs/test_file2.txt.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:57604ebe6c01c7bcd180f5d87eaff64a275f1e1b1955b44b3d500c23c93d3b7e -size 137 From 226eaaf8adaa78d31be78e5fdc27f69958eff7c3 Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Wed, 28 May 2025 11:02:42 +0000 Subject: [PATCH 28/72] CI code cleanup --- tests/data/.lfs/*.tar.gz | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 tests/data/.lfs/*.tar.gz diff --git a/tests/data/.lfs/*.tar.gz b/tests/data/.lfs/*.tar.gz new file mode 100644 index 0000000000..015ffb632b --- /dev/null +++ b/tests/data/.lfs/*.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85cea451eec057fa7e734548ca3ba6d779ed5836a3f9de14b8394575ef0d7d8e +size 45 From 64824ac801f70ac6f0f217e9c4e424b19764f220 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:20:43 +0300 Subject: [PATCH 29/72] cleanup --- .pre-commit-config.yaml | 3 ++- dimos/utils/test_testing.py | 2 -- tests/data/.lfs/*.tar.gz | 3 --- 3 files changed, 2 insertions(+), 6 deletions(-) delete mode 100644 tests/data/.lfs/*.tar.gz diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e11a291598..c735d3382e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -20,12 +20,13 @@ repos: - id: check-toml - id: check-yaml - id: pretty-format-json + name: format json args: [ --autofix, --no-sort-keys ] - repo: local hooks: - id: lfs_push - name: lfs data compress and push + name: new LFS files compress and push always_run: true pass_filenames: false verbose: true diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index d729866d3c..c1c0c10072 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -3,8 +3,6 @@ import subprocess from dimos.utils import testing -# trigger rebuild1 - def test_pull_file(): repo_root = testing._get_repo_root() diff --git a/tests/data/.lfs/*.tar.gz b/tests/data/.lfs/*.tar.gz deleted file mode 100644 index 015ffb632b..0000000000 --- a/tests/data/.lfs/*.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:85cea451eec057fa7e734548ca3ba6d779ed5836a3f9de14b8394575ef0d7d8e -size 45 From adfaed3abb0a79526c9a313c72436c0a11fbbf84 Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Wed, 28 May 2025 11:21:31 +0000 Subject: [PATCH 30/72] CI code cleanup --- tests/data/.lfs/*.tar.gz | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 tests/data/.lfs/*.tar.gz diff --git a/tests/data/.lfs/*.tar.gz b/tests/data/.lfs/*.tar.gz new file mode 100644 index 0000000000..015ffb632b --- /dev/null +++ b/tests/data/.lfs/*.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85cea451eec057fa7e734548ca3ba6d779ed5836a3f9de14b8394575ef0d7d8e +size 45 From ec8e3ea2dbbd86b0597adf3e759c5ae0add4569c Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:34:07 +0300 Subject: [PATCH 31/72] pre-commit doesn't push to LFS, it just checks --- .pre-commit-config.yaml | 7 +++---- bin/lfs_check | 40 ++++++++++++++++++++++++++++++++++++++++ bin/lfs_push | 3 --- tests/data/.lfs/*.tar.gz | 3 --- 4 files changed, 43 insertions(+), 10 deletions(-) create mode 100755 bin/lfs_check delete mode 100644 tests/data/.lfs/*.tar.gz diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c735d3382e..46161bb5e6 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -25,10 +25,9 @@ repos: - repo: local hooks: - - id: lfs_push - name: new LFS files compress and push + - id: lfs_check + name: LFS data always_run: true pass_filenames: false - verbose: true - entry: bin/lfs_push + entry: bin/lfs_check language: script diff --git a/bin/lfs_check b/bin/lfs_check new file mode 100755 index 0000000000..c69d9e5838 --- /dev/null +++ b/bin/lfs_check @@ -0,0 +1,40 @@ +#!/bin/bash + +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +NC='\033[0m' + +ROOT=$(git rev-parse --show-toplevel) +cd $ROOT + +new_data=() + +# Iterate through all directories in tests/data +for dir_path in tests/data/*; do + # Skip if no directories found (glob didn't match) + [ ! "$dir_path" ] && continue + + # Extract directory name + dir_name=$(basename "$dir_path") + + # Skip .lfs directory if it exists + [ "$dir_name" = ".lfs" ] && continue + + # Define compressed file path + compressed_file="tests/data/.lfs/${dir_name}.tar.gz" + + # Check if compressed file already exists + if [ -f "$compressed_file" ]; then + continue + fi + + new_data+=("$dir_name") +done + +if [ ${#new_data[@]} -gt 0 ]; then + echo -e "${RED}✗${NC} New test data detected:" + echo -e " ${GREEN}${new_data[@]}${NC}" + echo -e "\neither delete or run ./bin/lfs_push to add the data to LFS and your commit" + exit 1 +fi diff --git a/bin/lfs_push b/bin/lfs_push index e4c70bd633..47f50b1354 100755 --- a/bin/lfs_push +++ b/bin/lfs_push @@ -15,9 +15,6 @@ NC='\033[0m' # No Color ROOT=$(git rev-parse --show-toplevel) cd $ROOT -# Ensure tests/data/.lfs directory exists -mkdir -p tests/data/.lfs - # Check if tests/data exists if [ ! -d "tests/data" ]; then echo -e "${YELLOW}No tests/data directory found, skipping compression.${NC}" diff --git a/tests/data/.lfs/*.tar.gz b/tests/data/.lfs/*.tar.gz deleted file mode 100644 index 015ffb632b..0000000000 --- a/tests/data/.lfs/*.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:85cea451eec057fa7e734548ca3ba6d779ed5836a3f9de14b8394575ef0d7d8e -size 45 From 4a7aed7aa2b3dbf6186213afd03fa7e28d419c57 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:36:47 +0300 Subject: [PATCH 32/72] null glob fix --- bin/lfs_check | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/bin/lfs_check b/bin/lfs_check index c69d9e5838..f0cbdcc695 100755 --- a/bin/lfs_check +++ b/bin/lfs_check @@ -10,10 +10,11 @@ cd $ROOT new_data=() +# Enable nullglob to make globs expand to nothing when not matching +shopt -s nullglob + # Iterate through all directories in tests/data for dir_path in tests/data/*; do - # Skip if no directories found (glob didn't match) - [ ! "$dir_path" ] && continue # Extract directory name dir_name=$(basename "$dir_path") From bf887497a324568e2dc0db7feb2b038fb4653fd3 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:38:59 +0300 Subject: [PATCH 33/72] slightly nicer lfs check output --- bin/lfs_check | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bin/lfs_check b/bin/lfs_check index f0cbdcc695..ce0dada6c1 100755 --- a/bin/lfs_check +++ b/bin/lfs_check @@ -36,6 +36,6 @@ done if [ ${#new_data[@]} -gt 0 ]; then echo -e "${RED}✗${NC} New test data detected:" echo -e " ${GREEN}${new_data[@]}${NC}" - echo -e "\neither delete or run ./bin/lfs_push to add the data to LFS and your commit" + echo -e "\neither delete or run ${GREEN}./bin/lfs_push${NC} to add the data to LFS and your commit" exit 1 fi From 2d8049cf04de4956479f3ffa4d673780900fe3e7 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:44:09 +0300 Subject: [PATCH 34/72] small workflow naming fixes --- .github/workflows/{code-cleanup.yml => cleanup.yml} | 4 ++-- .github/workflows/docker.yml | 2 +- .github/workflows/tests.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) rename .github/workflows/{code-cleanup.yml => cleanup.yml} (94%) diff --git a/.github/workflows/code-cleanup.yml b/.github/workflows/cleanup.yml similarity index 94% rename from .github/workflows/code-cleanup.yml rename to .github/workflows/cleanup.yml index ec46f3e4cd..ee1ec788e2 100644 --- a/.github/workflows/code-cleanup.yml +++ b/.github/workflows/cleanup.yml @@ -1,4 +1,4 @@ -name: code-cleanup +name: cleanup on: push permissions: @@ -28,6 +28,6 @@ jobs: with: commit_message: "CI code cleanup" - run-docker-builds: + docker: needs: [pre-commit] uses: ./.github/workflows/docker.yml diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 67a4f4987f..0ace5cbd81 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,4 +1,4 @@ -name: docker-tree +name: docker on: workflow_call permissions: diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index ae1be9b751..7efc7bad01 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -1,4 +1,4 @@ -name: testing +name: tests on: workflow_call: From 2de8581aba158b1dd4ec6f49b46521b24e71e49e Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 14:52:35 +0300 Subject: [PATCH 35/72] better lfs_check output --- bin/lfs_check | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/bin/lfs_check b/bin/lfs_check index ce0dada6c1..09d09bd219 100755 --- a/bin/lfs_check +++ b/bin/lfs_check @@ -34,8 +34,9 @@ for dir_path in tests/data/*; do done if [ ${#new_data[@]} -gt 0 ]; then - echo -e "${RED}✗${NC} New test data detected:" + echo -e "${RED}✗${NC} New test data detected at /tests/data:" echo -e " ${GREEN}${new_data[@]}${NC}" - echo -e "\neither delete or run ${GREEN}./bin/lfs_push${NC} to add the data to LFS and your commit" + echo -e "\nEither delete or run ${GREEN}./bin/lfs_push${NC}" + echo -e "(lfs_push will compress the files into /tests/data/.lfs/, upload to LFS, and add them to your commit)" exit 1 fi From 022a5bf90836f15fcd3b6f5c8e69920cdc30f6aa Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 15:27:39 +0300 Subject: [PATCH 36/72] renaming actions for better UI view --- .github/workflows/docker.yml | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 0ace5cbd81..a5500530e3 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -56,7 +56,7 @@ jobs: echo "branch tag determined: ${branch_tag}" echo branch_tag="${branch_tag}" >> "$GITHUB_OUTPUT" - build-ros: + ros: needs: [check-changes] if: needs.check-changes.outputs.ros == 'true' uses: ./.github/workflows/_docker-build-template.yml @@ -66,21 +66,21 @@ jobs: # just a debugger inspect-needs: - needs: [check-changes, build-ros] + needs: [check-changes, ros] runs-on: dimos-runner-ubuntu-2204 if: always() steps: - run: | echo '${{ toJSON(needs) }}' - build-python: - needs: [check-changes, build-ros] + python: + needs: [check-changes, ros] if: | ${{ always() && !cancelled() && needs.check-changes.result == 'success' && - ((needs.build-ros.result == 'success') || - (needs.build-ros.result == 'skipped' && + ((needs.ros.result == 'success') || + (needs.ros.result == 'skipped' && needs.check-changes.outputs.python == 'true')) }} uses: ./.github/workflows/_docker-build-template.yml @@ -89,14 +89,14 @@ jobs: target: base-ros-python freespace: true - build-dev: - needs: [check-changes, build-python] + dev: + needs: [check-changes, python] if: | ${{ always() && !cancelled() && needs.check-changes.result == 'success' && - ((needs.build-python.result == 'success') || - (needs.build-python.result == 'skipped' && + ((needs.python.result == 'success') || + (needs.python.result == 'skipped' && needs.check-changes.outputs.dev == 'true')) }} uses: ./.github/workflows/_docker-build-template.yml @@ -105,15 +105,15 @@ jobs: target: dev run-tests: - needs: [check-changes, build-dev] + needs: [check-changes, dev] if: | ${{ always() && !cancelled() && needs.check-changes.result == 'success' && - ((needs.build-dev.result == 'success') || - (needs.build-dev.result == 'skipped' && + ((needs.dev.result == 'success') || + (needs.dev.result == 'skipped' && needs.check-changes.outputs.tests == 'true')) }} uses: ./.github/workflows/tests.yml with: - branch-tag: ${{ needs.build-dev.result != 'success' && 'dev' || needs.check-changes.outputs.branch-tag }} + branch-tag: ${{ needs.dev.result != 'success' && 'dev' || needs.check-changes.outputs.branch-tag }} From 0592c67391d35e7212de3a4a0436b6eafeac66e9 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 15:29:49 +0300 Subject: [PATCH 37/72] even shorter naming --- .github/workflows/cleanup.yml | 2 +- .github/workflows/docker.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/cleanup.yml b/.github/workflows/cleanup.yml index ee1ec788e2..101daefebb 100644 --- a/.github/workflows/cleanup.yml +++ b/.github/workflows/cleanup.yml @@ -28,6 +28,6 @@ jobs: with: commit_message: "CI code cleanup" - docker: + dkr: needs: [pre-commit] uses: ./.github/workflows/docker.yml diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index a5500530e3..b2f8eeae8e 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,4 +1,4 @@ -name: docker +name: dkr on: workflow_call permissions: From adeb3f732fd02ddd870d03a2a0b4e307cd87f5a2 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 15:31:44 +0300 Subject: [PATCH 38/72] checking explicit action naming --- .github/workflows/cleanup.yml | 2 +- .github/workflows/docker.yml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/cleanup.yml b/.github/workflows/cleanup.yml index 101daefebb..ee1ec788e2 100644 --- a/.github/workflows/cleanup.yml +++ b/.github/workflows/cleanup.yml @@ -28,6 +28,6 @@ jobs: with: commit_message: "CI code cleanup" - dkr: + docker: needs: [pre-commit] uses: ./.github/workflows/docker.yml diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index b2f8eeae8e..4217f0f974 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,4 +1,4 @@ -name: dkr +name: docker on: workflow_call permissions: @@ -57,6 +57,7 @@ jobs: echo branch_tag="${branch_tag}" >> "$GITHUB_OUTPUT" ros: + name: docker-ros needs: [check-changes] if: needs.check-changes.outputs.ros == 'true' uses: ./.github/workflows/_docker-build-template.yml From 1bcb9bd4f91091522f9d0a5d49314f22cb3d900a Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 15:36:02 +0300 Subject: [PATCH 39/72] decoupling workflows --- .github/workflows/cleanup.yml | 6 ------ .github/workflows/docker.yml | 21 +++++---------------- .github/workflows/tests.yml | 10 ++++------ 3 files changed, 9 insertions(+), 28 deletions(-) diff --git a/.github/workflows/cleanup.yml b/.github/workflows/cleanup.yml index ee1ec788e2..2eff2ffd7e 100644 --- a/.github/workflows/cleanup.yml +++ b/.github/workflows/cleanup.yml @@ -3,8 +3,6 @@ on: push permissions: contents: write - packages: write - pull-requests: read jobs: pre-commit: @@ -27,7 +25,3 @@ jobs: uses: stefanzweifel/git-auto-commit-action@v5 with: commit_message: "CI code cleanup" - - docker: - needs: [pre-commit] - uses: ./.github/workflows/docker.yml diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 4217f0f974..089b2ed36a 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,5 +1,9 @@ name: docker -on: workflow_call +on: + workflow_run: + workflows: ["cleanup"] + types: + - completed permissions: contents: read @@ -57,7 +61,6 @@ jobs: echo branch_tag="${branch_tag}" >> "$GITHUB_OUTPUT" ros: - name: docker-ros needs: [check-changes] if: needs.check-changes.outputs.ros == 'true' uses: ./.github/workflows/_docker-build-template.yml @@ -104,17 +107,3 @@ jobs: with: branch-tag: ${{ needs.check-changes.outputs.branch-tag }} target: dev - - run-tests: - needs: [check-changes, dev] - if: | - ${{ - always() && !cancelled() && - needs.check-changes.result == 'success' && - ((needs.dev.result == 'success') || - (needs.dev.result == 'skipped' && - needs.check-changes.outputs.tests == 'true')) - }} - uses: ./.github/workflows/tests.yml - with: - branch-tag: ${{ needs.dev.result != 'success' && 'dev' || needs.check-changes.outputs.branch-tag }} diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 7efc7bad01..325c88903e 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -1,12 +1,10 @@ name: tests on: - workflow_call: - inputs: - branch-tag: - required: true - type: string - default: "latest" + workflow_run: + workflows: ["docker"] + types: + - completed permissions: contents: read From 0c23a15e534033feafa666335d9ed276280dc4ed Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 15:39:01 +0300 Subject: [PATCH 40/72] re-coupling workflows --- .github/workflows/cleanup.yml | 6 ++++++ .github/workflows/docker.yml | 20 +++++++++++++++----- .github/workflows/tests.yml | 10 ++++++---- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/.github/workflows/cleanup.yml b/.github/workflows/cleanup.yml index 2eff2ffd7e..ee1ec788e2 100644 --- a/.github/workflows/cleanup.yml +++ b/.github/workflows/cleanup.yml @@ -3,6 +3,8 @@ on: push permissions: contents: write + packages: write + pull-requests: read jobs: pre-commit: @@ -25,3 +27,7 @@ jobs: uses: stefanzweifel/git-auto-commit-action@v5 with: commit_message: "CI code cleanup" + + docker: + needs: [pre-commit] + uses: ./.github/workflows/docker.yml diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 089b2ed36a..a5500530e3 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -1,9 +1,5 @@ name: docker -on: - workflow_run: - workflows: ["cleanup"] - types: - - completed +on: workflow_call permissions: contents: read @@ -107,3 +103,17 @@ jobs: with: branch-tag: ${{ needs.check-changes.outputs.branch-tag }} target: dev + + run-tests: + needs: [check-changes, dev] + if: | + ${{ + always() && !cancelled() && + needs.check-changes.result == 'success' && + ((needs.dev.result == 'success') || + (needs.dev.result == 'skipped' && + needs.check-changes.outputs.tests == 'true')) + }} + uses: ./.github/workflows/tests.yml + with: + branch-tag: ${{ needs.dev.result != 'success' && 'dev' || needs.check-changes.outputs.branch-tag }} diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 325c88903e..7efc7bad01 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -1,10 +1,12 @@ name: tests on: - workflow_run: - workflows: ["docker"] - types: - - completed + workflow_call: + inputs: + branch-tag: + required: true + type: string + default: "latest" permissions: contents: read From 15aa2d98a3cae95dd4a24510e856478073411b80 Mon Sep 17 00:00:00 2001 From: lesh Date: Wed, 28 May 2025 18:41:11 +0300 Subject: [PATCH 41/72] sensor replay test --- dimos/robot/unitree_webrtc/type/costmap.py | 44 ++++-- dimos/robot/unitree_webrtc/type/test_map.py | 23 ++++ dimos/robot/unitree_webrtc/type/vector.py | 140 -------------------- dimos/types/vector.py | 2 - dimos/utils/test_testing.py | 19 +++ dimos/utils/testing.py | 84 +++++++++++- 6 files changed, 156 insertions(+), 156 deletions(-) diff --git a/dimos/robot/unitree_webrtc/type/costmap.py b/dimos/robot/unitree_webrtc/type/costmap.py index 814184479e..ba8dba55e3 100644 --- a/dimos/robot/unitree_webrtc/type/costmap.py +++ b/dimos/robot/unitree_webrtc/type/costmap.py @@ -231,6 +231,34 @@ def smudge( origin=self.origin, ) + @property + def total_cells(self) -> int: + return self.width * self.height + + @property + def occupied_cells(self) -> int: + return np.sum(self.grid >= 0.1) + + @property + def unknown_cells(self) -> int: + return np.sum(self.grid == -1) + + @property + def free_cells(self) -> int: + return self.total_cells - self.occupied_cells - self.unknown_cells + + @property + def free_percent(self) -> float: + return (self.free_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + + @property + def occupied_percent(self) -> float: + return (self.occupied_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + + @property + def unknown_percent(self) -> float: + return (self.unknown_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + def __str__(self) -> str: """ Create a string representation of the Costmap. @@ -238,16 +266,6 @@ def __str__(self) -> str: Returns: A formatted string with key costmap information """ - # Calculate occupancy statistics - total_cells = self.width * self.height - occupied_cells = np.sum(self.grid >= 0.1) - unknown_cells = np.sum(self.grid == -1) - free_cells = total_cells - occupied_cells - unknown_cells - - # Calculate percentages - occupied_percent = (occupied_cells / total_cells) * 100 - unknown_percent = (unknown_cells / total_cells) * 100 - free_percent = (free_cells / total_cells) * 100 cell_info = [ "▦ Costmap", @@ -255,9 +273,9 @@ def __str__(self) -> str: f"({self.width * self.resolution:.1f}x{self.height * self.resolution:.1f}m @", f"{1 / self.resolution:.0f}cm res)", f"Origin: ({x(self.origin):.2f}, {y(self.origin):.2f})", - f"▣ {occupied_percent:.1f}%", - f"□ {free_percent:.1f}%", - f"◌ {unknown_percent:.1f}%", + f"▣ {self.occupied_percent:.1f}%", + f"□ {self.free_percent:.1f}%", + f"◌ {self.unknown_percent:.1f}%", ] return " ".join(cell_info) diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 25deccda00..180d473eb7 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -1,8 +1,10 @@ import pytest from dimos.robot.unitree_webrtc.testing.mock import Mock from dimos.robot.unitree_webrtc.testing.helpers import show3d_stream, show3d +from dimos.robot.unitree_webrtc.type.lidar import LidarMessage from dimos.utils.reactive import backpressure from dimos.robot.unitree_webrtc.type.map import splice_sphere, Map +from dimos.utils.testing import SensorReplay @pytest.mark.vis @@ -36,3 +38,24 @@ def test_robot_vis(): clearframe=True, title="gloal dynamic map test", ) + + +def test_robot_mapping(): + lidar_stream = SensorReplay("office_lidar", autocast=lambda x: LidarMessage.from_msg(x)) + map = Map(voxel_size=0.5) + map.consume(lidar_stream.stream(rate_hz=100.0)).subscribe(lambda x: ...) + + costmap = map.costmap + + shape = costmap.grid.shape + assert shape[0] > 150 + assert shape[1] > 150 + + assert costmap.unknown_percent > 80 + assert costmap.unknown_percent < 90 + + assert costmap.free_percent > 5 + assert costmap.free_percent < 10 + + assert costmap.occupied_percent > 8 + assert costmap.occupied_percent < 15 diff --git a/dimos/robot/unitree_webrtc/type/vector.py b/dimos/robot/unitree_webrtc/type/vector.py index e5fb446884..0343a71118 100644 --- a/dimos/robot/unitree_webrtc/type/vector.py +++ b/dimos/robot/unitree_webrtc/type/vector.py @@ -116,7 +116,6 @@ def __add__(self: T, other: Union["Vector", Iterable[float]]) -> T: def __sub__(self: T, other: Union["Vector", Iterable[float]]) -> T: if isinstance(other, Vector): - print(self, other) return self.__class__(self._data - other._data) return self.__class__(self._data - np.array(other, dtype=float)) @@ -433,142 +432,3 @@ def z(value: VectorLike) -> float: else: arr = to_numpy(value) return float(arr[2]) if len(arr) > 2 else 0.0 - - -if __name__ == "__main__": - # Test vectors in various directions - test_vectors = [ - Vector(1, 0), # Right - Vector(1, 1), # Up-Right - Vector(0, 1), # Up - Vector(-1, 1), # Up-Left - Vector(-1, 0), # Left - Vector(-1, -1), # Down-Left - Vector(0, -1), # Down - Vector(1, -1), # Down-Right - Vector(0.5, 0.5), # Up-Right (shorter) - Vector(-3, 4), # Up-Left (longer) - ] - - for v in test_vectors: - print(str(v)) - - # Test the vector compatibility functions - print("Testing vectortypes.py conversion functions\n") - - # Create test vectors in different formats - vector_obj = Vector(1.0, 2.0, 3.0) - numpy_arr = np.array([4.0, 5.0, 6.0]) - tuple_vec = (7.0, 8.0, 9.0) - list_vec = [10.0, 11.0, 12.0] - - print("Original values:") - print(f"Vector: {vector_obj}") - print(f"NumPy: {numpy_arr}") - print(f"Tuple: {tuple_vec}") - print(f"List: {list_vec}") - print() - - # Test to_numpy - print("to_numpy() conversions:") - print(f"Vector → NumPy: {to_numpy(vector_obj)}") - print(f"NumPy → NumPy: {to_numpy(numpy_arr)}") - print(f"Tuple → NumPy: {to_numpy(tuple_vec)}") - print(f"List → NumPy: {to_numpy(list_vec)}") - print() - - # Test to_vector - print("to_vector() conversions:") - print(f"Vector → Vector: {to_vector(vector_obj)}") - print(f"NumPy → Vector: {to_vector(numpy_arr)}") - print(f"Tuple → Vector: {to_vector(tuple_vec)}") - print(f"List → Vector: {to_vector(list_vec)}") - print() - - # Test to_tuple - print("to_tuple() conversions:") - print(f"Vector → Tuple: {to_tuple(vector_obj)}") - print(f"NumPy → Tuple: {to_tuple(numpy_arr)}") - print(f"Tuple → Tuple: {to_tuple(tuple_vec)}") - print(f"List → Tuple: {to_tuple(list_vec)}") - print() - - # Test to_list - print("to_list() conversions:") - print(f"Vector → List: {to_list(vector_obj)}") - print(f"NumPy → List: {to_list(numpy_arr)}") - print(f"Tuple → List: {to_list(tuple_vec)}") - print(f"List → List: {to_list(list_vec)}") - print() - - # Test component extraction - print("Component extraction:") - print("x() function:") - print(f"x(Vector): {x(vector_obj)}") - print(f"x(NumPy): {x(numpy_arr)}") - print(f"x(Tuple): {x(tuple_vec)}") - print(f"x(List): {x(list_vec)}") - print() - - print("y() function:") - print(f"y(Vector): {y(vector_obj)}") - print(f"y(NumPy): {y(numpy_arr)}") - print(f"y(Tuple): {y(tuple_vec)}") - print(f"y(List): {y(list_vec)}") - print() - - print("z() function:") - print(f"z(Vector): {z(vector_obj)}") - print(f"z(NumPy): {z(numpy_arr)}") - print(f"z(Tuple): {z(tuple_vec)}") - print(f"z(List): {z(list_vec)}") - print() - - # Test dimension checking - print("Dimension checking:") - vec2d = Vector(1.0, 2.0) - vec3d = Vector(1.0, 2.0, 3.0) - arr2d = np.array([1.0, 2.0]) - arr3d = np.array([1.0, 2.0, 3.0]) - - print(f"is_2d(Vector(1,2)): {is_2d(vec2d)}") - print(f"is_2d(Vector(1,2,3)): {is_2d(vec3d)}") - print(f"is_2d(np.array([1,2])): {is_2d(arr2d)}") - print(f"is_2d(np.array([1,2,3])): {is_2d(arr3d)}") - print(f"is_2d((1,2)): {is_2d((1.0, 2.0))}") - print(f"is_2d((1,2,3)): {is_2d((1.0, 2.0, 3.0))}") - print() - - print(f"is_3d(Vector(1,2)): {is_3d(vec2d)}") - print(f"is_3d(Vector(1,2,3)): {is_3d(vec3d)}") - print(f"is_3d(np.array([1,2])): {is_3d(arr2d)}") - print(f"is_3d(np.array([1,2,3])): {is_3d(arr3d)}") - print(f"is_3d((1,2)): {is_3d((1.0, 2.0))}") - print(f"is_3d((1,2,3)): {is_3d((1.0, 2.0, 3.0))}") - print() - - # Test the Protocol interface - print("Testing VectorLike Protocol:") - print(f"isinstance(Vector(1,2), VectorLike): {isinstance(vec2d, VectorLike)}") - print(f"isinstance(np.array([1,2]), VectorLike): {isinstance(arr2d, VectorLike)}") - print(f"isinstance((1,2), VectorLike): {isinstance((1.0, 2.0), VectorLike)}") - print(f"isinstance([1,2], VectorLike): {isinstance([1.0, 2.0], VectorLike)}") - print() - - # Test mixed operations using different vector types - # These functions aren't defined in vectortypes, but demonstrate the concept - def distance(a: VectorLike, b: VectorLike) -> float: - a_np = to_numpy(a) - b_np = to_numpy(b) - diff = a_np - b_np - return float(np.sqrt(np.sum(diff * diff))) - - def midpoint(a: VectorLike, b: VectorLike) -> NDArray[np.float64]: - a_np = to_numpy(a) - b_np = to_numpy(b) - return (a_np + b_np) / 2 - - print("Mixed operations between different vector types:") - print(f"distance(Vector(1,2,3), [4,5,6]): {distance(vec3d, [4.0, 5.0, 6.0])}") - print(f"distance(np.array([1,2,3]), (4,5,6)): {distance(arr3d, (4.0, 5.0, 6.0))}") - print(f"midpoint(Vector(1,2,3), np.array([4,5,6])): {midpoint(vec3d, numpy_arr)}") diff --git a/dimos/types/vector.py b/dimos/types/vector.py index eb43c04945..d5fcd08165 100644 --- a/dimos/types/vector.py +++ b/dimos/types/vector.py @@ -91,8 +91,6 @@ def __str__(self) -> str: def getArrow(): repr = ["←", "↖", "↑", "↗", "→", "↘", "↓", "↙"] - print("SELF X", self.x) - print("SELF Y", self.y) if self.x == 0 and self.y == 0: return "·" diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index c1c0c10072..796c190c8a 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -2,6 +2,7 @@ import os import subprocess from dimos.utils import testing +from dimos.robot.unitree_webrtc.type.lidar import LidarMessage def test_pull_file(): @@ -108,3 +109,21 @@ def test_pull_dir(): with file.open("rb") as f: sha256 = hashlib.sha256(f.read()).hexdigest() assert sha256 == expected_hash + + +def test_sensor_replay(): + counter = 0 + for message in testing.SensorReplay(name="office_lidar").iterate(): + counter += 1 + assert isinstance(message, dict) + assert counter == 500 + + +def test_sensor_replay_cast(): + counter = 0 + for message in testing.SensorReplay( + name="office_lidar", autocast=lambda x: LidarMessage.from_msg(x) + ).iterate(): + counter += 1 + assert isinstance(message, LidarMessage) + assert counter == 500 diff --git a/dimos/utils/testing.py b/dimos/utils/testing.py index 53b9849718..bd9a37d00a 100644 --- a/dimos/utils/testing.py +++ b/dimos/utils/testing.py @@ -1,8 +1,15 @@ import subprocess import tarfile +import glob +import os +import pickle from functools import cache from pathlib import Path -from typing import Union +from typing import Union, Iterator, TypeVar, Generic, Optional, Any, Type, Callable + +from reactivex import operators as ops +from reactivex import interval, from_iterable +from reactivex.observable import Observable def _check_git_lfs_available() -> None: @@ -140,3 +147,78 @@ def testData(filename: Union[str, Path]) -> Path: return file_path return _decompress_archive(_pull_lfs_archive(filename)) + + +T = TypeVar("T") + + +class SensorReplay(Generic[T]): + """Generic sensor data replay utility. + + Args: + name: The name of the test dataset + autocast: Optional function that takes unpickled data and returns a processed result. + For example: lambda data: LidarMessage.from_msg(data) + """ + + def __init__(self, name: str, autocast: Optional[Callable[[Any], T]] = None): + self.root_dir = testData(name) + self.autocast = autocast + self.cnt = 0 + + def load(self, *names: Union[int, str]) -> Union[T, Any, list[T], list[Any]]: + if len(names) == 1: + return self.load_one(names[0]) + return list(map(lambda name: self.load_one(name), names)) + + def load_one(self, name: Union[int, str, Path]) -> Union[T, Any]: + if isinstance(name, int): + full_path = self.root_dir / f"/{name:03d}.pickle" + elif isinstance(name, Path): + full_path = self.root_dir / f"/{name}.pickle" + else: + full_path = name + + with open(full_path, "rb") as f: + data = pickle.load(f) + if self.autocast: + return self.autocast(data) + return data + + def iterate(self) -> Iterator[Union[T, Any]]: + pattern = os.path.join(self.root_dir, "*") + for file_path in sorted(glob.glob(pattern)): + yield self.load_one(file_path) + + def stream(self, rate_hz: float = 10.0) -> Observable[Union[T, Any]]: + sleep_time = 1.0 / rate_hz + + return from_iterable(self.iterate()).pipe( + ops.zip(interval(sleep_time)), + ops.map(lambda x: x[0] if isinstance(x, tuple) else x), + ) + + def save_stream(self, observable: Observable[Union[T, Any]]) -> Observable[int]: + return observable.pipe(ops.map(lambda frame: self.save_one(frame))) + + def save(self, *frames) -> int: + [self.save_one(frame) for frame in frames] + return self.cnt + + def save_one(self, frame) -> int: + file_name = f"/{self.cnt:03d}.pickle" + full_path = self.root_dir + file_name + + self.cnt += 1 + + if os.path.isfile(full_path): + raise Exception(f"file {full_path} exists") + + # Convert to raw message if frame has a raw_msg attribute + if hasattr(frame, "raw_msg"): + frame = frame.raw_msg + + with open(full_path, "wb") as f: + pickle.dump(frame, f) + + return self.cnt From 792582c647d61408bddf31f2e3bae2d4df242d38 Mon Sep 17 00:00:00 2001 From: lesh Date: Thu, 29 May 2025 16:34:37 +0300 Subject: [PATCH 42/72] mapping test --- dimos/robot/unitree_webrtc/type/test_map.py | 26 +++++++++++---------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 180d473eb7..88c394236c 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -1,9 +1,10 @@ import pytest + +from dimos.robot.unitree_webrtc.testing.helpers import show3d, show3d_stream from dimos.robot.unitree_webrtc.testing.mock import Mock -from dimos.robot.unitree_webrtc.testing.helpers import show3d_stream, show3d from dimos.robot.unitree_webrtc.type.lidar import LidarMessage +from dimos.robot.unitree_webrtc.type.map import Map, splice_sphere from dimos.utils.reactive import backpressure -from dimos.robot.unitree_webrtc.type.map import splice_sphere, Map from dimos.utils.testing import SensorReplay @@ -43,19 +44,20 @@ def test_robot_vis(): def test_robot_mapping(): lidar_stream = SensorReplay("office_lidar", autocast=lambda x: LidarMessage.from_msg(x)) map = Map(voxel_size=0.5) - map.consume(lidar_stream.stream(rate_hz=100.0)).subscribe(lambda x: ...) + map.consume(lidar_stream.stream(rate_hz=100.0)).run() costmap = map.costmap - shape = costmap.grid.shape - assert shape[0] > 150 - assert shape[1] > 150 + assert costmap.grid.shape == (404, 276) - assert costmap.unknown_percent > 80 - assert costmap.unknown_percent < 90 + assert 70 <= costmap.unknown_percent <= 80, ( + f"Unknown percent {costmap.unknown_percent} is not within the range 70-80" + ) - assert costmap.free_percent > 5 - assert costmap.free_percent < 10 + assert 5 < costmap.free_percent < 10, ( + f"Free percent {costmap.free_percent} is not within the range 5-10" + ) - assert costmap.occupied_percent > 8 - assert costmap.occupied_percent < 15 + assert 8 < costmap.occupied_percent < 15, ( + f"Occupied percent {costmap.occupied_percent} is not within the range 8-15" + ) From 0f64117b68701ae55c74117d0c0349d10c306753 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 00:11:42 +0300 Subject: [PATCH 43/72] webrtc aio loop fix --- dimos/robot/unitree_webrtc/connection.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/dimos/robot/unitree_webrtc/connection.py b/dimos/robot/unitree_webrtc/connection.py index 7b9595b696..c873a79792 100644 --- a/dimos/robot/unitree_webrtc/connection.py +++ b/dimos/robot/unitree_webrtc/connection.py @@ -61,10 +61,14 @@ def start_background_loop(): self.connection_ready.wait() def move(self, vector: Vector): - self.conn.datachannel.pub_sub.publish_without_callback( - RTC_TOPIC["WIRELESS_CONTROLLER"], - data={"lx": vector.x, "ly": vector.y, "rx": vector.z, "ry": 0}, - ) + async def async_move(): + self.conn.datachannel.pub_sub.publish_without_callback( + RTC_TOPIC["WIRELESS_CONTROLLER"], + data={"lx": vector.x, "ly": vector.y, "rx": vector.z, "ry": 0}, + ) + + future = asyncio.run_coroutine_threadsafe(async_move(), self.loop) + return future.result() # Generic conversion of unitree subscription to Subject (used for all subs) def unitree_sub_stream(self, topic_name: str): From 4140cf578098eb4187f482e659cd64f21dbe1eab Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 11:48:13 +0300 Subject: [PATCH 44/72] testing if pre-commit actions work --- dimos/web/dimos_interface/tsconfig.json | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 4bf29f39d2..89d1495534 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -12,6 +12,8 @@ "node" ] }, + + "include": [ "src/**/*.ts", "src/**/*.js", From 9cb74e38b4185ec458d19feec135dbd9c4f5c5ef Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Fri, 30 May 2025 08:48:58 +0000 Subject: [PATCH 45/72] CI code cleanup --- dimos/web/dimos_interface/tsconfig.json | 2 -- 1 file changed, 2 deletions(-) diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 89d1495534..4bf29f39d2 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -12,8 +12,6 @@ "node" ] }, - - "include": [ "src/**/*.ts", "src/**/*.js", From e79c1d902445fd28869d2d9ba01147e8c42073a4 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 11:49:45 +0300 Subject: [PATCH 46/72] verifying subsequent run --- dimos/web/dimos_interface/tsconfig.json | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 89d1495534..4c452c7e00 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -19,6 +19,8 @@ "src/**/*.js", "src/**/*.svelte" ], + + "references": [ { "path": "./tsconfig.node.json" From bf76df11d8247d572610e63453b09ef822eb9872 Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Fri, 30 May 2025 08:51:19 +0000 Subject: [PATCH 47/72] CI code cleanup --- dimos/web/dimos_interface/tsconfig.json | 2 -- 1 file changed, 2 deletions(-) diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 27ffaec6be..4bf29f39d2 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -17,8 +17,6 @@ "src/**/*.js", "src/**/*.svelte" ], - - "references": [ { "path": "./tsconfig.node.json" From 39a7798a96608d61cf5842c4f74d5b44a7c8dc2e Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 15:09:54 +0300 Subject: [PATCH 48/72] local planner checkpoint --- dimos/robot/global_planner/planner.py | 1 + dimos/robot/local_planner/__init__.py | 7 - dimos/robot/local_planner/local_planner.py | 1279 +--------------- dimos/robot/local_planner/simple.py | 102 ++ dimos/robot/local_planner/test_simple.py | 191 +++ dimos/robot/local_planner/vfh/__init__.py | 7 + .../robot/local_planner/vfh/local_planner.py | 1297 +++++++++++++++++ .../{ => vfh}/vfh_local_planner.py | 40 +- dimos/robot/unitree_webrtc/type/odometry.py | 23 +- dimos/robot/unitree_webrtc/type/timeseries.py | 6 +- dimos/robot/unitree_webrtc/unitree_go2.py | 40 +- dimos/types/vector.py | 4 - dimos/web/dimos_interface/tsconfig.json | 2 - dimos/web/websocket_vis/server.py | 2 +- tests/run_webrtc.py | 48 +- 15 files changed, 1715 insertions(+), 1334 deletions(-) delete mode 100644 dimos/robot/local_planner/__init__.py create mode 100644 dimos/robot/local_planner/simple.py create mode 100644 dimos/robot/local_planner/test_simple.py create mode 100644 dimos/robot/local_planner/vfh/__init__.py create mode 100644 dimos/robot/local_planner/vfh/local_planner.py rename dimos/robot/local_planner/{ => vfh}/vfh_local_planner.py (93%) diff --git a/dimos/robot/global_planner/planner.py b/dimos/robot/global_planner/planner.py index f4b5d17bc6..7540b54182 100644 --- a/dimos/robot/global_planner/planner.py +++ b/dimos/robot/global_planner/planner.py @@ -44,6 +44,7 @@ def set_goal( if not path: logger.warning("No path found to the goal.") return False + print("pathing success", path) return self.set_local_nav(path, stop_event=stop_event, goal_theta=goal_theta) diff --git a/dimos/robot/local_planner/__init__.py b/dimos/robot/local_planner/__init__.py deleted file mode 100644 index 472b58dcd2..0000000000 --- a/dimos/robot/local_planner/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from dimos.robot.local_planner.local_planner import ( - BaseLocalPlanner, - navigate_to_goal_local, - navigate_path_local, -) - -from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner diff --git a/dimos/robot/local_planner/local_planner.py b/dimos/robot/local_planner/local_planner.py index d7af8a80d3..7e1ae601b3 100644 --- a/dimos/robot/local_planner/local_planner.py +++ b/dimos/robot/local_planner/local_planner.py @@ -1,1280 +1,15 @@ -#!/usr/bin/env python3 +from abc import abstractmethod +from typing import Optional -import math -import numpy as np -from typing import Dict, Tuple, Optional, Callable -from abc import ABC, abstractmethod -import cv2 from reactivex import Observable -from reactivex.subject import Subject -import threading -import time -import logging -from collections import deque -from dimos.utils.logging_config import setup_logger -from dimos.utils.ros_utils import normalize_angle, distance_angle_to_goal_xy -from dimos.types.vector import VectorLike, Vector, to_tuple -from dimos.types.path import Path -from nav_msgs.msg import OccupancyGrid +from dimos.types.vector import Vector, VectorLike +from dimos.web.websocket_vis.helpers import Visualizable -logger = setup_logger("dimos.robot.unitree.local_planner", level=logging.DEBUG) - - -class BaseLocalPlanner(ABC): - """ - Abstract base class for local planners that handle obstacle avoidance and path following. - - This class defines the common interface and shared functionality that all local planners - must implement, regardless of the specific algorithm used. - """ - - def __init__( - self, - get_costmap: Callable[[], Optional[OccupancyGrid]], - transform: object, - move_vel_control: Callable[[float, float, float], None], - safety_threshold: float = 0.5, - max_linear_vel: float = 0.8, - max_angular_vel: float = 1.0, - lookahead_distance: float = 1.0, - goal_tolerance: float = 0.75, - angle_tolerance: float = 0.15, - robot_width: float = 0.5, - robot_length: float = 0.7, - visualization_size: int = 400, - control_frequency: float = 10.0, - safe_goal_distance: float = 1.5, - ): # Control frequency in Hz - """ - Initialize the base local planner. - - Args: - get_costmap: Function to get the latest local costmap - transform: Object with transform methods (transform_point, transform_rot, etc.) - move_vel_control: Function to send velocity commands - safety_threshold: Distance to maintain from obstacles (meters) - max_linear_vel: Maximum linear velocity (m/s) - max_angular_vel: Maximum angular velocity (rad/s) - lookahead_distance: Lookahead distance for path following (meters) - goal_tolerance: Distance at which the goal is considered reached (meters) - angle_tolerance: Angle at which the goal orientation is considered reached (radians) - robot_width: Width of the robot for visualization (meters) - robot_length: Length of the robot for visualization (meters) - visualization_size: Size of the visualization image in pixels - control_frequency: Frequency at which the planner is called (Hz) - safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) - """ - # Store callables for robot interactions - self.get_costmap = get_costmap - self.transform = transform - self.move_vel_control = move_vel_control - - # Store parameters - self.safety_threshold = safety_threshold - self.max_linear_vel = max_linear_vel - self.max_angular_vel = max_angular_vel - self.lookahead_distance = lookahead_distance - self.goal_tolerance = goal_tolerance - self.angle_tolerance = angle_tolerance - self.robot_width = robot_width - self.robot_length = robot_length - self.visualization_size = visualization_size - self.control_frequency = control_frequency - self.control_period = 1.0 / control_frequency # Period in seconds - self.safe_goal_distance = safe_goal_distance # Distance to ignore obstacles at goal - self.ignore_obstacles = False # Flag for derived classes to check - - # Goal and Waypoint Tracking - self.goal_xy: Optional[Tuple[float, float]] = None # Current target for planning - self.goal_theta: Optional[float] = None # Goal orientation in odom frame - self.position_reached: bool = False # Flag indicating if position goal is reached - self.waypoints: Optional[Path] = None # Full path if following waypoints - self.waypoints_in_odom: Optional[Path] = None # Full path in odom frame - self.waypoint_frame: Optional[str] = None # Frame of the waypoints - self.current_waypoint_index: int = 0 # Index of the next waypoint to reach - self.final_goal_reached: bool = False # Flag indicating if the final waypoint is reached - - # Stuck detection - self.stuck_detection_window_seconds = 8.0 # Time window for stuck detection (seconds) - self.position_history_size = int(self.stuck_detection_window_seconds * control_frequency) - self.position_history = deque( - maxlen=self.position_history_size - ) # History of recent positions - self.stuck_distance_threshold = 0.1 # Distance threshold for stuck detection (meters) - self.unstuck_distance_threshold = 0.5 # Distance threshold for unstuck detection (meters) - self.stuck_time_threshold = 4.0 # Time threshold for stuck detection (seconds) - self.is_recovery_active = False # Whether recovery behavior is active - self.recovery_start_time = 0.0 # When recovery behavior started - self.recovery_duration = 8.0 # How long to run recovery before giving up (seconds) - self.last_update_time = time.time() # Last time position was updated - self.navigation_failed = False # Flag indicating if navigation should be terminated - - def reset(self): - """ - Reset all navigation and state tracking variables. - Should be called whenever a new goal is set. - """ - # Reset stuck detection state - self.position_history.clear() - self.is_recovery_active = False - self.recovery_start_time = 0.0 - self.last_update_time = time.time() - - # Reset navigation state flags - self.navigation_failed = False - self.position_reached = False - self.final_goal_reached = False - self.ignore_obstacles = False - - logger.info("Local planner state has been reset") - - def set_goal( - self, goal_xy: VectorLike, frame: str = "odom", goal_theta: Optional[float] = None - ): - """Set a single goal position, converting to odom frame if necessary. - This clears any existing waypoints being followed. - - Args: - goal_xy: The goal position to set. - frame: The frame of the goal position. - goal_theta: Optional goal orientation in radians (in the specified frame) - """ - # Reset all state variables - self.reset() - - # Clear waypoint following state - self.waypoints = None - self.current_waypoint_index = 0 - self.goal_xy = None # Clear previous goal - self.goal_theta = None # Clear previous goal orientation - - target_goal_xy: Optional[Tuple[float, float]] = None - - target_goal_xy = self.transform.transform_point( - goal_xy, source_frame=frame, target_frame="odom" - ).to_tuple() - - logger.info( - f"Goal set directly in odom frame: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" - ) - - # Check if goal is valid (in bounds and not colliding) - if not self.is_goal_in_costmap_bounds(target_goal_xy) or self.check_goal_collision( - target_goal_xy - ): - logger.warning( - "Goal is in collision or out of bounds. Adjusting goal to valid position." - ) - self.goal_xy = self.adjust_goal_to_valid_position(target_goal_xy) - else: - self.goal_xy = target_goal_xy # Set the adjusted or original valid goal - - # Set goal orientation if provided - if goal_theta is not None: - transformed_rot = self.transform.transform_rot( - Vector(0.0, 0.0, goal_theta), source_frame=frame, target_frame="odom" - ) - self.goal_theta = transformed_rot[2] - - def set_goal_waypoints( - self, waypoints: Path, frame: str = "map", goal_theta: Optional[float] = None - ): - """Sets a path of waypoints for the robot to follow. - - Args: - waypoints: A list of waypoints to follow. Each waypoint is a tuple of (x, y) coordinates in odom frame. - frame: The frame of the waypoints. - goal_theta: Optional final orientation in radians (in the specified frame) - """ - # Reset all state variables - self.reset() - - if not isinstance(waypoints, Path) or len(waypoints) == 0: - logger.warning("Invalid or empty path provided to set_goal_waypoints. Ignoring.") - self.waypoints = None - self.waypoint_frame = None - self.goal_xy = None - self.goal_theta = None - self.current_waypoint_index = 0 - return - - logger.info(f"Setting goal waypoints with {len(waypoints)} points.") - self.waypoints = waypoints - self.waypoint_frame = frame - self.current_waypoint_index = 0 - - # Transform waypoints to odom frame - self.waypoints_in_odom = self.transform.transform_path( - self.waypoints, source_frame=frame, target_frame="odom" - ) - - # Set the initial target to the first waypoint, adjusting if necessary - first_waypoint = self.waypoints_in_odom[0] - if not self.is_goal_in_costmap_bounds(first_waypoint) or self.check_goal_collision( - first_waypoint - ): - logger.warning("First waypoint is invalid. Adjusting...") - self.goal_xy = self.adjust_goal_to_valid_position(first_waypoint) - else: - self.goal_xy = to_tuple(first_waypoint) # Initial target - - # Set goal orientation if provided - if goal_theta is not None: - transformed_rot = self.transform.transform_rot( - Vector(0.0, 0.0, goal_theta), source_frame=frame, target_frame="odom" - ) - self.goal_theta = transformed_rot[2] - - def _get_robot_pose(self) -> Tuple[Tuple[float, float], float]: - """ - Get the current robot position and orientation. - - Returns: - Tuple containing: - - position as (x, y) tuple - - orientation (theta) in radians - """ - [pos, rot] = self.transform.transform_euler("base_link", "odom") - return (pos[0], pos[1]), rot[2] - - def _get_final_goal_position(self) -> Optional[Tuple[float, float]]: - """ - Get the final goal position (either last waypoint or direct goal). - - Returns: - Tuple (x, y) of the final goal, or None if no goal is set - """ - if self.waypoints_in_odom is not None and len(self.waypoints_in_odom) > 0: - return to_tuple(self.waypoints_in_odom[-1]) - elif self.goal_xy is not None: - return self.goal_xy - return None - - def _distance_to_position(self, target_position: Tuple[float, float]) -> float: - """ - Calculate distance from the robot to a target position. - - Args: - target_position: Target (x, y) position - - Returns: - Distance in meters - """ - robot_pos, _ = self._get_robot_pose() - return np.linalg.norm( - [target_position[0] - robot_pos[0], target_position[1] - robot_pos[1]] - ) - - def plan(self) -> Dict[str, float]: - """ - Main planning method that computes velocity commands. - This includes common planning logic like waypoint following, - with algorithm-specific calculations delegated to subclasses. - - Returns: - Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys - """ - # If goal orientation is specified, rotate to match it - if ( - self.position_reached - and self.goal_theta is not None - and not self._is_goal_orientation_reached() - ): - logger.info("Position goal reached. Rotating to target orientation.") - return self._rotate_to_goal_orientation() - - # Check if the robot is stuck and handle accordingly - if self.check_if_stuck() and not self.position_reached: - # Check if we're stuck but close to our goal - final_goal_pos = self._get_final_goal_position() - - # If we have a goal position, check distance to it - if final_goal_pos is not None: - distance_to_goal = self._distance_to_position(final_goal_pos) - - # If we're stuck but within 2x safe_goal_distance of the goal, consider it a success - if distance_to_goal < 2.0 * self.safe_goal_distance: - logger.info( - f"Robot is stuck but within {distance_to_goal:.2f}m of goal (< {2.0 * self.safe_goal_distance:.2f}m). Considering navigation successful." - ) - self.position_reached = True - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Otherwise, execute normal recovery behavior - logger.warning("Robot is stuck - executing recovery behavior") - return self.execute_recovery_behavior() - - # Reset obstacle ignore flag - self.ignore_obstacles = False - - # --- Waypoint Following Mode --- - if self.waypoints is not None: - if self.final_goal_reached: - logger.info("Final waypoint reached. Stopping.") - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Get current robot pose - robot_pos, robot_theta = self._get_robot_pose() - robot_pos_np = np.array(robot_pos) - - # Check if close to final waypoint - if self.waypoints_in_odom is not None and len(self.waypoints_in_odom) > 0: - final_waypoint = self.waypoints_in_odom[-1] - dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) - - # If we're close to the final waypoint, adjust it and ignore obstacles - if dist_to_final < self.safe_goal_distance: - final_wp_tuple = to_tuple(final_waypoint) - adjusted_goal = self.adjust_goal_to_valid_position(final_wp_tuple) - # Create a new Path with the adjusted final waypoint - new_waypoints = self.waypoints_in_odom[:-1] # Get all but the last waypoint - new_waypoints.append(adjusted_goal) # Append the adjusted goal - self.waypoints_in_odom = new_waypoints - self.ignore_obstacles = True - logger.debug("Within safe distance of final waypoint. Ignoring obstacles.") - - # Update the target goal based on waypoint progression - just_reached_final = self._update_waypoint_target(robot_pos_np) - - # If the helper indicates the final goal was just reached, stop immediately - if just_reached_final: - return {"x_vel": 0.0, "angular_vel": 0.0} - - # --- Single Goal or Current Waypoint Target Set --- - if self.goal_xy is None: - # If no goal is set (e.g., empty path or rejected goal), stop. - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Get necessary data for planning - costmap = self.get_costmap() - if costmap is None: - logger.warning("Local costmap is None. Cannot plan.") - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Check if close to single goal mode goal - if self.waypoints is None: - # Get distance to goal - goal_distance = self._distance_to_position(self.goal_xy) - - # If within safe distance of goal, adjust it and ignore obstacles - if goal_distance < self.safe_goal_distance: - self.goal_xy = self.adjust_goal_to_valid_position(self.goal_xy) - self.ignore_obstacles = True - logger.debug("Within safe distance of goal. Ignoring obstacles.") - - # First check position - if goal_distance < self.goal_tolerance or self.position_reached: - self.position_reached = True - - else: - self.position_reached = False - - # Call the algorithm-specific planning implementation - return self._compute_velocity_commands() - - @abstractmethod - def _compute_velocity_commands(self) -> Dict[str, float]: - """ - Algorithm-specific method to compute velocity commands. - Must be implemented by derived classes. - - Returns: - Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys - """ - pass - - def _rotate_to_goal_orientation(self) -> Dict[str, float]: - """Compute velocity commands to rotate to the goal orientation. - - Returns: - Dict[str, float]: Velocity commands with zero linear velocity - """ - # Get current robot orientation - _, robot_theta = self._get_robot_pose() - - # Calculate the angle difference - angle_diff = normalize_angle(self.goal_theta - robot_theta) - - # Determine rotation direction and speed - if abs(angle_diff) < self.angle_tolerance: - # Already at correct orientation - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Calculate rotation speed - proportional to the angle difference - # but capped at max_angular_vel - direction = 1.0 if angle_diff > 0 else -1.0 - angular_vel = direction * min(abs(angle_diff) * 2.0, self.max_angular_vel) - - # logger.debug(f"Rotating to goal orientation: angle_diff={angle_diff:.4f}, angular_vel={angular_vel:.4f}") - return {"x_vel": 0.0, "angular_vel": angular_vel} - - def _is_goal_orientation_reached(self) -> bool: - """Check if the current robot orientation matches the goal orientation. - - Returns: - bool: True if orientation is reached or no orientation goal is set - """ - if self.goal_theta is None: - return True # No orientation goal set - - # Get current robot orientation - _, robot_theta = self._get_robot_pose() - - # Calculate the angle difference and normalize - angle_diff = abs(normalize_angle(self.goal_theta - robot_theta)) - - logger.debug( - f"Orientation error: {angle_diff:.4f} rad, tolerance: {self.angle_tolerance:.4f} rad" - ) - return angle_diff <= self.angle_tolerance - - def _update_waypoint_target(self, robot_pos_np: np.ndarray) -> bool: - """Helper function to manage waypoint progression and update the target goal. - - Args: - robot_pos_np: Current robot position as a numpy array [x, y]. - - Returns: - bool: True if the final waypoint has just been reached, False otherwise. - """ - if self.waypoints is None or len(self.waypoints) == 0: - return False # Not in waypoint mode or empty path - - self.waypoints_in_odom = self.transform.transform_path( - self.waypoints, source_frame=self.waypoint_frame, target_frame="odom" - ) - - # Check if final goal is reached - final_waypoint = self.waypoints_in_odom[-1] - dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) - - if dist_to_final < self.goal_tolerance: - self.position_reached = True - self.goal_xy = to_tuple(final_waypoint) - - # If goal orientation is not specified or achieved, consider fully reached - if self.goal_theta is None or self._is_goal_orientation_reached(): - self.final_goal_reached = True - logger.info("Reached final waypoint with correct orientation.") - return True - else: - logger.info("Reached final waypoint position, rotating to target orientation.") - return False - - # Always find the lookahead point - lookahead_point = None - for i in range(self.current_waypoint_index, len(self.waypoints_in_odom)): - wp = self.waypoints_in_odom[i] - dist_to_wp = np.linalg.norm(robot_pos_np - wp) - if dist_to_wp >= self.lookahead_distance: - lookahead_point = wp - # Update current waypoint index to this point - self.current_waypoint_index = i - break - - # If no point is far enough, target the final waypoint - if lookahead_point is None: - lookahead_point = self.waypoints_in_odom[-1] - self.current_waypoint_index = len(self.waypoints_in_odom) - 1 - - # Set the lookahead point as the immediate target, adjusting if needed - if not self.is_goal_in_costmap_bounds(lookahead_point) or self.check_goal_collision( - lookahead_point - ): - logger.debug("Lookahead point is invalid. Adjusting...") - adjusted_lookahead = self.adjust_goal_to_valid_position(lookahead_point) - # Only update if adjustment didn't fail completely - if adjusted_lookahead is not None: - self.goal_xy = adjusted_lookahead - else: - self.goal_xy = to_tuple(lookahead_point) - - return False # Final goal not reached in this update cycle +class LocalPlanner(Visualizable): @abstractmethod - def update_visualization(self) -> np.ndarray: - """ - Generate visualization of the planning state. - Must be implemented by derived classes. - - Returns: - np.ndarray: Visualization image as numpy array - """ - pass - - def create_stream(self, frequency_hz: float = None) -> Observable: - """ - Create an Observable stream that emits the visualization image at a fixed frequency. - - Args: - frequency_hz: Optional frequency override (defaults to 1/4 of control_frequency if None) - - Returns: - Observable: Stream of visualization frames - """ - # Default to 1/4 of control frequency if not specified (to reduce CPU usage) - if frequency_hz is None: - frequency_hz = self.control_frequency / 4.0 - - subject = Subject() - sleep_time = 1.0 / frequency_hz - - def frame_emitter(): - while True: - try: - # Generate the frame using the updated method - frame = self.update_visualization() - subject.on_next(frame) - except Exception as e: - logger.error(f"Error in frame emitter thread: {e}") - # Optionally, emit an error frame or simply skip - # subject.on_error(e) # This would terminate the stream - time.sleep(sleep_time) - - emitter_thread = threading.Thread(target=frame_emitter, daemon=True) - emitter_thread.start() - logger.info(f"Started visualization frame emitter thread at {frequency_hz:.1f} Hz") - return subject + def set_goal(self, goal: VectorLike) -> bool: ... @abstractmethod - def check_collision(self, direction: float) -> bool: - """ - Check if there's a collision in the given direction. - Must be implemented by derived classes. - - Args: - direction: Direction to check for collision in radians - - Returns: - bool: True if collision detected, False otherwise - """ - pass - - def is_goal_reached(self) -> bool: - """Check if the final goal (single or last waypoint) is reached, including orientation.""" - if self.waypoints is not None: - # Waypoint mode: check if the final waypoint and orientation have been reached - return self.final_goal_reached - else: - # Single goal mode: check distance to the single goal and orientation - if self.goal_xy is None: - return False # No goal set - - return self.position_reached and self._is_goal_orientation_reached() - - def check_goal_collision(self, goal_xy: VectorLike) -> bool: - """Check if the current goal is in collision with obstacles in the costmap. - - Returns: - bool: True if goal is in collision, False if goal is safe or cannot be checked - """ - - costmap = self.get_costmap() - if costmap is None: - logger.warning("Cannot check collision: No costmap available") - return False - - # Check if the position is occupied - collision_threshold = 80 # Consider values above 80 as obstacles - - # Use Costmap's is_occupied method - return costmap.is_occupied(goal_xy, threshold=collision_threshold) - - def is_goal_in_costmap_bounds(self, goal_xy: VectorLike) -> bool: - """Check if the goal position is within the bounds of the costmap. - - Args: - goal_xy: Goal position (x, y) in odom frame - - Returns: - bool: True if the goal is within the costmap bounds, False otherwise - """ - costmap = self.get_costmap() - if costmap is None: - logger.warning("Cannot check bounds: No costmap available") - return False - - # Get goal position in grid coordinates - goal_point = costmap.world_to_grid(goal_xy) - goal_cell_x, goal_cell_y = goal_point.x, goal_point.y - - # Check if goal is within the costmap bounds - is_in_bounds = 0 <= goal_cell_x < costmap.width and 0 <= goal_cell_y < costmap.height - - if not is_in_bounds: - logger.warning(f"Goal ({goal_xy[0]:.2f}, {goal_xy[1]:.2f}) is outside costmap bounds") - - return is_in_bounds - - def adjust_goal_to_valid_position( - self, goal_xy: VectorLike, clearance: float = 0.5 - ) -> Tuple[float, float]: - """Find a valid (non-colliding) goal position by moving it towards the robot. - - Args: - goal_xy: Original goal position (x, y) in odom frame - clearance: Additional distance to move back from obstacles for better clearance (meters) - - Returns: - Tuple[float, float]: A valid goal position, or the original goal if already valid - """ - [pos, rot] = self.transform.transform_euler("base_link", "odom") - - robot_x, robot_y = pos[0], pos[1] - - # Original goal - goal_x, goal_y = to_tuple(goal_xy) - - if not self.check_goal_collision((goal_x, goal_y)): - return (goal_x, goal_y) - - # Calculate vector from goal to robot - dx = robot_x - goal_x - dy = robot_y - goal_y - distance = np.sqrt(dx * dx + dy * dy) - - if distance < 0.001: # Goal is at robot position - return to_tuple(goal_xy) - - # Normalize direction vector - dx /= distance - dy /= distance - - # Step size - step_size = 0.25 # meters - - # Move goal towards robot step by step - current_x, current_y = goal_x, goal_y - steps = 0 - max_steps = 50 # Safety limit - - # Variables to store the first valid position found - valid_found = False - valid_x, valid_y = None, None - - while steps < max_steps: - # Move towards robot - current_x += dx * step_size - current_y += dy * step_size - steps += 1 - - # Check if we've reached or passed the robot - new_distance = np.sqrt((current_x - robot_x) ** 2 + (current_y - robot_y) ** 2) - if new_distance < step_size: - # We've reached the robot without finding a valid point - # Move back one step from robot to avoid self-collision - current_x = robot_x - dx * step_size - current_y = robot_y - dy * step_size - break - - # Check if this position is valid - if not self.check_goal_collision( - (current_x, current_y) - ) and self.is_goal_in_costmap_bounds((current_x, current_y)): - # Store the first valid position - if not valid_found: - valid_found = True - valid_x, valid_y = current_x, current_y - - # If clearance is requested, continue searching for a better position - if clearance > 0: - continue - - # Calculate position with additional clearance - if clearance > 0: - # Calculate clearance position - clearance_x = current_x + dx * clearance - clearance_y = current_y + dy * clearance - - logger.info( - f"Checking clearance position at ({clearance_x:.2f}, {clearance_y:.2f})" - ) - - # Check if the clearance position is also valid - if not self.check_goal_collision( - (clearance_x, clearance_y) - ) and self.is_goal_in_costmap_bounds((clearance_x, clearance_y)): - logger.info( - f"Found valid goal with clearance at ({clearance_x:.2f}, {clearance_y:.2f})" - ) - return (clearance_x, clearance_y) - - # Return the valid position without clearance - logger.info(f"Found valid goal at ({current_x:.2f}, {current_y:.2f})") - return (current_x, current_y) - - # If we found a valid position earlier but couldn't add clearance - if valid_found: - logger.info(f"Using valid goal found at ({valid_x:.2f}, {valid_y:.2f})") - return (valid_x, valid_y) - - logger.warning( - f"Could not find valid goal after {steps} steps, using closest point to robot" - ) - return (current_x, current_y) - - def check_if_stuck(self) -> bool: - """ - Check if the robot is stuck by analyzing movement history. - - Returns: - bool: True if the robot is determined to be stuck, False otherwise - """ - # Get current position and time - current_time = time.time() - - # Get current robot position - [pos, _] = self.transform.transform_euler("base_link", "odom") - current_position = (pos[0], pos[1], current_time) - - # Add current position to history (newest is appended at the end) - self.position_history.append(current_position) - - # Need enough history to make a determination - min_history_size = self.stuck_detection_window_seconds * self.control_frequency - if len(self.position_history) < min_history_size: - return False - - # Find positions within our detection window (positions are already in order from oldest to newest) - window_start_time = current_time - self.stuck_detection_window_seconds - window_positions = [] - - # Collect positions within the window (newest entries will be at the end) - for pos_x, pos_y, timestamp in self.position_history: - if timestamp >= window_start_time: - window_positions.append((pos_x, pos_y, timestamp)) - - # Need at least a few positions in the window - if len(window_positions) < 3: - return False - - # Ensure correct order: oldest to newest - window_positions.sort(key=lambda p: p[2]) - - # Get the oldest and newest positions in the window - oldest_x, oldest_y, oldest_time = window_positions[0] - newest_x, newest_y, newest_time = window_positions[-1] - - # Calculate time range in the window (should always be positive) - time_range = newest_time - oldest_time - - # Calculate displacement from oldest to newest position - displacement = np.sqrt((newest_x - oldest_x) ** 2 + (newest_y - oldest_y) ** 2) - - # Check if we're stuck - moved less than threshold over minimum time - # Only consider it if the time range makes sense (positive and sufficient) - is_currently_stuck = ( - time_range >= self.stuck_time_threshold - and time_range <= self.stuck_detection_window_seconds - and displacement < self.stuck_distance_threshold - ) - - if is_currently_stuck: - logger.warning( - f"Robot appears to be stuck! Displacement {displacement:.3f}m over {time_range:.1f}s" - ) - - # Don't trigger recovery if it's already active - if not self.is_recovery_active: - self.is_recovery_active = True - self.recovery_start_time = current_time - return True - - # Check if we've been trying to recover for too long - elif current_time - self.recovery_start_time > self.recovery_duration: - logger.error( - f"Recovery behavior has been active for {self.recovery_duration}s without success" - ) - # Reset recovery state - maybe a different behavior will work - self.is_recovery_active = False - self.recovery_start_time = current_time - - # If we've moved enough, we're not stuck anymore - elif self.is_recovery_active and displacement > self.unstuck_distance_threshold: - logger.info(f"Robot has escaped from stuck state (moved {displacement:.3f}m)") - self.is_recovery_active = False - - return self.is_recovery_active - - def execute_recovery_behavior(self) -> Dict[str, float]: - """ - Execute a recovery behavior when the robot is stuck. - - Returns: - Dict[str, float]: Velocity commands for the recovery behavior - """ - # Calculate how long we've been in recovery - recovery_time = time.time() - self.recovery_start_time - - # Calculate recovery phases based on control frequency - backup_phase_time = 3.0 # seconds - rotate_phase_time = 2.0 # seconds - - # Simple recovery behavior state machine - if recovery_time < backup_phase_time: - # First try backing up - logger.info("Recovery: backing up") - return {"x_vel": -0.2, "angular_vel": 0.0} - elif recovery_time < backup_phase_time + rotate_phase_time: - # Then try rotating - logger.info("Recovery: rotating to find new path") - rotation_direction = 1.0 if np.random.random() > 0.5 else -1.0 - return {"x_vel": 0.0, "angular_vel": rotation_direction * self.max_angular_vel * 0.7} - else: - # If we're still stuck after backup and rotation, terminate navigation - logger.error("Recovery failed after backup and rotation. Navigation terminated.") - # Set a flag to indicate navigation should terminate - self.navigation_failed = True - # Stop the robot - return {"x_vel": 0.0, "angular_vel": 0.0} - - -def navigate_to_goal_local( - robot, - goal_xy_robot: Tuple[float, float], - goal_theta: Optional[float] = None, - distance: float = 0.0, - timeout: float = 60.0, - stop_event: Optional[threading.Event] = None, -) -> bool: - """ - Navigates the robot to a goal specified in the robot's local frame - using the local planner. - - Args: - robot: Robot instance to control - goal_xy_robot: Tuple (x, y) representing the goal position relative - to the robot's current position and orientation. - distance: Desired distance to maintain from the goal in meters. - If non-zero, the robot will stop this far away from the goal. - timeout: Maximum time (in seconds) allowed to reach the goal. - stop_event: Optional threading.Event to signal when navigation should stop - - Returns: - bool: True if the goal was reached within the timeout, False otherwise. - """ - logger.info( - f"Starting navigation to local goal {goal_xy_robot} with distance {distance}m and timeout {timeout}s." - ) - - goal_x, goal_y = goal_xy_robot - - # Calculate goal orientation to face the target - if goal_theta is None: - goal_theta = np.arctan2(goal_y, goal_x) - - # If distance is non-zero, adjust the goal to stop at the desired distance - if distance > 0: - # Calculate magnitude of the goal vector - goal_distance = np.sqrt(goal_x**2 + goal_y**2) - - # Only adjust if goal is further than the desired distance - if goal_distance > distance: - goal_x, goal_y = distance_angle_to_goal_xy(goal_distance - distance, goal_theta) - - # Set the goal in the robot's frame with orientation to face the original target - robot.local_planner.set_goal((goal_x, goal_y), frame="base_link", goal_theta=goal_theta) - - # Get control period from robot's local planner for consistent timing - control_period = 1.0 / robot.local_planner.control_frequency - - start_time = time.time() - goal_reached = False - - try: - while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): - # Check if goal has been reached - if robot.local_planner.is_goal_reached(): - logger.info("Goal reached successfully.") - goal_reached = True - break - - # Check if navigation failed flag is set - if robot.local_planner.navigation_failed: - logger.error("Navigation aborted due to repeated recovery failures.") - goal_reached = False - break - - # Get planned velocity towards the goal - vel_command = robot.local_planner.plan() - x_vel = vel_command.get("x_vel", 0.0) - angular_vel = vel_command.get("angular_vel", 0.0) - - # Send velocity command - robot.local_planner.move_vel_control(x=x_vel, y=0, yaw=angular_vel) - - # Control loop frequency - use robot's control frequency - time.sleep(control_period) - - if not goal_reached: - logger.warning(f"Navigation timed out after {timeout} seconds before reaching goal.") - - except KeyboardInterrupt: - logger.info("Navigation to local goal interrupted by user.") - goal_reached = False # Consider interruption as failure - except Exception as e: - logger.error(f"Error during navigation to local goal: {e}") - goal_reached = False # Consider error as failure - finally: - logger.info("Stopping robot after navigation attempt.") - robot.local_planner.move_vel_control(0, 0, 0) # Stop the robot - - return goal_reached - - -def navigate_path_local( - robot, - path: Path, - timeout: float = 120.0, - goal_theta: Optional[float] = None, - stop_event: Optional[threading.Event] = None, -) -> bool: - """ - Navigates the robot along a path of waypoints using the waypoint following capability - of the local planner. - - Args: - robot: Robot instance to control - path: Path object containing waypoints in odom/map frame - timeout: Maximum time (in seconds) allowed to follow the complete path - goal_theta: Optional final orientation in radians - stop_event: Optional threading.Event to signal when navigation should stop - - Returns: - bool: True if the entire path was successfully followed, False otherwise - """ - logger.info( - f"Starting navigation along path with {len(path)} waypoints and timeout {timeout}s." - ) - - # Set the path in the local planner - robot.local_planner.set_goal_waypoints(path, goal_theta=goal_theta) - - # Get control period from robot's local planner for consistent timing - control_period = 1.0 / robot.local_planner.control_frequency - - start_time = time.time() - path_completed = False - - try: - while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): - # Check if the entire path has been traversed - if robot.local_planner.is_goal_reached(): - logger.info("Path traversed successfully.") - path_completed = True - break - - # Check if navigation failed flag is set - if robot.local_planner.navigation_failed: - logger.error("Navigation aborted due to repeated recovery failures.") - path_completed = False - break - - # Get planned velocity towards the current waypoint target - vel_command = robot.local_planner.plan() - x_vel = vel_command.get("x_vel", 0.0) - angular_vel = vel_command.get("angular_vel", 0.0) - - # Send velocity command - robot.local_planner.move_vel_control(x=x_vel, y=0, yaw=angular_vel) - - # Control loop frequency - use robot's control frequency - time.sleep(control_period) - - if not path_completed: - logger.warning( - f"Path following timed out after {timeout} seconds before completing the path." - ) - - except KeyboardInterrupt: - logger.info("Path navigation interrupted by user.") - path_completed = False - except Exception as e: - logger.error(f"Error during path navigation: {e}") - path_completed = False - finally: - logger.info("Stopping robot after path navigation attempt.") - robot.local_planner.move_vel_control(0, 0, 0) # Stop the robot - - return path_completed - - -def visualize_local_planner_state( - occupancy_grid: np.ndarray, - grid_resolution: float, - grid_origin: Tuple[float, float, float], - robot_pose: Tuple[float, float, float], - visualization_size: int = 400, - robot_width: float = 0.5, - robot_length: float = 0.7, - map_size_meters: float = 10.0, - goal_xy: Optional[Tuple[float, float]] = None, - goal_theta: Optional[float] = None, - histogram: Optional[np.ndarray] = None, - selected_direction: Optional[float] = None, - waypoints: Optional["Path"] = None, - current_waypoint_index: Optional[int] = None, -) -> np.ndarray: - """Generate a bird's eye view visualization of the local costmap. - Optionally includes VFH histogram, selected direction, and waypoints path. - - Args: - occupancy_grid: 2D numpy array of the occupancy grid - grid_resolution: Resolution of the grid in meters/cell - grid_origin: Tuple (x, y, theta) of the grid origin in the odom frame - robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame - visualization_size: Size of the visualization image in pixels - robot_width: Width of the robot in meters - robot_length: Length of the robot in meters - map_size_meters: Size of the map to visualize in meters - goal_xy: Optional tuple (x, y) of the goal position in the odom frame - goal_theta: Optional goal orientation in radians (in odom frame) - histogram: Optional numpy array of the VFH histogram - selected_direction: Optional selected direction angle in radians - waypoints: Optional Path object containing waypoints to visualize - current_waypoint_index: Optional index of the current target waypoint - """ - - robot_x, robot_y, robot_theta = robot_pose - grid_origin_x, grid_origin_y, _ = grid_origin - vis_size = visualization_size - scale = vis_size / map_size_meters - - vis_img = np.ones((vis_size, vis_size, 3), dtype=np.uint8) * 255 - center_x = vis_size // 2 - center_y = vis_size // 2 - - grid_height, grid_width = occupancy_grid.shape - - # Calculate robot position relative to grid origin - robot_rel_x = robot_x - grid_origin_x - robot_rel_y = robot_y - grid_origin_y - robot_cell_x = int(robot_rel_x / grid_resolution) - robot_cell_y = int(robot_rel_y / grid_resolution) - - half_size_cells = int(map_size_meters / grid_resolution / 2) - - # Draw grid cells (using standard occupancy coloring) - for y in range( - max(0, robot_cell_y - half_size_cells), min(grid_height, robot_cell_y + half_size_cells) - ): - for x in range( - max(0, robot_cell_x - half_size_cells), min(grid_width, robot_cell_x + half_size_cells) - ): - cell_rel_x_meters = (x - robot_cell_x) * grid_resolution - cell_rel_y_meters = (y - robot_cell_y) * grid_resolution - - img_x = int(center_x + cell_rel_x_meters * scale) - img_y = int(center_y - cell_rel_y_meters * scale) # Flip y-axis - - if 0 <= img_x < vis_size and 0 <= img_y < vis_size: - cell_value = occupancy_grid[y, x] - if cell_value == -1: - color = (200, 200, 200) # Unknown (Light gray) - elif cell_value == 0: - color = (255, 255, 255) # Free (White) - else: # Occupied - # Scale darkness based on occupancy value (0-100) - darkness = 255 - int(155 * (cell_value / 100)) - 100 - color = (darkness, darkness, darkness) # Shades of gray/black - - cell_size_px = max(1, int(grid_resolution * scale)) - cv2.rectangle( - vis_img, - (img_x - cell_size_px // 2, img_y - cell_size_px // 2), - (img_x + cell_size_px // 2, img_y + cell_size_px // 2), - color, - -1, - ) - - # Draw waypoints path if provided - if waypoints is not None and len(waypoints) > 0: - try: - path_points = [] - for i, waypoint in enumerate(waypoints): - # Convert waypoint from odom frame to visualization frame - wp_x, wp_y = waypoint[0], waypoint[1] - wp_rel_x = wp_x - robot_x - wp_rel_y = wp_y - robot_y - - wp_img_x = int(center_x + wp_rel_x * scale) - wp_img_y = int(center_y - wp_rel_y * scale) # Flip y-axis - - if 0 <= wp_img_x < vis_size and 0 <= wp_img_y < vis_size: - path_points.append((wp_img_x, wp_img_y)) - - # Draw each waypoint as a small circle - cv2.circle(vis_img, (wp_img_x, wp_img_y), 3, (0, 128, 0), -1) # Dark green dots - - # Highlight current target waypoint - if current_waypoint_index is not None and i == current_waypoint_index: - cv2.circle(vis_img, (wp_img_x, wp_img_y), 6, (0, 0, 255), 2) # Red circle - - # Connect waypoints with lines to show the path - if len(path_points) > 1: - for i in range(len(path_points) - 1): - cv2.line( - vis_img, path_points[i], path_points[i + 1], (0, 200, 0), 1 - ) # Green line - except Exception as e: - logger.error(f"Error drawing waypoints: {e}") - - # Draw histogram - if histogram is not None: - num_bins = len(histogram) - # Find absolute maximum value (ignoring any negative debug values) - abs_histogram = np.abs(histogram) - max_hist_value = np.max(abs_histogram) if np.max(abs_histogram) > 0 else 1.0 - hist_scale = (vis_size / 2) * 0.8 # Scale histogram lines to 80% of half the viz size - - for i in range(num_bins): - # Angle relative to robot's forward direction - angle_relative_to_robot = (i / num_bins) * 2 * math.pi - math.pi - # Angle in the visualization frame (relative to image +X axis) - vis_angle = angle_relative_to_robot + robot_theta - - # Get the value and check if it's a special debug value (negative) - hist_val = histogram[i] - is_debug_value = hist_val < 0 - - # Use absolute value for line length - normalized_val = min(1.0, abs(hist_val) / max_hist_value) - line_length = normalized_val * hist_scale - - # Calculate endpoint using the visualization angle - end_x = int(center_x + line_length * math.cos(vis_angle)) - end_y = int(center_y - line_length * math.sin(vis_angle)) # Flipped Y - - # Color based on value and whether it's a debug value - if is_debug_value: - # Use green for debug values (minimum cost bin) - color = (0, 255, 0) # Green - line_width = 2 # Thicker line for emphasis - else: - # Regular coloring for normal values (blue to red gradient based on obstacle density) - blue = max(0, 255 - int(normalized_val * 255)) - red = min(255, int(normalized_val * 255)) - color = (blue, 0, red) # BGR format: obstacles are redder, clear areas are bluer - line_width = 1 - - cv2.line(vis_img, (center_x, center_y), (end_x, end_y), color, line_width) - - # Draw robot - robot_length_px = int(robot_length * scale) - robot_width_px = int(robot_width * scale) - robot_pts = np.array( - [ - [-robot_length_px / 2, -robot_width_px / 2], - [robot_length_px / 2, -robot_width_px / 2], - [robot_length_px / 2, robot_width_px / 2], - [-robot_length_px / 2, robot_width_px / 2], - ], - dtype=np.float32, - ) - rotation_matrix = np.array( - [ - [math.cos(robot_theta), -math.sin(robot_theta)], - [math.sin(robot_theta), math.cos(robot_theta)], - ] - ) - robot_pts = np.dot(robot_pts, rotation_matrix.T) - robot_pts[:, 0] += center_x - robot_pts[:, 1] = center_y - robot_pts[:, 1] # Flip y-axis - cv2.fillPoly( - vis_img, [robot_pts.reshape((-1, 1, 2)).astype(np.int32)], (0, 0, 255) - ) # Red robot - - # Draw robot direction line - front_x = int(center_x + (robot_length_px / 2) * math.cos(robot_theta)) - front_y = int(center_y - (robot_length_px / 2) * math.sin(robot_theta)) - cv2.line(vis_img, (center_x, center_y), (front_x, front_y), (255, 0, 0), 2) # Blue line - - # Draw selected direction - if selected_direction is not None: - # selected_direction is relative to robot frame - # Angle in the visualization frame (relative to image +X axis) - vis_angle_selected = selected_direction + robot_theta - - # Make slightly longer than max histogram line - sel_dir_line_length = (vis_size / 2) * 0.9 - - sel_end_x = int(center_x + sel_dir_line_length * math.cos(vis_angle_selected)) - sel_end_y = int(center_y - sel_dir_line_length * math.sin(vis_angle_selected)) # Flipped Y - - cv2.line( - vis_img, (center_x, center_y), (sel_end_x, sel_end_y), (0, 165, 255), 2 - ) # BGR for Orange - - # Draw goal - if goal_xy is not None: - goal_x, goal_y = goal_xy - goal_rel_x_map = goal_x - robot_x - goal_rel_y_map = goal_y - robot_y - goal_img_x = int(center_x + goal_rel_x_map * scale) - goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis - if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: - cv2.circle(vis_img, (goal_img_x, goal_img_y), 5, (0, 255, 0), -1) # Green circle - cv2.circle(vis_img, (goal_img_x, goal_img_y), 8, (0, 0, 0), 1) # Black outline - - # Draw goal orientation - if goal_theta is not None and goal_xy is not None: - # For waypoint mode, only draw orientation at the final waypoint - if waypoints is not None and len(waypoints) > 0: - # Use the final waypoint position - final_waypoint = waypoints[-1] - goal_x, goal_y = final_waypoint[0], final_waypoint[1] - else: - # Use the current goal position - goal_x, goal_y = goal_xy - - goal_rel_x_map = goal_x - robot_x - goal_rel_y_map = goal_y - robot_y - goal_img_x = int(center_x + goal_rel_x_map * scale) - goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis - - # Calculate goal orientation vector direction in visualization frame - # goal_theta is already in odom frame, need to adjust for visualization orientation - goal_dir_length = 30 # Length of direction indicator in pixels - goal_dir_end_x = int(goal_img_x + goal_dir_length * math.cos(goal_theta)) - goal_dir_end_y = int(goal_img_y - goal_dir_length * math.sin(goal_theta)) # Flip y-axis - - # Draw goal orientation arrow - if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: - cv2.arrowedLine( - vis_img, - (goal_img_x, goal_img_y), - (goal_dir_end_x, goal_dir_end_y), - (255, 0, 255), - 4, - ) # Magenta arrow - - # Add scale bar - scale_bar_length_px = int(1.0 * scale) - scale_bar_x = vis_size - scale_bar_length_px - 10 - scale_bar_y = vis_size - 20 - cv2.line( - vis_img, - (scale_bar_x, scale_bar_y), - (scale_bar_x + scale_bar_length_px, scale_bar_y), - (0, 0, 0), - 2, - ) - cv2.putText( - vis_img, "1m", (scale_bar_x, scale_bar_y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 0), 1 - ) - - # Add status info - status_text = [] - if waypoints is not None: - if current_waypoint_index is not None: - status_text.append(f"WP: {current_waypoint_index}/{len(waypoints)}") - else: - status_text.append(f"WPs: {len(waypoints)}") - - y_pos = 20 - for text in status_text: - cv2.putText(vis_img, text, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) - y_pos += 20 - - return vis_img + def get_move_stream(self, frequency: Optional[float]) -> Observable[Vector]: ... diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py new file mode 100644 index 0000000000..e9a920ecb3 --- /dev/null +++ b/dimos/robot/local_planner/simple.py @@ -0,0 +1,102 @@ +import math +from dataclasses import dataclass +from typing import Callable, Optional + +import reactivex as rx +from reactivex import operators as ops +from plum import dispatch + +from dimos.robot.local_planner.local_planner import LocalPlanner +from dimos.types.costmap import Costmap +from dimos.types.position import Position +from dimos.types.path import Path +from dimos.types.vector import Vector, VectorLike, to_vector +from dimos.utils.threadpool import get_scheduler +from dimos.utils.logging_config import setup_logger + +logger = setup_logger("dimos.robot.unitree.global_planner") + + +@dataclass +class SimplePlanner(LocalPlanner): + get_costmap: Callable[[], Costmap] + get_robot_pos: Callable[[], Position] + goal: Optional[Vector] = None + speed: float = 0.2 + + @dispatch + def set_goal(self, goal: Path, stop_event=None, goal_theta=None) -> bool: + self.goal = goal.last().to_2d() + logger.info(f"Setting goal: {self.goal}") + return True + + @dispatch + def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: + self.goal = to_vector(goal).to_2d() + logger.info(f"Setting goal: {self.goal}") + return True + + def _transform_to_robot_frame(self, global_vector: Vector, robot_position: Position) -> Vector: + """Transform a global coordinate vector to robot-relative coordinates. + + Args: + global_vector: Vector in global coordinates + robot_position: Robot's position and orientation + + Returns: + Vector in robot coordinates where X is forward/backward, Y is left/right + """ + # Get the robot's yaw angle (rotation around Z-axis) + robot_yaw = robot_position.rot.z + + # Create rotation matrix to transform from global to robot frame + cos_yaw = math.cos(-robot_yaw) # Negative because we're rotating coordinate frame + sin_yaw = math.sin(-robot_yaw) + + # Apply 2D rotation transformation + robot_x = global_vector.x * cos_yaw - global_vector.y * sin_yaw # Forward/backward + robot_y = global_vector.x * sin_yaw + global_vector.y * cos_yaw # Left/right + + return Vector(robot_x, robot_y, global_vector.z) + + def calc_move(self, direction: Vector) -> Vector: + """Calculate the movement vector based on the direction to the goal. + + Args: + direction: Direction vector towards the goal + + Returns: + Movement vector scaled by speed + """ + try: + # Normalize the direction vector and scale by speed + normalized_direction = direction.normalize() + move_vector = normalized_direction * self.speed + print("CALC MOVE", direction, normalized_direction, move_vector) + return move_vector + except Exception as e: + print("Error calculating move vector:", e) + + def spy(self, name: str): + def spyfun(x): + print(f"SPY {name}:", x) + return x + + return ops.map(spyfun) + + def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: + return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( + # do we have a goal? + ops.filter(lambda _: self.goal is not None), + # calculate direction vector + ops.map(lambda _: self.get_robot_pos().pos.to_2d() - self.goal), + ops.filter(lambda direction: direction.length() > 0.1), + # ops.map(self.calc_move), + self.spy("direction"), + ops.map( + lambda direction: self._transform_to_robot_frame( + direction.normalize() * self.speed, + self.get_robot_pos(), + ) + ), + ) diff --git a/dimos/robot/local_planner/test_simple.py b/dimos/robot/local_planner/test_simple.py new file mode 100644 index 0000000000..1883b0948f --- /dev/null +++ b/dimos/robot/local_planner/test_simple.py @@ -0,0 +1,191 @@ +import math +import time +from unittest.mock import Mock + +import pytest +import reactivex as rx +from reactivex import operators as ops + +from dimos.robot.local_planner.simple import SimplePlanner +from dimos.types.position import Position +from dimos.types.vector import Vector + + +class TestSimplePlanner: + """Test suite for SimplePlanner class.""" + + @pytest.fixture + def mock_get_costmap(self): + """Mock costmap getter function.""" + return Mock(return_value=None) + + @pytest.fixture + def mock_get_robot_pos(self): + """Mock robot position getter function.""" + return Mock(return_value=Position(Vector(0, 0, 0))) + + @pytest.fixture + def planner(self, mock_get_costmap, mock_get_robot_pos): + """Create a SimplePlanner instance with mocked dependencies.""" + return SimplePlanner(get_costmap=mock_get_costmap, get_robot_pos=mock_get_robot_pos) + + def test_initialization(self, planner, mock_get_costmap, mock_get_robot_pos): + """Test that SimplePlanner initializes correctly.""" + assert planner.get_costmap == mock_get_costmap + assert planner.get_robot_pos == mock_get_robot_pos + assert planner.goal is None + + def test_set_goal_with_vector(self, planner): + """Test setting goal with a Vector object.""" + goal = Vector(1, 2, 0) + result = planner.set_goal(goal) + + assert result is True + assert planner.goal == goal + + def test_get_move_stream_returns_observable(self, planner): + """Test that get_move_stream returns an Observable.""" + move_stream = planner.get_move_stream(frequency=10.0) + assert isinstance(move_stream, rx.Observable) + + def test_get_move_stream_with_no_goal(self, planner): + """Test that move stream doesn't emit when no goal is set.""" + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1), ops.timeout(0.5)).subscribe( + on_next=lambda x: emissions.append(x), + on_error=lambda e: None, # Ignore timeout errors + ) + + time.sleep(0.6) + assert len(emissions) == 0 + + def test_get_move_stream_no_rotation(self, planner, mock_get_robot_pos): + """Test movement with robot facing forward (no rotation).""" + # Robot at origin facing forward (no rotation), goal ahead + mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, 0)) + planner.set_goal(Vector(1, 0, 0)) # Goal directly ahead + + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) + + time.sleep(0.5) + + assert len(emissions) == 1 + move_command = emissions[0] + assert isinstance(move_command, Vector) + + # Should move forward (positive X in robot frame) + assert abs(move_command.x - 0.2) < 0.001 # Forward + assert abs(move_command.y) < 0.001 # No left/right movement + + def test_get_move_stream_with_rotation_90_degrees(self, planner, mock_get_robot_pos): + """Test movement with robot rotated 90 degrees (facing left in global frame).""" + # Robot facing left (90 degrees rotation), goal to the right in global frame + mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, math.pi / 2)) + planner.set_goal(Vector(1, 0, 0)) # Goal to the right in global frame + + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) + + time.sleep(0.5) + + assert len(emissions) == 1 + move_command = emissions[0] + + # Goal is to the right in global frame, but robot is facing left + # So in robot frame, this should be a right turn (negative Y) + assert abs(move_command.x) < 0.001 # No forward/backward + assert abs(move_command.y + 0.2) < 0.001 # Right turn (negative Y) + + def test_get_move_stream_with_rotation_180_degrees(self, planner, mock_get_robot_pos): + """Test movement with robot rotated 180 degrees (facing backward).""" + # Robot facing backward, goal behind in global frame (which is forward for robot) + mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, math.pi)) + planner.set_goal(Vector(-1, 0, 0)) # Goal behind in global frame + + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) + + time.sleep(0.5) + + assert len(emissions) == 1 + move_command = emissions[0] + + # Goal is behind in global frame, but robot is facing backward + # So in robot frame, this should be forward movement + assert abs(move_command.x - 0.2) < 0.001 # Forward + assert abs(move_command.y) < 0.001 # No left/right + + def test_get_move_stream_diagonal_movement(self, planner, mock_get_robot_pos): + """Test diagonal movement with no robot rotation.""" + # Robot at origin, goal at (1,1) - should move diagonally + mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, 0)) + planner.set_goal(Vector(1, 1, 0)) + + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) + + time.sleep(0.5) + + assert len(emissions) == 1 + move_command = emissions[0] + + # Should be normalized direction vector * 0.2 speed + # Direction from (0,0) to (1,1) normalized is approximately (0.707, 0.707) + # Multiplied by 0.2 gives approximately (0.141, 0.141) + assert abs(move_command.x - 0.14142136) < 0.001 + assert abs(move_command.y - 0.14142136) < 0.001 + + def test_transform_to_robot_frame_no_rotation(self, planner): + """Test coordinate transformation with no robot rotation.""" + robot_pos = Position(Vector(0, 0, 0), Vector(0, 0, 0)) # No rotation + global_vec = Vector(1, 0, 0) # Forward in global frame + + result = planner._transform_to_robot_frame(global_vec, robot_pos) + + # With no rotation, global and robot frames should be the same + assert abs(result.x - 1.0) < 0.001 + assert abs(result.y) < 0.001 + + def test_transform_to_robot_frame_90_degree_rotation(self, planner): + """Test coordinate transformation with 90-degree robot rotation.""" + robot_pos = Position(Vector(0, 0, 0), Vector(0, 0, math.pi / 2)) # 90 degrees + global_vec = Vector(1, 0, 0) # Forward in global frame + + result = planner._transform_to_robot_frame(global_vec, robot_pos) + + # Robot is facing left, so global forward becomes robot right (negative Y) + assert abs(result.x) < 0.001 # No forward/backward + assert abs(result.y + 1.0) < 0.001 # Right turn (negative Y) + + def test_goal_at_robot_position(self, planner, mock_get_robot_pos): + """Test behavior when goal is at current robot position.""" + position = Position(Vector(1, 1, 0), Vector(0, 0, 0)) + mock_get_robot_pos.return_value = position + planner.set_goal(Vector(1, 1, 0)) + + move_stream = planner.get_move_stream(frequency=10.0) + + emissions = [] + move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) + + time.sleep(0.3) + + assert len(emissions) == 1 + move_command = emissions[0] + + # When goal equals current position, difference is zero + # Normalizing zero vector should result in zero vector + # So movement should be zero + assert abs(move_command.x) < 0.001 + assert abs(move_command.y) < 0.001 diff --git a/dimos/robot/local_planner/vfh/__init__.py b/dimos/robot/local_planner/vfh/__init__.py new file mode 100644 index 0000000000..22bf61986c --- /dev/null +++ b/dimos/robot/local_planner/vfh/__init__.py @@ -0,0 +1,7 @@ +from dimos.robot.local_planner.vfh.local_planner import ( + BaseLocalPlanner, + navigate_to_goal_local, + navigate_path_local, +) + +from dimos.robot.local_planner.vfh.vfh_local_planner import VFHPurePursuitPlanner diff --git a/dimos/robot/local_planner/vfh/local_planner.py b/dimos/robot/local_planner/vfh/local_planner.py new file mode 100644 index 0000000000..70760e164d --- /dev/null +++ b/dimos/robot/local_planner/vfh/local_planner.py @@ -0,0 +1,1297 @@ +#!/usr/bin/env python3 + +import math +import numpy as np +from typing import Dict, Tuple, Optional, List, Any, Callable, Protocol +from abc import ABC, abstractmethod +import cv2 +from reactivex import Observable +from reactivex.subject import Subject +import threading +import time +import logging +from collections import deque +from dimos.utils.logging_config import setup_logger +from dimos.utils.ros_utils import normalize_angle, distance_angle_to_goal_xy + +from dimos.robot.robot import Robot +from dimos.types.vector import VectorLike, Vector, to_tuple +from dimos.types.path import Path +from dimos.types.costmap import Costmap +from dimos.robot.global_planner.algo import astar +from nav_msgs.msg import OccupancyGrid + +logger = setup_logger("dimos.robot.unitree.local_planner", level=logging.DEBUG) + + +class BaseLocalPlanner(ABC): + """ + Abstract base class for local planners that handle obstacle avoidance and path following. + + This class defines the common interface and shared functionality that all local planners + must implement, regardless of the specific algorithm used. + """ + + def __init__( + self, + get_costmap: Callable[[], Optional[OccupancyGrid]], + get_robot_pose: Callable[[], Any], + move: Callable[[Vector], None], + safety_threshold: float = 0.5, + max_linear_vel: float = 0.8, + max_angular_vel: float = 1.0, + lookahead_distance: float = 1.0, + goal_tolerance: float = 0.75, + angle_tolerance: float = 0.15, + robot_width: float = 0.5, + robot_length: float = 0.7, + visualization_size: int = 400, + control_frequency: float = 10.0, + safe_goal_distance: float = 1.5, + ): # Control frequency in Hz + """ + Initialize the base local planner. + + Args: + get_costmap: Function to get the latest local costmap + get_robot_pose: Function to get the latest robot pose (returning odom object) + move: Function to send velocity commands + safety_threshold: Distance to maintain from obstacles (meters) + max_linear_vel: Maximum linear velocity (m/s) + max_angular_vel: Maximum angular velocity (rad/s) + lookahead_distance: Lookahead distance for path following (meters) + goal_tolerance: Distance at which the goal is considered reached (meters) + angle_tolerance: Angle at which the goal orientation is considered reached (radians) + robot_width: Width of the robot for visualization (meters) + robot_length: Length of the robot for visualization (meters) + visualization_size: Size of the visualization image in pixels + control_frequency: Frequency at which the planner is called (Hz) + safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) + """ + # Store callables for robot interactions + self.get_costmap = get_costmap + self.get_robot_pose = get_robot_pose + self.move = move + + # Store parameters + self.safety_threshold = safety_threshold + self.max_linear_vel = max_linear_vel + self.max_angular_vel = max_angular_vel + self.lookahead_distance = lookahead_distance + self.goal_tolerance = goal_tolerance + self.angle_tolerance = angle_tolerance + self.robot_width = robot_width + self.robot_length = robot_length + self.visualization_size = visualization_size + self.control_frequency = control_frequency + self.control_period = 1.0 / control_frequency # Period in seconds + self.safe_goal_distance = safe_goal_distance # Distance to ignore obstacles at goal + self.ignore_obstacles = False # Flag for derived classes to check + + # Goal and Waypoint Tracking + self.goal_xy: Optional[Tuple[float, float]] = None # Current target for planning + self.goal_theta: Optional[float] = None # Goal orientation in odom frame + self.position_reached: bool = False # Flag indicating if position goal is reached + self.waypoints: Optional[Path] = None # Full path if following waypoints + self.waypoints_in_absolute: Optional[Path] = None # Full path in absolute frame + self.waypoint_is_relative: bool = False # Whether waypoints are in relative frame + self.current_waypoint_index: int = 0 # Index of the next waypoint to reach + self.final_goal_reached: bool = False # Flag indicating if the final waypoint is reached + + # Stuck detection + self.stuck_detection_window_seconds = 8.0 # Time window for stuck detection (seconds) + self.position_history_size = int(self.stuck_detection_window_seconds * control_frequency) + self.position_history = deque( + maxlen=self.position_history_size + ) # History of recent positions + self.stuck_distance_threshold = 0.1 # Distance threshold for stuck detection (meters) + self.unstuck_distance_threshold = 0.5 # Distance threshold for unstuck detection (meters) + self.stuck_time_threshold = 4.0 # Time threshold for stuck detection (seconds) + self.is_recovery_active = False # Whether recovery behavior is active + self.recovery_start_time = 0.0 # When recovery behavior started + self.recovery_duration = 8.0 # How long to run recovery before giving up (seconds) + self.last_update_time = time.time() # Last time position was updated + self.navigation_failed = False # Flag indicating if navigation should be terminated + + def reset(self): + """ + Reset all navigation and state tracking variables. + Should be called whenever a new goal is set. + """ + # Reset stuck detection state + self.position_history.clear() + self.is_recovery_active = False + self.recovery_start_time = 0.0 + self.last_update_time = time.time() + + # Reset navigation state flags + self.navigation_failed = False + self.position_reached = False + self.final_goal_reached = False + self.ignore_obstacles = False + + logger.info("Local planner state has been reset") + + def set_goal( + self, goal_xy: VectorLike, is_relative: bool = False, goal_theta: Optional[float] = None + ): + """Set a single goal position, converting to absolute frame if necessary. + This clears any existing waypoints being followed. + + Args: + goal_xy: The goal position to set. + is_relative: Whether the goal is in the robot's relative frame. + goal_theta: Optional goal orientation in radians + """ + # Reset all state variables + self.reset() + + # Clear waypoint following state + self.waypoints = None + self.current_waypoint_index = 0 + self.goal_xy = None # Clear previous goal + self.goal_theta = None # Clear previous goal orientation + + target_goal_xy: Optional[Tuple[float, float]] = None + + # Transform goal to absolute frame if it's relative + if is_relative: + # Get current robot pose + odom = self.get_robot_pose() + robot_pos, robot_rot = odom.pos, odom.rot + + # Extract current position and orientation + robot_x, robot_y = robot_pos.x, robot_pos.y + robot_theta = robot_rot.z # Assuming rotation is euler angles + + # Transform the relative goal into absolute coordinates + goal_x, goal_y = to_tuple(goal_xy) + # Rotate + abs_x = goal_x * math.cos(robot_theta) - goal_y * math.sin(robot_theta) + abs_y = goal_x * math.sin(robot_theta) + goal_y * math.cos(robot_theta) + # Translate + target_goal_xy = (robot_x + abs_x, robot_y + abs_y) + + logger.info( + f"Goal set in relative frame, converted to absolute: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" + ) + else: + target_goal_xy = to_tuple(goal_xy) + logger.info( + f"Goal set directly in absolute frame: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" + ) + + # Check if goal is valid (in bounds and not colliding) + if not self.is_goal_in_costmap_bounds(target_goal_xy) or self.check_goal_collision( + target_goal_xy + ): + logger.warning( + "Goal is in collision or out of bounds. Adjusting goal to valid position." + ) + self.goal_xy = self.adjust_goal_to_valid_position(target_goal_xy) + else: + self.goal_xy = target_goal_xy # Set the adjusted or original valid goal + + # Set goal orientation if provided + if goal_theta is not None: + if is_relative: + # Transform the orientation to absolute frame + odom = self.get_robot_pose() + robot_theta = odom.rot.z + self.goal_theta = normalize_angle(goal_theta + robot_theta) + else: + self.goal_theta = goal_theta + + def set_goal_waypoints(self, waypoints: Path, goal_theta: Optional[float] = None): + """Sets a path of waypoints for the robot to follow. + + Args: + waypoints: A list of waypoints to follow. Each waypoint is a tuple of (x, y) coordinates in absolute frame. + goal_theta: Optional final orientation in radians + """ + # Reset all state variables + self.reset() + + if not isinstance(waypoints, Path) or len(waypoints) == 0: + logger.warning("Invalid or empty path provided to set_goal_waypoints. Ignoring.") + self.waypoints = None + self.waypoint_is_relative = False + self.goal_xy = None + self.goal_theta = None + self.current_waypoint_index = 0 + return + + logger.info(f"Setting goal waypoints with {len(waypoints)} points.") + self.waypoints = waypoints + self.waypoint_is_relative = False + self.current_waypoint_index = 0 + + # Waypoints are always in absolute frame + self.waypoints_in_absolute = waypoints + + # Set the initial target to the first waypoint, adjusting if necessary + first_waypoint = self.waypoints_in_absolute[0] + if not self.is_goal_in_costmap_bounds(first_waypoint) or self.check_goal_collision( + first_waypoint + ): + logger.warning("First waypoint is invalid. Adjusting...") + self.goal_xy = self.adjust_goal_to_valid_position(first_waypoint) + else: + self.goal_xy = to_tuple(first_waypoint) # Initial target + + # Set goal orientation if provided + if goal_theta is not None: + self.goal_theta = goal_theta + + def _get_robot_pose(self) -> Tuple[Tuple[float, float], float]: + """ + Get the current robot position and orientation. + + Returns: + Tuple containing: + - position as (x, y) tuple + - orientation (theta) in radians + """ + odom = self.get_robot_pose() + pos, rot = odom.pos, odom.rot + return (pos.x, pos.y), rot.z + + def _get_final_goal_position(self) -> Optional[Tuple[float, float]]: + """ + Get the final goal position (either last waypoint or direct goal). + + Returns: + Tuple (x, y) of the final goal, or None if no goal is set + """ + if self.waypoints_in_absolute is not None and len(self.waypoints_in_absolute) > 0: + return to_tuple(self.waypoints_in_absolute[-1]) + elif self.goal_xy is not None: + return self.goal_xy + return None + + def _distance_to_position(self, target_position: Tuple[float, float]) -> float: + """ + Calculate distance from the robot to a target position. + + Args: + target_position: Target (x, y) position + + Returns: + Distance in meters + """ + robot_pos, _ = self._get_robot_pose() + return np.linalg.norm( + [target_position[0] - robot_pos[0], target_position[1] - robot_pos[1]] + ) + + def plan(self) -> Dict[str, float]: + """ + Main planning method that computes velocity commands. + This includes common planning logic like waypoint following, + with algorithm-specific calculations delegated to subclasses. + + Returns: + Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys + """ + # If goal orientation is specified, rotate to match it + if ( + self.position_reached + and self.goal_theta is not None + and not self._is_goal_orientation_reached() + ): + logger.info("Position goal reached. Rotating to target orientation.") + return self._rotate_to_goal_orientation() + + # Check if the robot is stuck and handle accordingly + if self.check_if_stuck() and not self.position_reached: + # Check if we're stuck but close to our goal + final_goal_pos = self._get_final_goal_position() + + # If we have a goal position, check distance to it + if final_goal_pos is not None: + distance_to_goal = self._distance_to_position(final_goal_pos) + + # If we're stuck but within 2x safe_goal_distance of the goal, consider it a success + if distance_to_goal < 2.0 * self.safe_goal_distance: + logger.info( + f"Robot is stuck but within {distance_to_goal:.2f}m of goal (< {2.0 * self.safe_goal_distance:.2f}m). Considering navigation successful." + ) + self.position_reached = True + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Otherwise, execute normal recovery behavior + logger.warning("Robot is stuck - executing recovery behavior") + return self.execute_recovery_behavior() + + # Reset obstacle ignore flag + self.ignore_obstacles = False + + # --- Waypoint Following Mode --- + if self.waypoints is not None: + if self.final_goal_reached: + logger.info("Final waypoint reached. Stopping.") + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Get current robot pose + robot_pos, robot_theta = self._get_robot_pose() + robot_pos_np = np.array(robot_pos) + + # Check if close to final waypoint + if self.waypoints_in_absolute is not None and len(self.waypoints_in_absolute) > 0: + final_waypoint = self.waypoints_in_absolute[-1] + dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) + + # If we're close to the final waypoint, adjust it and ignore obstacles + if dist_to_final < self.safe_goal_distance: + final_wp_tuple = to_tuple(final_waypoint) + adjusted_goal = self.adjust_goal_to_valid_position(final_wp_tuple) + # Create a new Path with the adjusted final waypoint + new_waypoints = self.waypoints_in_absolute[:-1] # Get all but the last waypoint + new_waypoints.append(adjusted_goal) # Append the adjusted goal + self.waypoints_in_absolute = new_waypoints + self.ignore_obstacles = True + logger.debug(f"Within safe distance of final waypoint. Ignoring obstacles.") + + # Update the target goal based on waypoint progression + just_reached_final = self._update_waypoint_target(robot_pos_np) + + # If the helper indicates the final goal was just reached, stop immediately + if just_reached_final: + return {"x_vel": 0.0, "angular_vel": 0.0} + + # --- Single Goal or Current Waypoint Target Set --- + if self.goal_xy is None: + # If no goal is set (e.g., empty path or rejected goal), stop. + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Get necessary data for planning + costmap = self.get_costmap() + if costmap is None: + logger.warning("Local costmap is None. Cannot plan.") + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Check if close to single goal mode goal + if self.waypoints is None: + # Get distance to goal + goal_distance = self._distance_to_position(self.goal_xy) + + # If within safe distance of goal, adjust it and ignore obstacles + if goal_distance < self.safe_goal_distance: + self.goal_xy = self.adjust_goal_to_valid_position(self.goal_xy) + self.ignore_obstacles = True + logger.debug(f"Within safe distance of goal. Ignoring obstacles.") + + # First check position + if goal_distance < self.goal_tolerance or self.position_reached: + self.position_reached = True + + else: + self.position_reached = False + + # Call the algorithm-specific planning implementation + return self._compute_velocity_commands() + + @abstractmethod + def _compute_velocity_commands(self) -> Dict[str, float]: + """ + Algorithm-specific method to compute velocity commands. + Must be implemented by derived classes. + + Returns: + Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys + """ + pass + + def _rotate_to_goal_orientation(self) -> Dict[str, float]: + """Compute velocity commands to rotate to the goal orientation. + + Returns: + Dict[str, float]: Velocity commands with zero linear velocity + """ + # Get current robot orientation + _, robot_theta = self._get_robot_pose() + + # Calculate the angle difference + angle_diff = normalize_angle(self.goal_theta - robot_theta) + + # Determine rotation direction and speed + if abs(angle_diff) < self.angle_tolerance: + # Already at correct orientation + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Calculate rotation speed - proportional to the angle difference + # but capped at max_angular_vel + direction = 1.0 if angle_diff > 0 else -1.0 + angular_vel = direction * min(abs(angle_diff) * 2.0, self.max_angular_vel) + + # logger.debug(f"Rotating to goal orientation: angle_diff={angle_diff:.4f}, angular_vel={angular_vel:.4f}") + return {"x_vel": 0.0, "angular_vel": angular_vel} + + def _is_goal_orientation_reached(self) -> bool: + """Check if the current robot orientation matches the goal orientation. + + Returns: + bool: True if orientation is reached or no orientation goal is set + """ + if self.goal_theta is None: + return True # No orientation goal set + + # Get current robot orientation + _, robot_theta = self._get_robot_pose() + + # Calculate the angle difference and normalize + angle_diff = abs(normalize_angle(self.goal_theta - robot_theta)) + + logger.debug( + f"Orientation error: {angle_diff:.4f} rad, tolerance: {self.angle_tolerance:.4f} rad" + ) + return angle_diff <= self.angle_tolerance + + def _update_waypoint_target(self, robot_pos_np: np.ndarray) -> bool: + """Helper function to manage waypoint progression and update the target goal. + + Args: + robot_pos_np: Current robot position as a numpy array [x, y]. + + Returns: + bool: True if the final waypoint has just been reached, False otherwise. + """ + if self.waypoints is None or len(self.waypoints) == 0: + return False # Not in waypoint mode or empty path + + # Waypoints are always in absolute frame + self.waypoints_in_absolute = self.waypoints + + # Check if final goal is reached + final_waypoint = self.waypoints_in_absolute[-1] + dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) + + if dist_to_final < self.goal_tolerance: + self.position_reached = True + self.goal_xy = to_tuple(final_waypoint) + + # If goal orientation is not specified or achieved, consider fully reached + if self.goal_theta is None or self._is_goal_orientation_reached(): + self.final_goal_reached = True + logger.info("Reached final waypoint with correct orientation.") + return True + else: + logger.info("Reached final waypoint position, rotating to target orientation.") + return False + + # Always find the lookahead point + lookahead_point = None + for i in range(self.current_waypoint_index, len(self.waypoints_in_absolute)): + wp = self.waypoints_in_absolute[i] + dist_to_wp = np.linalg.norm(robot_pos_np - wp) + if dist_to_wp >= self.lookahead_distance: + lookahead_point = wp + # Update current waypoint index to this point + self.current_waypoint_index = i + break + + # If no point is far enough, target the final waypoint + if lookahead_point is None: + lookahead_point = self.waypoints_in_absolute[-1] + self.current_waypoint_index = len(self.waypoints_in_absolute) - 1 + + # Set the lookahead point as the immediate target, adjusting if needed + if not self.is_goal_in_costmap_bounds(lookahead_point) or self.check_goal_collision( + lookahead_point + ): + logger.debug("Lookahead point is invalid. Adjusting...") + adjusted_lookahead = self.adjust_goal_to_valid_position(lookahead_point) + # Only update if adjustment didn't fail completely + if adjusted_lookahead is not None: + self.goal_xy = adjusted_lookahead + else: + self.goal_xy = to_tuple(lookahead_point) + + return False # Final goal not reached in this update cycle + + @abstractmethod + def update_visualization(self) -> np.ndarray: + """ + Generate visualization of the planning state. + Must be implemented by derived classes. + + Returns: + np.ndarray: Visualization image as numpy array + """ + pass + + def create_stream(self, frequency_hz: float = None) -> Observable: + """ + Create an Observable stream that emits the visualization image at a fixed frequency. + + Args: + frequency_hz: Optional frequency override (defaults to 1/4 of control_frequency if None) + + Returns: + Observable: Stream of visualization frames + """ + # Default to 1/4 of control frequency if not specified (to reduce CPU usage) + if frequency_hz is None: + frequency_hz = self.control_frequency / 4.0 + + subject = Subject() + sleep_time = 1.0 / frequency_hz + + def frame_emitter(): + while True: + try: + # Generate the frame using the updated method + frame = self.update_visualization() + subject.on_next(frame) + except Exception as e: + logger.error(f"Error in frame emitter thread: {e}") + # Optionally, emit an error frame or simply skip + # subject.on_error(e) # This would terminate the stream + time.sleep(sleep_time) + + emitter_thread = threading.Thread(target=frame_emitter, daemon=True) + emitter_thread.start() + logger.info(f"Started visualization frame emitter thread at {frequency_hz:.1f} Hz") + return subject + + @abstractmethod + def check_collision(self, direction: float) -> bool: + """ + Check if there's a collision in the given direction. + Must be implemented by derived classes. + + Args: + direction: Direction to check for collision in radians + + Returns: + bool: True if collision detected, False otherwise + """ + pass + + def is_goal_reached(self) -> bool: + """Check if the final goal (single or last waypoint) is reached, including orientation.""" + if self.waypoints is not None: + # Waypoint mode: check if the final waypoint and orientation have been reached + return self.final_goal_reached + else: + # Single goal mode: check distance to the single goal and orientation + if self.goal_xy is None: + return False # No goal set + + return self.position_reached and self._is_goal_orientation_reached() + + def check_goal_collision(self, goal_xy: VectorLike) -> bool: + """Check if the current goal is in collision with obstacles in the costmap. + + Returns: + bool: True if goal is in collision, False if goal is safe or cannot be checked + """ + + costmap = self.get_costmap() + if costmap is None: + logger.warning("Cannot check collision: No costmap available") + return False + + # Check if the position is occupied + collision_threshold = 80 # Consider values above 80 as obstacles + + # Use Costmap's is_occupied method + return costmap.is_occupied(goal_xy, threshold=collision_threshold) + + def is_goal_in_costmap_bounds(self, goal_xy: VectorLike) -> bool: + """Check if the goal position is within the bounds of the costmap. + + Args: + goal_xy: Goal position (x, y) in odom frame + + Returns: + bool: True if the goal is within the costmap bounds, False otherwise + """ + costmap = self.get_costmap() + if costmap is None: + logger.warning("Cannot check bounds: No costmap available") + return False + + # Get goal position in grid coordinates + goal_point = costmap.world_to_grid(goal_xy) + goal_cell_x, goal_cell_y = goal_point.x, goal_point.y + + # Check if goal is within the costmap bounds + is_in_bounds = 0 <= goal_cell_x < costmap.width and 0 <= goal_cell_y < costmap.height + + if not is_in_bounds: + logger.warning(f"Goal ({goal_xy[0]:.2f}, {goal_xy[1]:.2f}) is outside costmap bounds") + + return is_in_bounds + + def adjust_goal_to_valid_position( + self, goal_xy: VectorLike, clearance: float = 0.5 + ) -> Tuple[float, float]: + """Find a valid (non-colliding) goal position by moving it towards the robot. + + Args: + goal_xy: Original goal position (x, y) in odom frame + clearance: Additional distance to move back from obstacles for better clearance (meters) + + Returns: + Tuple[float, float]: A valid goal position, or the original goal if already valid + """ + [pos, rot] = self._get_robot_pose() + + robot_x, robot_y = pos[0], pos[1] + + # Original goal + goal_x, goal_y = to_tuple(goal_xy) + + if not self.check_goal_collision((goal_x, goal_y)): + return (goal_x, goal_y) + + # Calculate vector from goal to robot + dx = robot_x - goal_x + dy = robot_y - goal_y + distance = np.sqrt(dx * dx + dy * dy) + + if distance < 0.001: # Goal is at robot position + return to_tuple(goal_xy) + + # Normalize direction vector + dx /= distance + dy /= distance + + # Step size + step_size = 0.25 # meters + + # Move goal towards robot step by step + current_x, current_y = goal_x, goal_y + steps = 0 + max_steps = 50 # Safety limit + + # Variables to store the first valid position found + valid_found = False + valid_x, valid_y = None, None + + while steps < max_steps: + # Move towards robot + current_x += dx * step_size + current_y += dy * step_size + steps += 1 + + # Check if we've reached or passed the robot + new_distance = np.sqrt((current_x - robot_x) ** 2 + (current_y - robot_y) ** 2) + if new_distance < step_size: + # We've reached the robot without finding a valid point + # Move back one step from robot to avoid self-collision + current_x = robot_x - dx * step_size + current_y = robot_y - dy * step_size + break + + # Check if this position is valid + if not self.check_goal_collision( + (current_x, current_y) + ) and self.is_goal_in_costmap_bounds((current_x, current_y)): + # Store the first valid position + if not valid_found: + valid_found = True + valid_x, valid_y = current_x, current_y + + # If clearance is requested, continue searching for a better position + if clearance > 0: + continue + + # Calculate position with additional clearance + if clearance > 0: + # Calculate clearance position + clearance_x = current_x + dx * clearance + clearance_y = current_y + dy * clearance + + logger.info( + f"Checking clearance position at ({clearance_x:.2f}, {clearance_y:.2f})" + ) + + # Check if the clearance position is also valid + if not self.check_goal_collision( + (clearance_x, clearance_y) + ) and self.is_goal_in_costmap_bounds((clearance_x, clearance_y)): + logger.info( + f"Found valid goal with clearance at ({clearance_x:.2f}, {clearance_y:.2f})" + ) + return (clearance_x, clearance_y) + + # Return the valid position without clearance + logger.info(f"Found valid goal at ({current_x:.2f}, {current_y:.2f})") + return (current_x, current_y) + + # If we found a valid position earlier but couldn't add clearance + if valid_found: + logger.info(f"Using valid goal found at ({valid_x:.2f}, {valid_y:.2f})") + return (valid_x, valid_y) + + logger.warning( + f"Could not find valid goal after {steps} steps, using closest point to robot" + ) + return (current_x, current_y) + + def check_if_stuck(self) -> bool: + """ + Check if the robot is stuck by analyzing movement history. + + Returns: + bool: True if the robot is determined to be stuck, False otherwise + """ + # Get current position and time + current_time = time.time() + + # Get current robot position + [pos, _] = self._get_robot_pose() + current_position = (pos[0], pos[1], current_time) + + # Add current position to history (newest is appended at the end) + self.position_history.append(current_position) + + # Need enough history to make a determination + min_history_size = self.stuck_detection_window_seconds * self.control_frequency + if len(self.position_history) < min_history_size: + return False + + # Find positions within our detection window (positions are already in order from oldest to newest) + window_start_time = current_time - self.stuck_detection_window_seconds + window_positions = [] + + # Collect positions within the window (newest entries will be at the end) + for pos_x, pos_y, timestamp in self.position_history: + if timestamp >= window_start_time: + window_positions.append((pos_x, pos_y, timestamp)) + + # Need at least a few positions in the window + if len(window_positions) < 3: + return False + + # Ensure correct order: oldest to newest + window_positions.sort(key=lambda p: p[2]) + + # Get the oldest and newest positions in the window + oldest_x, oldest_y, oldest_time = window_positions[0] + newest_x, newest_y, newest_time = window_positions[-1] + + # Calculate time range in the window (should always be positive) + time_range = newest_time - oldest_time + + # Calculate displacement from oldest to newest position + displacement = np.sqrt((newest_x - oldest_x) ** 2 + (newest_y - oldest_y) ** 2) + + # Check if we're stuck - moved less than threshold over minimum time + # Only consider it if the time range makes sense (positive and sufficient) + is_currently_stuck = ( + time_range >= self.stuck_time_threshold + and time_range <= self.stuck_detection_window_seconds + and displacement < self.stuck_distance_threshold + ) + + if is_currently_stuck: + logger.warning( + f"Robot appears to be stuck! Displacement {displacement:.3f}m over {time_range:.1f}s" + ) + + # Don't trigger recovery if it's already active + if not self.is_recovery_active: + self.is_recovery_active = True + self.recovery_start_time = current_time + return True + + # Check if we've been trying to recover for too long + elif current_time - self.recovery_start_time > self.recovery_duration: + logger.error( + f"Recovery behavior has been active for {self.recovery_duration}s without success" + ) + # Reset recovery state - maybe a different behavior will work + self.is_recovery_active = False + self.recovery_start_time = current_time + + # If we've moved enough, we're not stuck anymore + elif self.is_recovery_active and displacement > self.unstuck_distance_threshold: + logger.info(f"Robot has escaped from stuck state (moved {displacement:.3f}m)") + self.is_recovery_active = False + + return self.is_recovery_active + + def execute_recovery_behavior(self) -> Dict[str, float]: + """ + Execute a recovery behavior when the robot is stuck. + + Returns: + Dict[str, float]: Velocity commands for the recovery behavior + """ + # Calculate how long we've been in recovery + recovery_time = time.time() - self.recovery_start_time + + # Calculate recovery phases based on control frequency + backup_phase_time = 3.0 # seconds + rotate_phase_time = 2.0 # seconds + + # Simple recovery behavior state machine + if recovery_time < backup_phase_time: + # First try backing up + logger.info("Recovery: backing up") + return {"x_vel": -0.2, "angular_vel": 0.0} + elif recovery_time < backup_phase_time + rotate_phase_time: + # Then try rotating + logger.info("Recovery: rotating to find new path") + rotation_direction = 1.0 if np.random.random() > 0.5 else -1.0 + return {"x_vel": 0.0, "angular_vel": rotation_direction * self.max_angular_vel * 0.7} + else: + # If we're still stuck after backup and rotation, terminate navigation + logger.error("Recovery failed after backup and rotation. Navigation terminated.") + # Set a flag to indicate navigation should terminate + self.navigation_failed = True + # Stop the robot + return {"x_vel": 0.0, "angular_vel": 0.0} + + +def navigate_to_goal_local( + robot, + goal_xy_robot: Tuple[float, float], + goal_theta: Optional[float] = None, + distance: float = 0.0, + timeout: float = 60.0, + stop_event: Optional[threading.Event] = None, +) -> bool: + """ + Navigates the robot to a goal specified in the robot's local frame + using the local planner. + + Args: + robot: Robot instance to control + goal_xy_robot: Tuple (x, y) representing the goal position relative + to the robot's current position and orientation. + distance: Desired distance to maintain from the goal in meters. + If non-zero, the robot will stop this far away from the goal. + timeout: Maximum time (in seconds) allowed to reach the goal. + stop_event: Optional threading.Event to signal when navigation should stop + + Returns: + bool: True if the goal was reached within the timeout, False otherwise. + """ + logger.info( + f"Starting navigation to local goal {goal_xy_robot} with distance {distance}m and timeout {timeout}s." + ) + + goal_x, goal_y = goal_xy_robot + + # Calculate goal orientation to face the target + if goal_theta is None: + goal_theta = np.arctan2(goal_y, goal_x) + + # If distance is non-zero, adjust the goal to stop at the desired distance + if distance > 0: + # Calculate magnitude of the goal vector + goal_distance = np.sqrt(goal_x**2 + goal_y**2) + + # Only adjust if goal is further than the desired distance + if goal_distance > distance: + goal_x, goal_y = distance_angle_to_goal_xy(goal_distance - distance, goal_theta) + + # Set the goal in the robot's frame with orientation to face the original target + robot.local_planner.set_goal((goal_x, goal_y), is_relative=True, goal_theta=goal_theta) + + # Get control period from robot's local planner for consistent timing + control_period = 1.0 / robot.local_planner.control_frequency + + start_time = time.time() + goal_reached = False + + try: + while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): + # Check if goal has been reached + if robot.local_planner.is_goal_reached(): + logger.info("Goal reached successfully.") + goal_reached = True + break + + # Check if navigation failed flag is set + if robot.local_planner.navigation_failed: + logger.error("Navigation aborted due to repeated recovery failures.") + goal_reached = False + break + + # Get planned velocity towards the goal + vel_command = robot.local_planner.plan() + x_vel = vel_command.get("x_vel", 0.0) + angular_vel = vel_command.get("angular_vel", 0.0) + + # Send velocity command + robot.local_planner.move(Vector(x_vel, 0, angular_vel)) + + # Control loop frequency - use robot's control frequency + time.sleep(control_period) + + if not goal_reached: + logger.warning(f"Navigation timed out after {timeout} seconds before reaching goal.") + + except KeyboardInterrupt: + logger.info("Navigation to local goal interrupted by user.") + goal_reached = False # Consider interruption as failure + except Exception as e: + logger.error(f"Error during navigation to local goal: {e}") + goal_reached = False # Consider error as failure + finally: + logger.info("Stopping robot after navigation attempt.") + robot.local_planner.move(Vector(0, 0, 0)) # Stop the robot + + return goal_reached + + +def navigate_path_local( + robot, + path: Path, + timeout: float = 120.0, + goal_theta: Optional[float] = None, + stop_event: Optional[threading.Event] = None, +) -> bool: + """ + Navigates the robot along a path of waypoints using the waypoint following capability + of the local planner. + + Args: + robot: Robot instance to control + path: Path object containing waypoints in absolute frame + timeout: Maximum time (in seconds) allowed to follow the complete path + goal_theta: Optional final orientation in radians + stop_event: Optional threading.Event to signal when navigation should stop + + Returns: + bool: True if the entire path was successfully followed, False otherwise + """ + logger.info( + f"Starting navigation along path with {len(path)} waypoints and timeout {timeout}s." + ) + + # Set the path in the local planner + robot.local_planner.set_goal_waypoints(path, goal_theta=goal_theta) + + # Get control period from robot's local planner for consistent timing + control_period = 1.0 / robot.local_planner.control_frequency + + start_time = time.time() + path_completed = False + + try: + while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): + # Check if the entire path has been traversed + if robot.local_planner.is_goal_reached(): + logger.info("Path traversed successfully.") + path_completed = True + break + + # Check if navigation failed flag is set + if robot.local_planner.navigation_failed: + logger.error("Navigation aborted due to repeated recovery failures.") + path_completed = False + break + + # Get planned velocity towards the current waypoint target + vel_command = robot.local_planner.plan() + x_vel = vel_command.get("x_vel", 0.0) + angular_vel = vel_command.get("angular_vel", 0.0) + + # Send velocity command + robot.local_planner.move(Vector(x_vel, 0, angular_vel)) + + # Control loop frequency - use robot's control frequency + time.sleep(control_period) + + if not path_completed: + logger.warning( + f"Path following timed out after {timeout} seconds before completing the path." + ) + + except KeyboardInterrupt: + logger.info("Path navigation interrupted by user.") + path_completed = False + except Exception as e: + logger.error(f"Error during path navigation: {e}") + path_completed = False + finally: + logger.info("Stopping robot after path navigation attempt.") + robot.local_planner.move(Vector(0, 0, 0)) # Stop the robot + + return path_completed + + +def visualize_local_planner_state( + occupancy_grid: np.ndarray, + grid_resolution: float, + grid_origin: Tuple[float, float], + robot_pose: Tuple[float, float, float], + visualization_size: int = 400, + robot_width: float = 0.5, + robot_length: float = 0.7, + map_size_meters: float = 10.0, + goal_xy: Optional[Tuple[float, float]] = None, + goal_theta: Optional[float] = None, + histogram: Optional[np.ndarray] = None, + selected_direction: Optional[float] = None, + waypoints: Optional["Path"] = None, + current_waypoint_index: Optional[int] = None, +) -> np.ndarray: + """Generate a bird's eye view visualization of the local costmap. + Optionally includes VFH histogram, selected direction, and waypoints path. + + Args: + occupancy_grid: 2D numpy array of the occupancy grid + grid_resolution: Resolution of the grid in meters/cell + grid_origin: Tuple (x, y) of the grid origin in the odom frame + robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame + visualization_size: Size of the visualization image in pixels + robot_width: Width of the robot in meters + robot_length: Length of the robot in meters + map_size_meters: Size of the map to visualize in meters + goal_xy: Optional tuple (x, y) of the goal position in the odom frame + goal_theta: Optional goal orientation in radians (in odom frame) + histogram: Optional numpy array of the VFH histogram + selected_direction: Optional selected direction angle in radians + waypoints: Optional Path object containing waypoints to visualize + current_waypoint_index: Optional index of the current target waypoint + """ + + robot_x, robot_y, robot_theta = robot_pose + grid_origin_x, grid_origin_y = grid_origin + vis_size = visualization_size + scale = vis_size / map_size_meters + + vis_img = np.ones((vis_size, vis_size, 3), dtype=np.uint8) * 255 + center_x = vis_size // 2 + center_y = vis_size // 2 + + grid_height, grid_width = occupancy_grid.shape + + # Calculate robot position relative to grid origin + robot_rel_x = robot_x - grid_origin_x + robot_rel_y = robot_y - grid_origin_y + robot_cell_x = int(robot_rel_x / grid_resolution) + robot_cell_y = int(robot_rel_y / grid_resolution) + + half_size_cells = int(map_size_meters / grid_resolution / 2) + + # Draw grid cells (using standard occupancy coloring) + for y in range( + max(0, robot_cell_y - half_size_cells), min(grid_height, robot_cell_y + half_size_cells) + ): + for x in range( + max(0, robot_cell_x - half_size_cells), min(grid_width, robot_cell_x + half_size_cells) + ): + cell_rel_x_meters = (x - robot_cell_x) * grid_resolution + cell_rel_y_meters = (y - robot_cell_y) * grid_resolution + + img_x = int(center_x + cell_rel_x_meters * scale) + img_y = int(center_y - cell_rel_y_meters * scale) # Flip y-axis + + if 0 <= img_x < vis_size and 0 <= img_y < vis_size: + cell_value = occupancy_grid[y, x] + if cell_value == -1: + color = (200, 200, 200) # Unknown (Light gray) + elif cell_value == 0: + color = (255, 255, 255) # Free (White) + else: # Occupied + # Scale darkness based on occupancy value (0-100) + darkness = 255 - int(155 * (cell_value / 100)) - 100 + color = (darkness, darkness, darkness) # Shades of gray/black + + cell_size_px = max(1, int(grid_resolution * scale)) + cv2.rectangle( + vis_img, + (img_x - cell_size_px // 2, img_y - cell_size_px // 2), + (img_x + cell_size_px // 2, img_y + cell_size_px // 2), + color, + -1, + ) + + # Draw waypoints path if provided + if waypoints is not None and len(waypoints) > 0: + try: + path_points = [] + for i, waypoint in enumerate(waypoints): + # Convert waypoint from odom frame to visualization frame + wp_x, wp_y = waypoint[0], waypoint[1] + wp_rel_x = wp_x - robot_x + wp_rel_y = wp_y - robot_y + + wp_img_x = int(center_x + wp_rel_x * scale) + wp_img_y = int(center_y - wp_rel_y * scale) # Flip y-axis + + if 0 <= wp_img_x < vis_size and 0 <= wp_img_y < vis_size: + path_points.append((wp_img_x, wp_img_y)) + + # Draw each waypoint as a small circle + cv2.circle(vis_img, (wp_img_x, wp_img_y), 3, (0, 128, 0), -1) # Dark green dots + + # Highlight current target waypoint + if current_waypoint_index is not None and i == current_waypoint_index: + cv2.circle(vis_img, (wp_img_x, wp_img_y), 6, (0, 0, 255), 2) # Red circle + + # Connect waypoints with lines to show the path + if len(path_points) > 1: + for i in range(len(path_points) - 1): + cv2.line( + vis_img, path_points[i], path_points[i + 1], (0, 200, 0), 1 + ) # Green line + except Exception as e: + logger.error(f"Error drawing waypoints: {e}") + + # Draw histogram + if histogram is not None: + num_bins = len(histogram) + # Find absolute maximum value (ignoring any negative debug values) + abs_histogram = np.abs(histogram) + max_hist_value = np.max(abs_histogram) if np.max(abs_histogram) > 0 else 1.0 + hist_scale = (vis_size / 2) * 0.8 # Scale histogram lines to 80% of half the viz size + + for i in range(num_bins): + # Angle relative to robot's forward direction + angle_relative_to_robot = (i / num_bins) * 2 * math.pi - math.pi + # Angle in the visualization frame (relative to image +X axis) + vis_angle = angle_relative_to_robot + robot_theta + + # Get the value and check if it's a special debug value (negative) + hist_val = histogram[i] + is_debug_value = hist_val < 0 + + # Use absolute value for line length + normalized_val = min(1.0, abs(hist_val) / max_hist_value) + line_length = normalized_val * hist_scale + + # Calculate endpoint using the visualization angle + end_x = int(center_x + line_length * math.cos(vis_angle)) + end_y = int(center_y - line_length * math.sin(vis_angle)) # Flipped Y + + # Color based on value and whether it's a debug value + if is_debug_value: + # Use green for debug values (minimum cost bin) + color = (0, 255, 0) # Green + line_width = 2 # Thicker line for emphasis + else: + # Regular coloring for normal values (blue to red gradient based on obstacle density) + blue = max(0, 255 - int(normalized_val * 255)) + red = min(255, int(normalized_val * 255)) + color = (blue, 0, red) # BGR format: obstacles are redder, clear areas are bluer + line_width = 1 + + cv2.line(vis_img, (center_x, center_y), (end_x, end_y), color, line_width) + + # Draw robot + robot_length_px = int(robot_length * scale) + robot_width_px = int(robot_width * scale) + robot_pts = np.array( + [ + [-robot_length_px / 2, -robot_width_px / 2], + [robot_length_px / 2, -robot_width_px / 2], + [robot_length_px / 2, robot_width_px / 2], + [-robot_length_px / 2, robot_width_px / 2], + ], + dtype=np.float32, + ) + rotation_matrix = np.array( + [ + [math.cos(robot_theta), -math.sin(robot_theta)], + [math.sin(robot_theta), math.cos(robot_theta)], + ] + ) + robot_pts = np.dot(robot_pts, rotation_matrix.T) + robot_pts[:, 0] += center_x + robot_pts[:, 1] = center_y - robot_pts[:, 1] # Flip y-axis + cv2.fillPoly( + vis_img, [robot_pts.reshape((-1, 1, 2)).astype(np.int32)], (0, 0, 255) + ) # Red robot + + # Draw robot direction line + front_x = int(center_x + (robot_length_px / 2) * math.cos(robot_theta)) + front_y = int(center_y - (robot_length_px / 2) * math.sin(robot_theta)) + cv2.line(vis_img, (center_x, center_y), (front_x, front_y), (255, 0, 0), 2) # Blue line + + # Draw selected direction + if selected_direction is not None: + # selected_direction is relative to robot frame + # Angle in the visualization frame (relative to image +X axis) + vis_angle_selected = selected_direction + robot_theta + + # Make slightly longer than max histogram line + sel_dir_line_length = (vis_size / 2) * 0.9 + + sel_end_x = int(center_x + sel_dir_line_length * math.cos(vis_angle_selected)) + sel_end_y = int(center_y - sel_dir_line_length * math.sin(vis_angle_selected)) # Flipped Y + + cv2.line( + vis_img, (center_x, center_y), (sel_end_x, sel_end_y), (0, 165, 255), 2 + ) # BGR for Orange + + # Draw goal + if goal_xy is not None: + goal_x, goal_y = goal_xy + goal_rel_x_map = goal_x - robot_x + goal_rel_y_map = goal_y - robot_y + goal_img_x = int(center_x + goal_rel_x_map * scale) + goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis + if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: + cv2.circle(vis_img, (goal_img_x, goal_img_y), 5, (0, 255, 0), -1) # Green circle + cv2.circle(vis_img, (goal_img_x, goal_img_y), 8, (0, 0, 0), 1) # Black outline + + # Draw goal orientation + if goal_theta is not None and goal_xy is not None: + # For waypoint mode, only draw orientation at the final waypoint + if waypoints is not None and len(waypoints) > 0: + # Use the final waypoint position + final_waypoint = waypoints[-1] + goal_x, goal_y = final_waypoint[0], final_waypoint[1] + else: + # Use the current goal position + goal_x, goal_y = goal_xy + + goal_rel_x_map = goal_x - robot_x + goal_rel_y_map = goal_y - robot_y + goal_img_x = int(center_x + goal_rel_x_map * scale) + goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis + + # Calculate goal orientation vector direction in visualization frame + # goal_theta is already in odom frame, need to adjust for visualization orientation + goal_dir_length = 30 # Length of direction indicator in pixels + goal_dir_end_x = int(goal_img_x + goal_dir_length * math.cos(goal_theta)) + goal_dir_end_y = int(goal_img_y - goal_dir_length * math.sin(goal_theta)) # Flip y-axis + + # Draw goal orientation arrow + if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: + cv2.arrowedLine( + vis_img, + (goal_img_x, goal_img_y), + (goal_dir_end_x, goal_dir_end_y), + (255, 0, 255), + 4, + ) # Magenta arrow + + # Add scale bar + scale_bar_length_px = int(1.0 * scale) + scale_bar_x = vis_size - scale_bar_length_px - 10 + scale_bar_y = vis_size - 20 + cv2.line( + vis_img, + (scale_bar_x, scale_bar_y), + (scale_bar_x + scale_bar_length_px, scale_bar_y), + (0, 0, 0), + 2, + ) + cv2.putText( + vis_img, "1m", (scale_bar_x, scale_bar_y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 0), 1 + ) + + # Add status info + status_text = [] + if waypoints is not None: + if current_waypoint_index is not None: + status_text.append(f"WP: {current_waypoint_index}/{len(waypoints)}") + else: + status_text.append(f"WPs: {len(waypoints)}") + + y_pos = 20 + for text in status_text: + cv2.putText(vis_img, text, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + y_pos += 20 + + return vis_img diff --git a/dimos/robot/local_planner/vfh_local_planner.py b/dimos/robot/local_planner/vfh/vfh_local_planner.py similarity index 93% rename from dimos/robot/local_planner/vfh_local_planner.py rename to dimos/robot/local_planner/vfh/vfh_local_planner.py index d133c3ade8..09d77a9743 100644 --- a/dimos/robot/local_planner/vfh_local_planner.py +++ b/dimos/robot/local_planner/vfh/vfh_local_planner.py @@ -1,15 +1,21 @@ #!/usr/bin/env python3 +import math import numpy as np -from typing import Dict, Tuple, Optional, Callable +from typing import Dict, Tuple, Optional, Callable, Any import cv2 import logging +import time from dimos.utils.logging_config import setup_logger from dimos.utils.ros_utils import normalize_angle -from dimos.robot.local_planner.local_planner import BaseLocalPlanner, visualize_local_planner_state +from dimos.robot.local_planner.vfh.local_planner import ( + BaseLocalPlanner, + visualize_local_planner_state, +) from dimos.types.costmap import Costmap +from dimos.types.vector import Vector from nav_msgs.msg import OccupancyGrid logger = setup_logger("dimos.robot.unitree.vfh_local_planner", level=logging.DEBUG) @@ -24,8 +30,8 @@ class VFHPurePursuitPlanner(BaseLocalPlanner): def __init__( self, get_costmap: Callable[[], Optional[OccupancyGrid]], - transform: object, - move_vel_control: Callable[[float, float, float], None], + get_robot_pose: Callable[[], Any], + move: Callable[[Vector], None], safety_threshold: float = 0.8, histogram_bins: int = 144, max_linear_vel: float = 0.8, @@ -44,8 +50,8 @@ def __init__( Args: get_costmap: Function to get the latest local costmap - transform: Object with transform methods (transform_point, transform_rot, etc.) - move_vel_control: Function to send velocity commands + get_robot_pose: Function to get the latest robot pose (returning odom object) + move: Function to send velocity commands safety_threshold: Distance to maintain from obstacles (meters) histogram_bins: Number of directional bins in the polar histogram max_linear_vel: Maximum linear velocity (m/s) @@ -62,8 +68,8 @@ def __init__( # Initialize base class super().__init__( get_costmap=get_costmap, - transform=transform, - move_vel_control=move_vel_control, + get_robot_pose=get_robot_pose, + move=move, safety_threshold=safety_threshold, max_linear_vel=max_linear_vel, max_angular_vel=max_angular_vel, @@ -109,8 +115,8 @@ def _compute_velocity_commands(self) -> Dict[str, float]: logger.warning("No costmap available for planning") return {"x_vel": 0.0, "angular_vel": 0.0} - [pos, rot] = self.transform.transform_euler("base_link", "odom") - robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + robot_pos, robot_theta = self._get_robot_pose() + robot_x, robot_y = robot_pos robot_pose = (robot_x, robot_y, robot_theta) # Calculate goal-related parameters @@ -327,8 +333,8 @@ def check_collision(self, selected_direction: float, safety_threshold: float = 1 if costmap is None: return False # No costmap available - [pos, rot] = self.transform.transform_euler("base_link", "odom") - robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + robot_pos, robot_theta = self._get_robot_pose() + robot_x, robot_y = robot_pos # Direction in world frame direction_world = robot_theta + selected_direction @@ -363,8 +369,8 @@ def update_visualization(self) -> np.ndarray: if costmap is None: raise ValueError("Costmap is None") - [pos, rot] = self.transform.transform_euler("base_link", "odom") - robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + robot_pos, robot_theta = self._get_robot_pose() + robot_x, robot_y = robot_pos robot_pose = (robot_x, robot_y, robot_theta) goal_xy = self.goal_xy # This could be a lookahead point or final goal @@ -374,9 +380,9 @@ def update_visualization(self) -> np.ndarray: selected_direction = getattr(self, "selected_direction", None) # Get waypoint data if in waypoint mode - waypoints_to_draw = self.waypoints_in_odom + waypoints_to_draw = self.waypoints_in_absolute current_wp_index_to_draw = ( - self.current_waypoint_index if self.waypoints_in_odom is not None else None + self.current_waypoint_index if self.waypoints_in_absolute is not None else None ) # Ensure index is valid before passing if waypoints_to_draw is not None and current_wp_index_to_draw is not None: @@ -386,7 +392,7 @@ def update_visualization(self) -> np.ndarray: return visualize_local_planner_state( occupancy_grid=costmap.grid, grid_resolution=costmap.resolution, - grid_origin=(costmap.origin.x, costmap.origin.y, costmap.origin_theta), + grid_origin=(costmap.origin.x, costmap.origin.y), robot_pose=robot_pose, goal_xy=goal_xy, # Current target (lookahead or final) goal_theta=self.goal_theta, # Pass goal orientation if available diff --git a/dimos/robot/unitree_webrtc/type/odometry.py b/dimos/robot/unitree_webrtc/type/odometry.py index df10bd8d54..4e7a293076 100644 --- a/dimos/robot/unitree_webrtc/type/odometry.py +++ b/dimos/robot/unitree_webrtc/type/odometry.py @@ -1,9 +1,15 @@ from typing import TypedDict, Literal from datetime import datetime from dataclasses import dataclass -from dimos.types.vector import Vector +from dimos.types.vector import VectorLike, Vector from dimos.types.position import Position -from dimos.robot.unitree_webrtc.type.timeseries import Timestamped, to_human_readable +from dimos.robot.unitree_webrtc.type.timeseries import ( + Timestamped, + to_human_readable, + to_datetime, + EpochLike, +) + raw_odometry_msg_sample = { "type": "msg", @@ -57,18 +63,19 @@ class RawOdometryMessage(TypedDict): data: OdometryData -@dataclass -class Odometry(Timestamped, Position): - ts: datetime +class Odometry(Position): + def __init__(self, pos: VectorLike, rot: VectorLike, ts: EpochLike): + super().__init__(pos, rot) + self.ts = to_datetime(ts) if ts else datetime.now() @classmethod def from_msg(cls, msg: RawOdometryMessage) -> "Odometry": pose = msg["data"]["pose"] orientation = pose["orientation"] position = pose["position"] - pos = Vector(position.get("x"), position.get("y"), position.get("z")) - rot = Vector(orientation.get("x"), orientation.get("y"), orientation.get("z")) - return cls(pos=pos, rot=rot, ts=msg["data"]["header"]["stamp"]) + pos = [position.get("x"), position.get("y"), position.get("z")] + rot = [orientation.get("x"), orientation.get("y"), orientation.get("z")] + return cls(pos, rot, msg["data"]["header"]["stamp"]) def __repr__(self) -> str: return f"Odom ts({to_human_readable(self.ts)}) pos({self.pos}), rot({self.rot})" diff --git a/dimos/robot/unitree_webrtc/type/timeseries.py b/dimos/robot/unitree_webrtc/type/timeseries.py index 84d2910622..fc392bb621 100644 --- a/dimos/robot/unitree_webrtc/type/timeseries.py +++ b/dimos/robot/unitree_webrtc/type/timeseries.py @@ -40,8 +40,10 @@ def to_datetime(ts: EpochLike, tz: timezone = None) -> datetime: class Timestamped(ABC): """Abstract class for an event with a timestamp.""" - def __init__(self, timestamp: EpochLike): - self.ts = to_datetime(timestamp) + ts: datetime + + def __init__(self, ts: EpochLike): + self.ts = to_datetime(ts) class TEvent(Timestamped, Generic[PAYLOAD]): diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 5d92d61d97..231a4c799f 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -3,12 +3,14 @@ from dimos.robot.unitree_webrtc.type.map import Map from dimos.robot.unitree_webrtc.connection import WebRTCRobot from dimos.robot.global_planner.planner import AstarPlanner +from dimos.robot.local_planner.simple import SimplePlanner from dimos.utils.reactive import getter_streaming from dimos.robot.unitree.unitree_skills import MyUnitreeSkills from dimos.skills.skills import AbstractSkill, SkillLibrary import os from go2_webrtc_driver.constants import VUI_COLOR -from dimos.robot.local_planner import navigate_path_local +from dimos.robot.local_planner.vfh.vfh_local_planner import VFHPurePursuitPlanner +from dimos.robot.local_planner.vfh.local_planner import navigate_path_local class Color(VUI_COLOR): ... @@ -26,17 +28,45 @@ def __init__( super().__init__(ip=ip, mode=mode) self.odom = getter_streaming(self.odom_stream()) - self.map = Map() + + self.map = Map(voxel_size=1) self.map_stream = self.map.consume(self.lidar_stream()) + self.local_planner = SimplePlanner( + get_costmap=lambda: self.map.costmap, get_robot_pos=lambda: self.odom() + ) + + def set_goal(path, *args, **kwargs): + self.local_planner.set_goal(path.last()) + self.global_planner = AstarPlanner( - set_local_nav=lambda path, stop_event=None, goal_theta=None: navigate_path_local( - self, path, timeout=120.0, goal_theta=goal_theta, stop_event=stop_event - ), + set_local_nav=set_goal, get_costmap=lambda: self.map.costmap, get_robot_pos=lambda: self.odom().pos, ) + self.local_planner.get_move_stream().subscribe(self.move) + + # #self.local_planner.get_move_stream().subscribe(on_next=self.move, on_error=print) + # self.local_planner = VFHPurePursuitPlanner( + # get_costmap=lambda: self.map.costmap, + # get_robot_pose=lambda: self.odom(), + # move=self.move, + # robot_width=0.36, # Unitree Go2 width in meters + # robot_length=0.6, # Unitree Go2 length in meters + # max_linear_vel=0.5, + # lookahead_distance=2.0, + # visualization_size=500, # 500x500 pixel visualization + # ) + + # self.global_planner = AstarPlanner( + # set_local_nav=lambda path, stop_event=None, goal_theta=None: navigate_path_local( + # self, path, timeout=120.0, goal_theta=goal_theta, stop_event=stop_event + # ), + # get_costmap=lambda: self.map.costmap, + # get_robot_pos=lambda: self.odom().pos, + # ) + # # Initialize skills # if skills is None: # skills = MyUnitreeSkills(robot=self) diff --git a/dimos/types/vector.py b/dimos/types/vector.py index 8d3ae1ef91..d5fcd08165 100644 --- a/dimos/types/vector.py +++ b/dimos/types/vector.py @@ -107,10 +107,6 @@ def serialize(self) -> Tuple: """Serialize the vector to a tuple.""" return {"type": "vector", "c": self._data.tolist()} - def __len__(self) -> int: - """Return the dimension of the vector.""" - return len(self._data) - def __eq__(self, other) -> bool: """Check if two vectors are equal using numpy's allclose for floating point comparison.""" if not isinstance(other, Vector): diff --git a/dimos/web/dimos_interface/tsconfig.json b/dimos/web/dimos_interface/tsconfig.json index 27ffaec6be..4bf29f39d2 100644 --- a/dimos/web/dimos_interface/tsconfig.json +++ b/dimos/web/dimos_interface/tsconfig.json @@ -17,8 +17,6 @@ "src/**/*.js", "src/**/*.svelte" ], - - "references": [ { "path": "./tsconfig.node.json" diff --git a/dimos/web/websocket_vis/server.py b/dimos/web/websocket_vis/server.py index ea9fbb00d5..cee6978fb6 100644 --- a/dimos/web/websocket_vis/server.py +++ b/dimos/web/websocket_vis/server.py @@ -91,7 +91,7 @@ async def update_state(new_data): class WebsocketVis: - def __init__(self, port=7778, use_reload=False, msg_handler=None): + def __init__(self, port=7779, use_reload=False, msg_handler=None): self.port = port self.server = None self.server_thread = None diff --git a/tests/run_webrtc.py b/tests/run_webrtc.py index ff96bed14e..fa84d800ae 100644 --- a/tests/run_webrtc.py +++ b/tests/run_webrtc.py @@ -11,20 +11,16 @@ # 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 cv2 import os -import asyncio +import threading +import time + +import reactivex.operators as ops from dotenv import load_dotenv -from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2, Color -from dimos.robot.unitree_webrtc.testing.helpers import show3d_stream -from dimos.web.websocket_vis.server import WebsocketVis + +from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 from dimos.types.vector import Vector -import logging -import open3d as o3d -import reactivex.operators as ops -import numpy as np -import time -import threading +from dimos.web.websocket_vis.server import WebsocketVis # logging.basicConfig(level=logging.DEBUG) @@ -39,6 +35,7 @@ def msg_handler(msgtype, data): if msgtype == "click": try: + print(data) robot.global_planner.set_goal(Vector(data["position"])) except Exception as e: print(f"Error setting goal: {e}") @@ -55,6 +52,7 @@ def threaded_msg_handler(msgtype, data): print("standing up") robot.standup() + print("robot is up") @@ -65,11 +63,29 @@ def newmap(msg): websocket_vis.connect(robot.map_stream.pipe(ops.map(newmap))) websocket_vis.connect(robot.odom_stream().pipe(ops.map(lambda pos: ["robot_pos", pos.pos.to_2d()]))) -try: - while True: - # robot.move_vel(Vector(0.1, 0.1, 0.1)) - time.sleep(0.01) +# Keep the script running and handle graceful shutdown +shutdown_event = threading.Event() + + +# def signal_handler(signum, frame): +# print("\nShutdown signal received. Cleaning up...") +# shutdown_event.set() + +# # Register signal handlers for graceful shutdown +# signal.signal(signal.SIGINT, signal_handler) +# signal.signal(signal.SIGTERM, signal_handler) + + +print("Robot is running. Press Ctrl+C to stop.") + +try: + # Keep the main thread alive while the robot loop runs in the background + while not shutdown_event.is_set(): + time.sleep(0.1) except KeyboardInterrupt: - print("Stopping robot") + print("\nKeyboard interrupt received. Shutting down...") +finally: + print("Stopping robot...") robot.liedown() + print("Robot stopped.") From bfec90a5be6e0d8b6d703039dad53e98b35083b7 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 16:59:18 +0300 Subject: [PATCH 49/72] odometry stream test --- .../unitree_webrtc/type/test_odometry.py | 48 +++++++++++++++++-- .../data/.lfs/raw_odometry_rotate_walk.tar.gz | 3 ++ 2 files changed, 46 insertions(+), 5 deletions(-) create mode 100644 tests/data/.lfs/raw_odometry_rotate_walk.tar.gz diff --git a/dimos/robot/unitree_webrtc/type/test_odometry.py b/dimos/robot/unitree_webrtc/type/test_odometry.py index 6e23f034c9..64662a1921 100644 --- a/dimos/robot/unitree_webrtc/type/test_odometry.py +++ b/dimos/robot/unitree_webrtc/type/test_odometry.py @@ -1,10 +1,48 @@ import pytest from dimos.robot.unitree_webrtc.type.odometry import Odometry from dimos.robot.unitree_webrtc.testing.multimock import Multimock +from dimos.utils.testing import SensorReplay -@pytest.mark.needsdata -def test_odometry_time(): - (timestamp, odom_raw) = Multimock("athens_odom").load_one(33) - print("RAW MSG", odom_raw) - print(Odometry.from_msg(odom_raw)) +@pytest.mark.tool +def test_store_odometry_stream(): + import os + import threading + import time + from dotenv import load_dotenv + from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 + from dimos.utils.testing import SensorStorage + + load_dotenv() + + robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") + robot.standup() + + def streamlogger(element): + print(element) + return element + + SensorStorage("raw_odometry_rotate_walk").save_stream(robot.raw_odom_stream()).subscribe(print) + + shutdown_event = threading.Event() + print("Robot is running. Press Ctrl+C to stop.") + + try: + # Keep the main thread alive while the robot loop runs in the background + while not shutdown_event.is_set(): + time.sleep(0.1) + except KeyboardInterrupt: + print("\nKeyboard interrupt received. Shutting down...") + finally: + print("Stopping robot...") + robot.liedown() + print("Robot stopped.") + + +def test_odometry_stream(): + counter = 0 + for message in SensorReplay( + name="raw_odometry_rotate_walk", autocast=lambda x: Odometry.from_msg(x) + ).iterate(): + counter += 1 + print(message) diff --git a/tests/data/.lfs/raw_odometry_rotate_walk.tar.gz b/tests/data/.lfs/raw_odometry_rotate_walk.tar.gz new file mode 100644 index 0000000000..ce8bb1d2b0 --- /dev/null +++ b/tests/data/.lfs/raw_odometry_rotate_walk.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:396345f0cd7a94bb9d85540d4bbce01b027618972f83e713e4550abf1d6ec445 +size 15685 From 7927d0a1cc4f41510b5319a6a0c960be1378f71e Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 20:09:26 +0300 Subject: [PATCH 50/72] sensor storage class, odometry tests --- dimos/robot/unitree_webrtc/type/odometry.py | 41 +++- .../unitree_webrtc/type/test_odometry.py | 191 +++++++++++++++++- dimos/utils/test_testing.py | 3 +- dimos/utils/testing.py | 67 ++++-- 4 files changed, 275 insertions(+), 27 deletions(-) diff --git a/dimos/robot/unitree_webrtc/type/odometry.py b/dimos/robot/unitree_webrtc/type/odometry.py index 4e7a293076..50e1586008 100644 --- a/dimos/robot/unitree_webrtc/type/odometry.py +++ b/dimos/robot/unitree_webrtc/type/odometry.py @@ -1,15 +1,14 @@ -from typing import TypedDict, Literal +import math from datetime import datetime -from dataclasses import dataclass -from dimos.types.vector import VectorLike, Vector -from dimos.types.position import Position +from typing import Literal, TypedDict + from dimos.robot.unitree_webrtc.type.timeseries import ( - Timestamped, - to_human_readable, - to_datetime, EpochLike, + to_datetime, + to_human_readable, ) - +from dimos.types.position import Position +from dimos.types.vector import VectorLike raw_odometry_msg_sample = { "type": "msg", @@ -68,14 +67,36 @@ def __init__(self, pos: VectorLike, rot: VectorLike, ts: EpochLike): super().__init__(pos, rot) self.ts = to_datetime(ts) if ts else datetime.now() + @staticmethod + def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: + """Convert quaternion to yaw angle (rotation around z-axis) in radians.""" + # Calculate yaw (rotation around z-axis) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = math.atan2(siny_cosp, cosy_cosp) + return yaw + @classmethod def from_msg(cls, msg: RawOdometryMessage) -> "Odometry": pose = msg["data"]["pose"] orientation = pose["orientation"] position = pose["position"] + + # Extract position pos = [position.get("x"), position.get("y"), position.get("z")] - rot = [orientation.get("x"), orientation.get("y"), orientation.get("z")] + + # Extract quaternion components + qx = orientation.get("x") + qy = orientation.get("y") + qz = orientation.get("z") + qw = orientation.get("w") + + # Convert quaternion to yaw angle and store in rot.z + # Keep x,y as quaternion components for now, but z becomes the actual yaw angle + yaw_radians = cls.quaternion_to_yaw(qx, qy, qz, qw) + rot = [qx, qy, yaw_radians] + return cls(pos, rot, msg["data"]["header"]["stamp"]) def __repr__(self) -> str: - return f"Odom ts({to_human_readable(self.ts)}) pos({self.pos}), rot({self.rot})" + return f"Odom ts({to_human_readable(self.ts)}) pos({self.pos}), rot({self.rot}) yaw({math.degrees(self.rot.z):.1f}°)" diff --git a/dimos/robot/unitree_webrtc/type/test_odometry.py b/dimos/robot/unitree_webrtc/type/test_odometry.py index 64662a1921..4a48432257 100644 --- a/dimos/robot/unitree_webrtc/type/test_odometry.py +++ b/dimos/robot/unitree_webrtc/type/test_odometry.py @@ -1,15 +1,120 @@ +import math + import pytest + from dimos.robot.unitree_webrtc.type.odometry import Odometry -from dimos.robot.unitree_webrtc.testing.multimock import Multimock from dimos.utils.testing import SensorReplay +def quaternion_to_euler_z(x, y, z, w): + """Convert quaternion to Euler angles and return the Z (yaw) angle in radians.""" + # Calculate yaw (rotation around z-axis) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = math.atan2(siny_cosp, cosy_cosp) + return yaw + + +def test_calculate_rotation_degrees(): + """Calculate total degrees of rotation change from the odometry stream.""" + messages = list( + SensorReplay( + name="raw_odometry_rotate_walk", + autocast=lambda x: x, # Keep raw messages + ).iterate() + ) + + if not messages: + print("No messages found in the replay data") + return + + print(f"Found {len(messages)} odometry messages") + + # Extract yaw angles from quaternions + yaw_angles = [] + timestamps = [] + + for msg in messages: + orientation = msg["data"]["pose"]["orientation"] + x, y, z, w = orientation["x"], orientation["y"], orientation["z"], orientation["w"] + + # Convert quaternion to yaw angle (rotation around z-axis) + yaw = quaternion_to_euler_z(x, y, z, w) + yaw_angles.append(yaw) + + # Extract timestamp + stamp = msg["data"]["header"]["stamp"] + timestamp = stamp["sec"] + stamp["nanosec"] / 1e9 + timestamps.append(timestamp) + + if len(yaw_angles) < 2: + print("Need at least 2 messages to calculate rotation change") + return + + # Calculate cumulative rotation change + total_rotation_radians = 0 + rotation_changes = [] + + for i in range(1, len(yaw_angles)): + # Calculate the difference, handling wrap-around at ±π + diff = yaw_angles[i] - yaw_angles[i - 1] + + # Normalize the difference to [-π, π] + while diff > math.pi: + diff -= 2 * math.pi + while diff < -math.pi: + diff += 2 * math.pi + + rotation_changes.append(diff) + total_rotation_radians += diff + + # Convert to degrees + total_rotation_degrees = math.degrees(total_rotation_radians) + + # Calculate time span + time_span = timestamps[-1] - timestamps[0] + + # Statistics + rotation_changes_degrees = [math.degrees(r) for r in rotation_changes] + max_rotation_rate = ( + max(abs(r) for r in rotation_changes_degrees) if rotation_changes_degrees else 0 + ) + avg_rotation_rate = ( + sum(abs(r) for r in rotation_changes_degrees) / len(rotation_changes_degrees) + if rotation_changes_degrees + else 0 + ) + + print("\n=== ROTATION ANALYSIS ===") + print(f"Total rotation change: {total_rotation_degrees:.2f} degrees") + print(f"Time span: {time_span:.2f} seconds") + print(f"Average rotation rate: {avg_rotation_rate:.2f} degrees per step") + print(f"Maximum rotation rate: {max_rotation_rate:.2f} degrees per step") + print(f"Number of rotation steps: {len(rotation_changes)}") + + if abs(total_rotation_degrees) > 360: + full_rotations = abs(total_rotation_degrees) // 360 + remaining_degrees = abs(total_rotation_degrees) % 360 + direction = "clockwise" if total_rotation_degrees < 0 else "counter-clockwise" + print( + f"That's {full_rotations:.0f} full rotation(s) + {remaining_degrees:.2f}° {direction}!" + ) + + # Show first few and last few rotation changes for inspection + print(f"\nFirst 5 rotation changes (degrees): {rotation_changes_degrees[:5]}") + print(f"Last 5 rotation changes (degrees): {rotation_changes_degrees[-5:]}") + + return total_rotation_degrees + + @pytest.mark.tool def test_store_odometry_stream(): import os import threading import time + from dotenv import load_dotenv + from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 from dimos.utils.testing import SensorStorage @@ -46,3 +151,87 @@ def test_odometry_stream(): ).iterate(): counter += 1 print(message) + + +def test_updated_odometry_rotation(): + """Test that the updated Odometry class now provides correct yaw angles in rot.z.""" + print("Testing updated Odometry class with proper quaternion conversion...") + + # Test with a few messages + messages = list( + SensorReplay( + name="raw_odometry_rotate_walk", autocast=lambda x: Odometry.from_msg(x) + ).iterate() + ) + + if len(messages) < 10: + print("Need at least 10 messages for testing") + return + + print("\nFirst 5 Odometry objects with corrected rotation:") + for i, odom in enumerate(messages[:5]): + yaw_degrees = math.degrees(odom.rot.z) + print(f" {i + 1}: {odom}") + + # Calculate total rotation using the new rot.z values + total_rotation_radians = 0 + rotation_changes = [] + + for i in range(1, len(messages)): + # Now we can directly use rot.z which contains the yaw angle + diff = messages[i].rot.z - messages[i - 1].rot.z + + # Normalize the difference to [-π, π] + while diff > math.pi: + diff -= 2 * math.pi + while diff < -math.pi: + diff += 2 * math.pi + + rotation_changes.append(diff) + total_rotation_radians += diff + + total_rotation_degrees = math.degrees(total_rotation_radians) + + print("\n=== SIMPLIFIED ROTATION ANALYSIS ===") + print(f"Total rotation using Odometry.rot.z: {total_rotation_degrees:.2f} degrees") + print(f"Number of messages: {len(messages)}") + print(f"First few rotation changes: {[math.degrees(r) for r in rotation_changes[:5]]}") + + return total_rotation_degrees + + +def test_compare_rotation_methods(): + """Compare quaternion-based rotation calculation with Odometry.rot values.""" + messages = list( + SensorReplay( + name="raw_odometry_rotate_walk", + autocast=lambda x: x, # Keep raw messages + ).iterate() + ) + + if not messages: + print("No messages found in the replay data") + return + + print("Analyzing first 10 messages to compare rotation methods...") + + for i, msg in enumerate(messages[:10]): + # Extract quaternion from raw message + orientation = msg["data"]["pose"]["orientation"] + qx, qy, qz, qw = orientation["x"], orientation["y"], orientation["z"], orientation["w"] + + # Convert to Odometry object (which only uses x,y,z components) + odom = Odometry.from_msg(msg) + + # Calculate yaw from full quaternion + yaw_from_quaternion = quaternion_to_euler_z(qx, qy, qz, qw) + yaw_degrees = math.degrees(yaw_from_quaternion) + + # Show what Odometry.rot contains (just x,y,z components of quaternion) + rot_vector = odom.rot + + print(f"\nMessage {i + 1}:") + print(f" Full quaternion: x={qx:.6f}, y={qy:.6f}, z={qz:.6f}, w={qw:.6f}") + print(f" Odometry.rot (x,y,z only): {rot_vector}") + print(f" Yaw from quaternion: {yaw_degrees:.2f}°") + print(f" Z-component magnitude: {abs(qz):.6f}") diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index 796c190c8a..0c2e4621e5 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -1,8 +1,9 @@ import hashlib import os import subprocess -from dimos.utils import testing + from dimos.robot.unitree_webrtc.type.lidar import LidarMessage +from dimos.utils import testing def test_pull_file(): diff --git a/dimos/utils/testing.py b/dimos/utils/testing.py index bd9a37d00a..e6585130e7 100644 --- a/dimos/utils/testing.py +++ b/dimos/utils/testing.py @@ -1,14 +1,14 @@ -import subprocess -import tarfile import glob import os import pickle +import subprocess +import tarfile from functools import cache from pathlib import Path -from typing import Union, Iterator, TypeVar, Generic, Optional, Any, Type, Callable +from typing import Any, Callable, Generic, Iterator, Optional, TypeVar, Union +from reactivex import from_iterable, interval from reactivex import operators as ops -from reactivex import interval, from_iterable from reactivex.observable import Observable @@ -164,7 +164,6 @@ class SensorReplay(Generic[T]): def __init__(self, name: str, autocast: Optional[Callable[[Any], T]] = None): self.root_dir = testData(name) self.autocast = autocast - self.cnt = 0 def load(self, *names: Union[int, str]) -> Union[T, Any, list[T], list[Any]]: if len(names) == 1: @@ -198,27 +197,65 @@ def stream(self, rate_hz: float = 10.0) -> Observable[Union[T, Any]]: ops.map(lambda x: x[0] if isinstance(x, tuple) else x), ) + +class SensorStorage(Generic[T]): + """Generic sensor data storage utility. + + Creates a directory in the test data directory and stores pickled sensor data. + + Args: + name: The name of the storage directory + autocast: Optional function that takes data and returns a processed result before storage. + """ + + def __init__(self, name: str, autocast: Optional[Callable[[T], Any]] = None): + self.name = name + self.autocast = autocast + self.cnt = 0 + + # Create storage directory in the data dir + self.root_dir = _get_data_dir() / name + + # Check if directory exists and is not empty + if self.root_dir.exists(): + existing_files = list(self.root_dir.glob("*.pickle")) + if existing_files: + raise RuntimeError( + f"Storage directory '{name}' already exists and contains {len(existing_files)} files. " + f"Please use a different name or clean the directory first." + ) + else: + # Create the directory + self.root_dir.mkdir(parents=True, exist_ok=True) + def save_stream(self, observable: Observable[Union[T, Any]]) -> Observable[int]: + """Save an observable stream of sensor data to pickle files.""" return observable.pipe(ops.map(lambda frame: self.save_one(frame))) def save(self, *frames) -> int: - [self.save_one(frame) for frame in frames] + """Save one or more frames to pickle files.""" + for frame in frames: + self.save_one(frame) return self.cnt def save_one(self, frame) -> int: - file_name = f"/{self.cnt:03d}.pickle" - full_path = self.root_dir + file_name - - self.cnt += 1 + """Save a single frame to a pickle file.""" + file_name = f"{self.cnt:03d}.pickle" + full_path = self.root_dir / file_name - if os.path.isfile(full_path): - raise Exception(f"file {full_path} exists") + if full_path.exists(): + raise RuntimeError(f"File {full_path} already exists") + # Apply autocast if provided + data_to_save = frame + if self.autocast: + data_to_save = self.autocast(frame) # Convert to raw message if frame has a raw_msg attribute - if hasattr(frame, "raw_msg"): - frame = frame.raw_msg + elif hasattr(frame, "raw_msg"): + data_to_save = frame.raw_msg with open(full_path, "wb") as f: - pickle.dump(frame, f) + pickle.dump(data_to_save, f) + self.cnt += 1 return self.cnt From 0f29b6f84959f8e4e183ad8a16438b9c8aef02f9 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 20:09:47 +0300 Subject: [PATCH 51/72] move cmd vector info --- dimos/robot/unitree_webrtc/connection.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/dimos/robot/unitree_webrtc/connection.py b/dimos/robot/unitree_webrtc/connection.py index c873a79792..dd1359a68b 100644 --- a/dimos/robot/unitree_webrtc/connection.py +++ b/dimos/robot/unitree_webrtc/connection.py @@ -61,6 +61,9 @@ def start_background_loop(): self.connection_ready.wait() def move(self, vector: Vector): + # x - Positive right, negative left + # y - positive forward, negative backwards + # z - Positive rotate right, negative rotate left async def async_move(): self.conn.datachannel.pub_sub.publish_without_callback( RTC_TOPIC["WIRELESS_CONTROLLER"], From bea9447f15dfe522685eb72d6bb3ff8e7f597555 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 20:09:59 +0300 Subject: [PATCH 52/72] simple planner experiments --- dimos/robot/local_planner/simple.py | 125 ++++++++++---- dimos/robot/local_planner/test_simple.py | 191 ---------------------- dimos/robot/unitree_webrtc/unitree_go2.py | 4 +- 3 files changed, 94 insertions(+), 226 deletions(-) delete mode 100644 dimos/robot/local_planner/test_simple.py diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index e9a920ecb3..5b8443b1f7 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -1,28 +1,57 @@ import math +import time from dataclasses import dataclass from typing import Callable, Optional import reactivex as rx -from reactivex import operators as ops from plum import dispatch +from reactivex import operators as ops from dimos.robot.local_planner.local_planner import LocalPlanner from dimos.types.costmap import Costmap -from dimos.types.position import Position from dimos.types.path import Path +from dimos.types.position import Position from dimos.types.vector import Vector, VectorLike, to_vector -from dimos.utils.threadpool import get_scheduler from dimos.utils.logging_config import setup_logger +from dimos.utils.threadpool import get_scheduler logger = setup_logger("dimos.robot.unitree.global_planner") +def transform_to_robot_frame(global_vector: Vector, robot_position: Position) -> Vector: + """Transform a global coordinate vector to robot-relative coordinates. + + Args: + global_vector: Vector in global coordinates + robot_position: Robot's position and orientation + + Returns: + Vector in robot coordinates where X is forward/backward, Y is left/right + """ + # Get the robot's yaw angle (rotation around Z-axis) + robot_yaw = robot_position.rot.z + + # Create rotation matrix to transform from global to robot frame + # We need to rotate the coordinate system by -robot_yaw to get robot-relative coordinates + cos_yaw = math.cos(-robot_yaw) + sin_yaw = math.sin(-robot_yaw) + + # Apply 2D rotation transformation + # This transforms a global direction vector into the robot's coordinate frame + # In robot frame: X=forward/backward, Y=left/right + # In global frame: X=east/west, Y=north/south + robot_x = global_vector.x * cos_yaw - global_vector.y * sin_yaw # Forward/backward + robot_y = global_vector.x * sin_yaw + global_vector.y * cos_yaw # Left/right + + return Vector(robot_y, robot_x, 0) + + @dataclass class SimplePlanner(LocalPlanner): get_costmap: Callable[[], Costmap] get_robot_pos: Callable[[], Position] goal: Optional[Vector] = None - speed: float = 0.2 + speed: float = 0.3 @dispatch def set_goal(self, goal: Path, stop_event=None, goal_theta=None) -> bool: @@ -36,29 +65,6 @@ def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: logger.info(f"Setting goal: {self.goal}") return True - def _transform_to_robot_frame(self, global_vector: Vector, robot_position: Position) -> Vector: - """Transform a global coordinate vector to robot-relative coordinates. - - Args: - global_vector: Vector in global coordinates - robot_position: Robot's position and orientation - - Returns: - Vector in robot coordinates where X is forward/backward, Y is left/right - """ - # Get the robot's yaw angle (rotation around Z-axis) - robot_yaw = robot_position.rot.z - - # Create rotation matrix to transform from global to robot frame - cos_yaw = math.cos(-robot_yaw) # Negative because we're rotating coordinate frame - sin_yaw = math.sin(-robot_yaw) - - # Apply 2D rotation transformation - robot_x = global_vector.x * cos_yaw - global_vector.y * sin_yaw # Forward/backward - robot_y = global_vector.x * sin_yaw + global_vector.y * cos_yaw # Left/right - - return Vector(robot_x, robot_y, global_vector.z) - def calc_move(self, direction: Vector) -> Vector: """Calculate the movement vector based on the direction to the goal. @@ -89,14 +95,67 @@ def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: # do we have a goal? ops.filter(lambda _: self.goal is not None), # calculate direction vector + self.spy("goal"), ops.map(lambda _: self.get_robot_pos().pos.to_2d() - self.goal), ops.filter(lambda direction: direction.length() > 0.1), # ops.map(self.calc_move), self.spy("direction"), - ops.map( - lambda direction: self._transform_to_robot_frame( - direction.normalize() * self.speed, - self.get_robot_pos(), - ) - ), + # ops.map( + # lambda direction: transform_to_robot_frame( + # direction.normalize() * self.speed, + # self.get_robot_pos(), + # ) + # ), + ops.map(self.rotate_to_target), + ) + + def rotate_to_target(self, direction_to_goal: Vector) -> Vector: + """Calculate the rotation needed for the robot to face the target. + + Args: + direction_to_goal: Vector pointing from robot position to goal in global coordinates + + Returns: + Vector with (x=0, y=0, z=angular_velocity) for rotation only + """ + # Calculate the desired yaw angle to face the target + desired_yaw = math.atan2(direction_to_goal.y, direction_to_goal.x) + + # Get current robot yaw + current_yaw = self.get_robot_pos().rot.z + + # Calculate the yaw error using a more robust method to avoid oscillation + yaw_error = math.atan2( + math.sin(desired_yaw - current_yaw), math.cos(desired_yaw - current_yaw) ) + + print( + f"DEBUG: direction_to_goal={direction_to_goal}, desired_yaw={math.degrees(desired_yaw):.1f}°, current_yaw={math.degrees(current_yaw):.1f}°" + ) + print( + f"DEBUG: yaw_error={math.degrees(yaw_error):.1f}°, abs_error={abs(yaw_error):.3f}, tolerance=0.1" + ) + + # Calculate angular velocity (proportional control) + max_angular_speed = 0.3 # rad/s + raw_angular_velocity = yaw_error * 2.0 + angular_velocity = max(-max_angular_speed, min(max_angular_speed, raw_angular_velocity)) + + print( + f"DEBUG: raw_ang_vel={raw_angular_velocity:.3f}, clamped_ang_vel={angular_velocity:.3f}" + ) + + # Stop rotating if we're close enough to the target angle + if abs(yaw_error) < 0.1: # ~5.7 degrees tolerance + print("DEBUG: Within tolerance - stopping rotation") + angular_velocity = 0.0 + else: + print("DEBUG: Outside tolerance - continuing rotation") + + print( + f"Rotation control: current_yaw={math.degrees(current_yaw):.1f}°, desired_yaw={math.degrees(desired_yaw):.1f}°, error={math.degrees(yaw_error):.1f}°, ang_vel={angular_velocity:.3f}" + ) + + # Return movement command: no translation (x=0, y=0), only rotation (z=angular_velocity) + # Try flipping the sign in case the rotation convention is opposite + return Vector(0, 0, -angular_velocity) diff --git a/dimos/robot/local_planner/test_simple.py b/dimos/robot/local_planner/test_simple.py deleted file mode 100644 index 1883b0948f..0000000000 --- a/dimos/robot/local_planner/test_simple.py +++ /dev/null @@ -1,191 +0,0 @@ -import math -import time -from unittest.mock import Mock - -import pytest -import reactivex as rx -from reactivex import operators as ops - -from dimos.robot.local_planner.simple import SimplePlanner -from dimos.types.position import Position -from dimos.types.vector import Vector - - -class TestSimplePlanner: - """Test suite for SimplePlanner class.""" - - @pytest.fixture - def mock_get_costmap(self): - """Mock costmap getter function.""" - return Mock(return_value=None) - - @pytest.fixture - def mock_get_robot_pos(self): - """Mock robot position getter function.""" - return Mock(return_value=Position(Vector(0, 0, 0))) - - @pytest.fixture - def planner(self, mock_get_costmap, mock_get_robot_pos): - """Create a SimplePlanner instance with mocked dependencies.""" - return SimplePlanner(get_costmap=mock_get_costmap, get_robot_pos=mock_get_robot_pos) - - def test_initialization(self, planner, mock_get_costmap, mock_get_robot_pos): - """Test that SimplePlanner initializes correctly.""" - assert planner.get_costmap == mock_get_costmap - assert planner.get_robot_pos == mock_get_robot_pos - assert planner.goal is None - - def test_set_goal_with_vector(self, planner): - """Test setting goal with a Vector object.""" - goal = Vector(1, 2, 0) - result = planner.set_goal(goal) - - assert result is True - assert planner.goal == goal - - def test_get_move_stream_returns_observable(self, planner): - """Test that get_move_stream returns an Observable.""" - move_stream = planner.get_move_stream(frequency=10.0) - assert isinstance(move_stream, rx.Observable) - - def test_get_move_stream_with_no_goal(self, planner): - """Test that move stream doesn't emit when no goal is set.""" - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1), ops.timeout(0.5)).subscribe( - on_next=lambda x: emissions.append(x), - on_error=lambda e: None, # Ignore timeout errors - ) - - time.sleep(0.6) - assert len(emissions) == 0 - - def test_get_move_stream_no_rotation(self, planner, mock_get_robot_pos): - """Test movement with robot facing forward (no rotation).""" - # Robot at origin facing forward (no rotation), goal ahead - mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, 0)) - planner.set_goal(Vector(1, 0, 0)) # Goal directly ahead - - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) - - time.sleep(0.5) - - assert len(emissions) == 1 - move_command = emissions[0] - assert isinstance(move_command, Vector) - - # Should move forward (positive X in robot frame) - assert abs(move_command.x - 0.2) < 0.001 # Forward - assert abs(move_command.y) < 0.001 # No left/right movement - - def test_get_move_stream_with_rotation_90_degrees(self, planner, mock_get_robot_pos): - """Test movement with robot rotated 90 degrees (facing left in global frame).""" - # Robot facing left (90 degrees rotation), goal to the right in global frame - mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, math.pi / 2)) - planner.set_goal(Vector(1, 0, 0)) # Goal to the right in global frame - - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) - - time.sleep(0.5) - - assert len(emissions) == 1 - move_command = emissions[0] - - # Goal is to the right in global frame, but robot is facing left - # So in robot frame, this should be a right turn (negative Y) - assert abs(move_command.x) < 0.001 # No forward/backward - assert abs(move_command.y + 0.2) < 0.001 # Right turn (negative Y) - - def test_get_move_stream_with_rotation_180_degrees(self, planner, mock_get_robot_pos): - """Test movement with robot rotated 180 degrees (facing backward).""" - # Robot facing backward, goal behind in global frame (which is forward for robot) - mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, math.pi)) - planner.set_goal(Vector(-1, 0, 0)) # Goal behind in global frame - - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) - - time.sleep(0.5) - - assert len(emissions) == 1 - move_command = emissions[0] - - # Goal is behind in global frame, but robot is facing backward - # So in robot frame, this should be forward movement - assert abs(move_command.x - 0.2) < 0.001 # Forward - assert abs(move_command.y) < 0.001 # No left/right - - def test_get_move_stream_diagonal_movement(self, planner, mock_get_robot_pos): - """Test diagonal movement with no robot rotation.""" - # Robot at origin, goal at (1,1) - should move diagonally - mock_get_robot_pos.return_value = Position(Vector(0, 0, 0), Vector(0, 0, 0)) - planner.set_goal(Vector(1, 1, 0)) - - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) - - time.sleep(0.5) - - assert len(emissions) == 1 - move_command = emissions[0] - - # Should be normalized direction vector * 0.2 speed - # Direction from (0,0) to (1,1) normalized is approximately (0.707, 0.707) - # Multiplied by 0.2 gives approximately (0.141, 0.141) - assert abs(move_command.x - 0.14142136) < 0.001 - assert abs(move_command.y - 0.14142136) < 0.001 - - def test_transform_to_robot_frame_no_rotation(self, planner): - """Test coordinate transformation with no robot rotation.""" - robot_pos = Position(Vector(0, 0, 0), Vector(0, 0, 0)) # No rotation - global_vec = Vector(1, 0, 0) # Forward in global frame - - result = planner._transform_to_robot_frame(global_vec, robot_pos) - - # With no rotation, global and robot frames should be the same - assert abs(result.x - 1.0) < 0.001 - assert abs(result.y) < 0.001 - - def test_transform_to_robot_frame_90_degree_rotation(self, planner): - """Test coordinate transformation with 90-degree robot rotation.""" - robot_pos = Position(Vector(0, 0, 0), Vector(0, 0, math.pi / 2)) # 90 degrees - global_vec = Vector(1, 0, 0) # Forward in global frame - - result = planner._transform_to_robot_frame(global_vec, robot_pos) - - # Robot is facing left, so global forward becomes robot right (negative Y) - assert abs(result.x) < 0.001 # No forward/backward - assert abs(result.y + 1.0) < 0.001 # Right turn (negative Y) - - def test_goal_at_robot_position(self, planner, mock_get_robot_pos): - """Test behavior when goal is at current robot position.""" - position = Position(Vector(1, 1, 0), Vector(0, 0, 0)) - mock_get_robot_pos.return_value = position - planner.set_goal(Vector(1, 1, 0)) - - move_stream = planner.get_move_stream(frequency=10.0) - - emissions = [] - move_stream.pipe(ops.take(1)).subscribe(on_next=lambda x: emissions.append(x)) - - time.sleep(0.3) - - assert len(emissions) == 1 - move_command = emissions[0] - - # When goal equals current position, difference is zero - # Normalizing zero vector should result in zero vector - # So movement should be zero - assert abs(move_command.x) < 0.001 - assert abs(move_command.y) < 0.001 diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 231a4c799f..de596f2149 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -33,7 +33,7 @@ def __init__( self.map_stream = self.map.consume(self.lidar_stream()) self.local_planner = SimplePlanner( - get_costmap=lambda: self.map.costmap, get_robot_pos=lambda: self.odom() + get_costmap=lambda: self.map.costmap, get_robot_pos=self.odom ) def set_goal(path, *args, **kwargs): @@ -45,7 +45,7 @@ def set_goal(path, *args, **kwargs): get_robot_pos=lambda: self.odom().pos, ) - self.local_planner.get_move_stream().subscribe(self.move) + self.local_planner.get_move_stream().subscribe(on_next=self.move, on_error=print) # #self.local_planner.get_move_stream().subscribe(on_next=self.move, on_error=print) # self.local_planner = VFHPurePursuitPlanner( From b63d985669b70b807cbbbb572cfe40acc10515f3 Mon Sep 17 00:00:00 2001 From: lesh Date: Fri, 30 May 2025 21:15:40 +0300 Subject: [PATCH 53/72] working version --- dimos/robot/local_planner/simple.py | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index 5b8443b1f7..563ca3c16a 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -92,21 +92,11 @@ def spyfun(x): def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( - # do we have a goal? ops.filter(lambda _: self.goal is not None), - # calculate direction vector - self.spy("goal"), - ops.map(lambda _: self.get_robot_pos().pos.to_2d() - self.goal), - ops.filter(lambda direction: direction.length() > 0.1), - # ops.map(self.calc_move), - self.spy("direction"), - # ops.map( - # lambda direction: transform_to_robot_frame( - # direction.normalize() * self.speed, - # self.get_robot_pos(), - # ) - # ), + ops.map(lambda _: self.goal - self.get_robot_pos().pos.to_2d()), + ops.filter(lambda direction: direction.length() > 0.2), ops.map(self.rotate_to_target), + ops.map(lambda rot: rot + Vector(0, 0.3, 0)), ) def rotate_to_target(self, direction_to_goal: Vector) -> Vector: @@ -137,7 +127,7 @@ def rotate_to_target(self, direction_to_goal: Vector) -> Vector: ) # Calculate angular velocity (proportional control) - max_angular_speed = 0.3 # rad/s + max_angular_speed = 0.75 # rad/s raw_angular_velocity = yaw_error * 2.0 angular_velocity = max(-max_angular_speed, min(max_angular_speed, raw_angular_velocity)) From 25ec70a35a79a37be8d67ffdf957af2e6da62643 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 31 May 2025 15:16:58 +0300 Subject: [PATCH 54/72] working implementation of simple planner --- dimos/robot/local_planner/simple.py | 87 ++++++----------------------- dimos/types/position.py | 23 ++++++++ dimos/types/test_position.py | 8 ++- dimos/types/test_vector.py | 22 ++++++++ 4 files changed, 70 insertions(+), 70 deletions(-) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index 563ca3c16a..d5ba08de9d 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -1,5 +1,6 @@ import math import time +from datetime import datetime from dataclasses import dataclass from typing import Callable, Optional @@ -18,34 +19,6 @@ logger = setup_logger("dimos.robot.unitree.global_planner") -def transform_to_robot_frame(global_vector: Vector, robot_position: Position) -> Vector: - """Transform a global coordinate vector to robot-relative coordinates. - - Args: - global_vector: Vector in global coordinates - robot_position: Robot's position and orientation - - Returns: - Vector in robot coordinates where X is forward/backward, Y is left/right - """ - # Get the robot's yaw angle (rotation around Z-axis) - robot_yaw = robot_position.rot.z - - # Create rotation matrix to transform from global to robot frame - # We need to rotate the coordinate system by -robot_yaw to get robot-relative coordinates - cos_yaw = math.cos(-robot_yaw) - sin_yaw = math.sin(-robot_yaw) - - # Apply 2D rotation transformation - # This transforms a global direction vector into the robot's coordinate frame - # In robot frame: X=forward/backward, Y=left/right - # In global frame: X=east/west, Y=north/south - robot_x = global_vector.x * cos_yaw - global_vector.y * sin_yaw # Forward/backward - robot_y = global_vector.x * sin_yaw + global_vector.y * cos_yaw # Left/right - - return Vector(robot_y, robot_x, 0) - - @dataclass class SimplePlanner(LocalPlanner): get_costmap: Callable[[], Costmap] @@ -65,24 +38,6 @@ def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: logger.info(f"Setting goal: {self.goal}") return True - def calc_move(self, direction: Vector) -> Vector: - """Calculate the movement vector based on the direction to the goal. - - Args: - direction: Direction vector towards the goal - - Returns: - Movement vector scaled by speed - """ - try: - # Normalize the direction vector and scale by speed - normalized_direction = direction.normalize() - move_vector = normalized_direction * self.speed - print("CALC MOVE", direction, normalized_direction, move_vector) - return move_vector - except Exception as e: - print("Error calculating move vector:", e) - def spy(self, name: str): def spyfun(x): print(f"SPY {name}:", x) @@ -93,10 +48,11 @@ def spyfun(x): def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( ops.filter(lambda _: self.goal is not None), - ops.map(lambda _: self.goal - self.get_robot_pos().pos.to_2d()), - ops.filter(lambda direction: direction.length() > 0.2), - ops.map(self.rotate_to_target), - ops.map(lambda rot: rot + Vector(0, 0.3, 0)), + ops.map(lambda _: self.get_robot_pos()), + ops.map(lambda odom: odom.vector_to(self.goal)), + ops.filter(lambda direction: direction.length() > 0.1), + ops.map(lambda direction: direction.normalize() * self.speed), + ops.map(lambda direction: Vector(-direction.y, direction.x)), ) def rotate_to_target(self, direction_to_goal: Vector) -> Vector: @@ -112,39 +68,32 @@ def rotate_to_target(self, direction_to_goal: Vector) -> Vector: desired_yaw = math.atan2(direction_to_goal.y, direction_to_goal.x) # Get current robot yaw - current_yaw = self.get_robot_pos().rot.z + current_yaw = self.get_robot_pos().yaw # Calculate the yaw error using a more robust method to avoid oscillation yaw_error = math.atan2( math.sin(desired_yaw - current_yaw), math.cos(desired_yaw - current_yaw) ) - print( - f"DEBUG: direction_to_goal={direction_to_goal}, desired_yaw={math.degrees(desired_yaw):.1f}°, current_yaw={math.degrees(current_yaw):.1f}°" - ) - print( - f"DEBUG: yaw_error={math.degrees(yaw_error):.1f}°, abs_error={abs(yaw_error):.3f}, tolerance=0.1" - ) + # print( + # f"DEBUG: direction_to_goal={direction_to_goal}, desired_yaw={math.degrees(desired_yaw):.1f}°, current_yaw={math.degrees(current_yaw):.1f}°" + # ) + # print( + # f"DEBUG: yaw_error={math.degrees(yaw_error):.1f}°, abs_error={abs(yaw_error):.3f}, tolerance=0.1" + # ) # Calculate angular velocity (proportional control) - max_angular_speed = 0.75 # rad/s - raw_angular_velocity = yaw_error * 2.0 + max_angular_speed = 0.2 # rad/s + raw_angular_velocity = 100.0 / yaw_error angular_velocity = max(-max_angular_speed, min(max_angular_speed, raw_angular_velocity)) - print( - f"DEBUG: raw_ang_vel={raw_angular_velocity:.3f}, clamped_ang_vel={angular_velocity:.3f}" - ) - # Stop rotating if we're close enough to the target angle if abs(yaw_error) < 0.1: # ~5.7 degrees tolerance - print("DEBUG: Within tolerance - stopping rotation") angular_velocity = 0.0 - else: - print("DEBUG: Outside tolerance - continuing rotation") - print( - f"Rotation control: current_yaw={math.degrees(current_yaw):.1f}°, desired_yaw={math.degrees(desired_yaw):.1f}°, error={math.degrees(yaw_error):.1f}°, ang_vel={angular_velocity:.3f}" - ) + # print( + # f"Rotation control: current_yaw={math.degrees(current_yaw):.1f}°, desired_yaw={math.degrees(desired_yaw):.1f}°, error={math.degrees(yaw_error):.1f}°, ang_vel={angular_velocity:.3f}" + # ) # Return movement command: no translation (x=0, y=0), only rotation (z=angular_velocity) # Try flipping the sign in case the rotation convention is opposite diff --git a/dimos/types/position.py b/dimos/types/position.py index 37515d3990..69af640fed 100644 --- a/dimos/types/position.py +++ b/dimos/types/position.py @@ -15,6 +15,7 @@ from typing import TypeVar, Union, Sequence import numpy as np from plum import dispatch +import math from dimos.types.vector import Vector, to_vector, to_numpy, VectorLike @@ -24,6 +25,12 @@ PositionLike = Union["Position", VectorLike, Sequence[VectorLike]] +def yaw_to_matrix(yaw: float) -> np.ndarray: + """2-D yaw (about Z) to a 3×3 rotation matrix.""" + c, s = math.cos(yaw), math.sin(yaw) + return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + class Position(Vector): """A position in 3D space, consisting of a position vector and a rotation vector. @@ -59,6 +66,17 @@ def serialize(self): """Serialize the position to a dictionary.""" return {"type": "position", "pos": self.to_list(), "rot": self.rot.to_list()} + def vector_to(self, target: Vector) -> Vector: + direction = target - self.pos.to_2d() + + cos_y = math.cos(-self.yaw) + sin_y = math.sin(-self.yaw) + + x = cos_y * direction.x - sin_y * direction.y + y = sin_y * direction.x + cos_y * direction.y + + return Vector(x, y) + def __eq__(self, other) -> bool: """Check if two positions are equal using numpy's allclose for floating point comparison.""" if not isinstance(other, Position): @@ -93,6 +111,11 @@ def __add__(self: T, other) -> T: # For other types, just use Vector's addition return Position(super().__add__(other), self.rot) + @property + def yaw(self) -> float: + """Get the yaw (rotation around Z-axis) from the rotation vector.""" + return self.rot.z + def __sub__(self: T, other) -> T: """Override Vector's __sub__ to handle Position objects specially. diff --git a/dimos/types/test_position.py b/dimos/types/test_position.py index c2dec9bf5f..bf4e4cf4c8 100644 --- a/dimos/types/test_position.py +++ b/dimos/types/test_position.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. import numpy as np - +import math from dimos.types.position import Position, to_position from dimos.types.vector import Vector @@ -315,3 +315,9 @@ def test_to_position_with_sequence(): assert pos3.rot.x == 16.0 assert pos3.rot.y == 17.0 assert pos3.rot.z == 18.0 + + +def test_vector_transform(): + robot_position = Position(Vector(4.0, 2.0, 0.5), Vector(0.0, 0.0, math.pi / 2)) + target = Vector(1.0, 3.0, 0.0) + print(robot_position.vector_to(target)) diff --git a/dimos/types/test_vector.py b/dimos/types/test_vector.py index b448620145..f2a48be294 100644 --- a/dimos/types/test_vector.py +++ b/dimos/types/test_vector.py @@ -351,3 +351,25 @@ def test_vector_bool_conversion(): pass # Expected path else: assert False, "Non-zero vector should be True in boolean context" + + +def test_vector_add(): + """Test vector addition operator.""" + v1 = Vector(1.0, 2.0, 3.0) + v2 = Vector(4.0, 5.0, 6.0) + + # Using __add__ method + v_add = v1.__add__(v2) + assert v_add.x == 5.0 + assert v_add.y == 7.0 + assert v_add.z == 9.0 + + # Using + operator + v_add_op = v1 + v2 + assert v_add_op.x == 5.0 + assert v_add_op.y == 7.0 + assert v_add_op.z == 9.0 + + # Adding zero vector should return original vector + v_zero = Vector.zeros(3) + assert (v1 + v_zero) == v1 From 69da4bddde8e3668df8092e975fc4801d4cca481 Mon Sep 17 00:00:00 2001 From: lesh Date: Sat, 31 May 2025 18:47:04 +0300 Subject: [PATCH 55/72] working simple planner, still tuning --- dimos/robot/local_planner/simple.py | 105 +++++++++------------- dimos/robot/unitree_webrtc/unitree_go2.py | 8 +- dimos/types/test_vector.py | 9 ++ dimos/types/vector.py | 38 +++++++- dimos/utils/reactive.py | 8 ++ 5 files changed, 100 insertions(+), 68 deletions(-) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index d5ba08de9d..f022daf562 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -24,77 +24,58 @@ class SimplePlanner(LocalPlanner): get_costmap: Callable[[], Costmap] get_robot_pos: Callable[[], Position] goal: Optional[Vector] = None - speed: float = 0.3 + path: Optional[Path] = None + speed: float = 0.5 + slowdown_range = 0.3 - @dispatch - def set_goal(self, goal: Path, stop_event=None, goal_theta=None) -> bool: - self.goal = goal.last().to_2d() - logger.info(f"Setting goal: {self.goal}") - return True + def set_path(self, path: Path) -> bool: + self.path = path + if self.path: + self.set_goal(path.head()) - @dispatch def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: self.goal = to_vector(goal).to_2d() logger.info(f"Setting goal: {self.goal}") - return True - - def spy(self, name: str): - def spyfun(x): - print(f"SPY {name}:", x) - return x - return ops.map(spyfun) + def speed_ctrl(self, direction: Vector): + dist = direction.length() + if dist >= self.slowdown_range: + f = 1.0 + else: + f = dist / self.slowdown_range + + return self.speed * f + + def yaw_ctrl(self, direction, k_p: float = 1.0, omega_max: float = 0.2): + alpha = math.atan2(-direction.y, direction.x) + omega = max(-omega_max, min(omega_max, k_p * alpha)) + return Vector(0.0, 0.0, omega) + + def pop_path(self): + self.set_path(self.path.tail()) + return False + + # dumb path popping filter (needs change) + def filter(self, direction): + length = direction.length() + if self.path: + if length < 0.4: + self.pop_path() + return False + else: + if length < 0.1: + return False + + return direction def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( - ops.filter(lambda _: self.goal is not None), + ops.filter(lambda _: (self.goal is not None) or (self.path is not None)), ops.map(lambda _: self.get_robot_pos()), ops.map(lambda odom: odom.vector_to(self.goal)), - ops.filter(lambda direction: direction.length() > 0.1), - ops.map(lambda direction: direction.normalize() * self.speed), - ops.map(lambda direction: Vector(-direction.y, direction.x)), + ops.filter(self.filter), + # somewhere here we should detect potential obstacles in direction of travel + ops.map(lambda direction: direction.normalize() * self.speed_ctrl(direction)), + ops.map(lambda direction: direction), # + self.yaw_ctrl(direction)), + ops.map(lambda direction: Vector(-direction.y, direction.x, direction.z)), ) - - def rotate_to_target(self, direction_to_goal: Vector) -> Vector: - """Calculate the rotation needed for the robot to face the target. - - Args: - direction_to_goal: Vector pointing from robot position to goal in global coordinates - - Returns: - Vector with (x=0, y=0, z=angular_velocity) for rotation only - """ - # Calculate the desired yaw angle to face the target - desired_yaw = math.atan2(direction_to_goal.y, direction_to_goal.x) - - # Get current robot yaw - current_yaw = self.get_robot_pos().yaw - - # Calculate the yaw error using a more robust method to avoid oscillation - yaw_error = math.atan2( - math.sin(desired_yaw - current_yaw), math.cos(desired_yaw - current_yaw) - ) - - # print( - # f"DEBUG: direction_to_goal={direction_to_goal}, desired_yaw={math.degrees(desired_yaw):.1f}°, current_yaw={math.degrees(current_yaw):.1f}°" - # ) - # print( - # f"DEBUG: yaw_error={math.degrees(yaw_error):.1f}°, abs_error={abs(yaw_error):.3f}, tolerance=0.1" - # ) - - # Calculate angular velocity (proportional control) - max_angular_speed = 0.2 # rad/s - raw_angular_velocity = 100.0 / yaw_error - angular_velocity = max(-max_angular_speed, min(max_angular_speed, raw_angular_velocity)) - - # Stop rotating if we're close enough to the target angle - if abs(yaw_error) < 0.1: # ~5.7 degrees tolerance - angular_velocity = 0.0 - - # print( - # f"Rotation control: current_yaw={math.degrees(current_yaw):.1f}°, desired_yaw={math.degrees(desired_yaw):.1f}°, error={math.degrees(yaw_error):.1f}°, ang_vel={angular_velocity:.3f}" - # ) - - # Return movement command: no translation (x=0, y=0), only rotation (z=angular_velocity) - # Try flipping the sign in case the rotation convention is opposite - return Vector(0, 0, -angular_velocity) diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index de596f2149..13c5b32382 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -29,18 +29,18 @@ def __init__( self.odom = getter_streaming(self.odom_stream()) - self.map = Map(voxel_size=1) + self.map = Map(voxel_size=0.5) self.map_stream = self.map.consume(self.lidar_stream()) self.local_planner = SimplePlanner( get_costmap=lambda: self.map.costmap, get_robot_pos=self.odom ) - def set_goal(path, *args, **kwargs): - self.local_planner.set_goal(path.last()) + def set_path(path, *args, **kwargs): + self.local_planner.set_path(path) self.global_planner = AstarPlanner( - set_local_nav=set_goal, + set_local_nav=set_path, get_costmap=lambda: self.map.costmap, get_robot_pos=lambda: self.odom().pos, ) diff --git a/dimos/types/test_vector.py b/dimos/types/test_vector.py index f2a48be294..6a93d37afd 100644 --- a/dimos/types/test_vector.py +++ b/dimos/types/test_vector.py @@ -373,3 +373,12 @@ def test_vector_add(): # Adding zero vector should return original vector v_zero = Vector.zeros(3) assert (v1 + v_zero) == v1 + + +def test_vector_add_dim_mismatch(): + """Test vector addition operator.""" + v1 = Vector(1.0, 2.0) + v2 = Vector(4.0, 5.0, 6.0) + + # Using + operator + v_add_op = v1 + v2 diff --git a/dimos/types/vector.py b/dimos/types/vector.py index d5fcd08165..1348d03698 100644 --- a/dimos/types/vector.py +++ b/dimos/types/vector.py @@ -117,13 +117,35 @@ def __eq__(self, other) -> bool: def __add__(self: T, other) -> T: if isinstance(other, Vector): + # Handle different dimensions by padding the smaller vector + if len(self._data) != len(other._data): + max_dim = max(len(self._data), len(other._data)) + return self.pad(max_dim) + other.pad(max_dim) return self.__class__(self._data + other._data) - return self.__class__(self._data + np.array(other, dtype=float)) + + # For non-Vector types, convert to array and handle potential dimension mismatch + other_arr = np.array(other, dtype=float) + if len(self._data) != len(other_arr): + max_dim = max(len(self._data), len(other_arr)) + other_vec = self.__class__(other_arr) + return self.pad(max_dim) + other_vec.pad(max_dim) + return self.__class__(self._data + other_arr) def __sub__(self: T, other) -> T: if isinstance(other, Vector): + # Handle different dimensions by padding the smaller vector + if len(self._data) != len(other._data): + max_dim = max(len(self._data), len(other._data)) + return self.pad(max_dim) - other.pad(max_dim) return self.__class__(self._data - other._data) - return self.__class__(self._data - np.array(other, dtype=float)) + + # For non-Vector types, convert to array and handle potential dimension mismatch + other_arr = np.array(other, dtype=float) + if len(self._data) != len(other_arr): + max_dim = max(len(self._data), len(other_arr)) + other_vec = self.__class__(other_arr) + return self.pad(max_dim) - other_vec.pad(max_dim) + return self.__class__(self._data - other_arr) def __mul__(self: T, scalar: float) -> T: return self.__class__(self._data * scalar) @@ -177,6 +199,18 @@ def to_2d(self: T) -> T: """Convert a vector to a 2D vector by taking only the x and y components.""" return self.__class__(self._data[:2]) + def pad(self: T, dim: int) -> T: + """Pad a vector with zeros to reach the specified dimension. + + If vector already has dimension >= dim, it is returned unchanged. + """ + if self.dim >= dim: + return self + + padded = np.zeros(dim, dtype=float) + padded[: len(self._data)] = self._data + return self.__class__(padded) + def distance(self, other) -> float: """Compute Euclidean distance to another vector.""" if isinstance(other, Vector): diff --git a/dimos/utils/reactive.py b/dimos/utils/reactive.py index 4836e18987..de4d31f8fb 100644 --- a/dimos/utils/reactive.py +++ b/dimos/utils/reactive.py @@ -147,3 +147,11 @@ def _on_msg(value: T): return Disposable(lambda: stop(_on_msg)) return rx.create(_subscribe) + + +def spy(name: str): + def spyfun(x): + print(f"SPY {name}:", x) + return x + + return ops.map(spyfun) From ba2807a063ba5696b9a5d5d5d4eaeb7bfa3d8df3 Mon Sep 17 00:00:00 2001 From: lesh Date: Mon, 2 Jun 2025 18:52:54 +0300 Subject: [PATCH 56/72] random fiddling --- dimos/robot/local_planner/simple.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index f022daf562..0d2ef1053e 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -26,7 +26,7 @@ class SimplePlanner(LocalPlanner): goal: Optional[Vector] = None path: Optional[Path] = None speed: float = 0.5 - slowdown_range = 0.3 + slowdown_range = 0.5 def set_path(self, path: Path) -> bool: self.path = path @@ -35,6 +35,7 @@ def set_path(self, path: Path) -> bool: def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: self.goal = to_vector(goal).to_2d() + self.vis("local_goal", self.goal) logger.info(f"Setting goal: {self.goal}") def speed_ctrl(self, direction: Vector): @@ -46,7 +47,7 @@ def speed_ctrl(self, direction: Vector): return self.speed * f - def yaw_ctrl(self, direction, k_p: float = 1.0, omega_max: float = 0.2): + def yaw_ctrl(self, direction, k_p: float = 0.3, omega_max: float = 1): alpha = math.atan2(-direction.y, direction.x) omega = max(-omega_max, min(omega_max, k_p * alpha)) return Vector(0.0, 0.0, omega) @@ -59,23 +60,31 @@ def pop_path(self): def filter(self, direction): length = direction.length() if self.path: - if length < 0.4: + if length < 0.5: self.pop_path() return False else: - if length < 0.1: + if length < 0.2: return False return direction - def get_move_stream(self, frequency: float = 40.0) -> rx.Observable: + def ctrl(self, direction): + jaw = self.yaw_ctrl(direction) + if jaw.length() > 0.25: + return jaw + else: + return (direction.normalize() * self.speed_ctrl(direction)) + jaw + + def get_move_stream(self, frequency: float = 30.0) -> rx.Observable: return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( ops.filter(lambda _: (self.goal is not None) or (self.path is not None)), ops.map(lambda _: self.get_robot_pos()), ops.map(lambda odom: odom.vector_to(self.goal)), ops.filter(self.filter), + ops.map(self.ctrl), # somewhere here we should detect potential obstacles in direction of travel - ops.map(lambda direction: direction.normalize() * self.speed_ctrl(direction)), - ops.map(lambda direction: direction), # + self.yaw_ctrl(direction)), + # ops.map(lambda direction: direction.normalize() * self.speed_ctrl(direction)), + # ops.map(lambda direction: direction + self.yaw_ctrl(direction)), ops.map(lambda direction: Vector(-direction.y, direction.x, direction.z)), ) From bf202e7c80aab640e008326b8c1eaa1f500d5e26 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 14:40:13 +0300 Subject: [PATCH 57/72] cleanup --- dimos/robot/global_planner/planner.py | 2 +- dimos/robot/local_planner/simple.py | 2 + dimos/robot/unitree_webrtc/type/costmap.py | 352 ------------------ dimos/robot/unitree_webrtc/type/lidar.py | 9 + dimos/robot/unitree_webrtc/type/map.py | 67 +--- dimos/robot/unitree_webrtc/type/test_map.py | 2 +- .../unitree_webrtc/type/test_odometry.py | 285 ++++---------- dimos/robot/unitree_webrtc/unitree_go2.py | 5 +- dimos/types/costmap.py | 134 ++++++- tests/run_webrtc.py | 1 + 10 files changed, 219 insertions(+), 640 deletions(-) delete mode 100644 dimos/robot/unitree_webrtc/type/costmap.py diff --git a/dimos/robot/global_planner/planner.py b/dimos/robot/global_planner/planner.py index 7540b54182..75a780d7cd 100644 --- a/dimos/robot/global_planner/planner.py +++ b/dimos/robot/global_planner/planner.py @@ -59,7 +59,7 @@ class AstarPlanner(Planner): def plan(self, goal: VectorLike) -> Path: goal = to_vector(goal).to_2d() pos = self.get_robot_pos().to_2d() - costmap = self.get_costmap().smudge(preserve_unknown=False) + costmap = self.get_costmap().smudge() # self.vis("costmap", costmap) self.vis("target", goal) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index 0d2ef1053e..43632d71ca 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -19,6 +19,8 @@ logger = setup_logger("dimos.robot.unitree.global_planner") +# experimental stream based vector based planner +# not production ready - just a sketch @dataclass class SimplePlanner(LocalPlanner): get_costmap: Callable[[], Costmap] diff --git a/dimos/robot/unitree_webrtc/type/costmap.py b/dimos/robot/unitree_webrtc/type/costmap.py deleted file mode 100644 index ba8dba55e3..0000000000 --- a/dimos/robot/unitree_webrtc/type/costmap.py +++ /dev/null @@ -1,352 +0,0 @@ -import base64 -import pickle -import numpy as np -from typing import Optional -from scipy import ndimage -from dimos.types.vector import Vector, VectorLike, x, y, to_vector -import open3d as o3d -from matplotlib import cm # any matplotlib colormap - -DTYPE2STR = { - np.float32: "f32", - np.float64: "f64", - np.int32: "i32", - np.int8: "i8", -} - -STR2DTYPE = {v: k for k, v in DTYPE2STR.items()} - - -def encode_ndarray(arr: np.ndarray, compress: bool = False): - arr_c = np.ascontiguousarray(arr) - payload = arr_c.tobytes() - b64 = base64.b64encode(payload).decode("ascii") - - return { - "type": "grid", - "shape": arr_c.shape, - "dtype": DTYPE2STR[arr_c.dtype.type], - "data": b64, - } - - -class Costmap: - """Class to hold ROS OccupancyGrid data.""" - - def __init__( - self, - grid: np.ndarray, - origin: VectorLike, - resolution: float = 0.05, - ): - """Initialize Costmap with its core attributes.""" - self.grid = grid - self.resolution = resolution - self.origin = to_vector(origin).to_2d() - self.width = self.grid.shape[1] - self.height = self.grid.shape[0] - - def serialize(self) -> dict: - """Serialize the Costmap instance to a dictionary.""" - return { - "type": "costmap", - "grid": encode_ndarray(self.grid), - "origin": self.origin.serialize(), - "resolution": self.resolution, - } - - def save_pickle(self, pickle_path: str): - """Save costmap to a pickle file. - - Args: - pickle_path: Path to save the pickle file - """ - with open(pickle_path, "wb") as f: - pickle.dump(self, f) - - @classmethod - def create_empty( - cls, width: int = 100, height: int = 100, resolution: float = 0.1 - ) -> "Costmap": - """Create an empty costmap with specified dimensions.""" - return cls( - grid=np.zeros((height, width), dtype=np.int8), - resolution=resolution, - origin=(0.0, 0.0), - ) - - def world_to_grid(self, point: VectorLike) -> Vector: - """Convert world coordinates to grid coordinates. - - Args: - point: A vector-like object containing X,Y coordinates - - Returns: - Vector containing grid_x and grid_y coordinates - """ - return (to_vector(point) - self.origin) / self.resolution - - def grid_to_world(self, grid_point: VectorLike) -> Vector: - return to_vector(grid_point) * self.resolution + self.origin - - def is_occupied(self, point: VectorLike, threshold: int = 50) -> bool: - """Check if a position in world coordinates is occupied. - - Args: - point: Vector-like object containing X,Y coordinates - threshold: Cost threshold above which a cell is considered occupied (0-100) - - Returns: - True if position is occupied or out of bounds, False otherwise - """ - grid_pos = self.world_to_grid(point) - - if 0 <= grid_pos.x < self.width and 0 <= grid_pos.y < self.height: - # Consider unknown (-1) as unoccupied for navigation purposes - # Convert to int coordinates for grid indexing - grid_y, grid_x = int(grid_pos.y), int(grid_pos.x) - value = self.grid[grid_y, grid_x] - return bool(value > 0 and value >= threshold) - return True # Consider out-of-bounds as occupied - - def get_value(self, point: VectorLike) -> Optional[int]: - grid_pos = self.world_to_grid(point) - - if 0 <= grid_pos.x < self.width and 0 <= grid_pos.y < self.height: - grid_y, grid_x = int(grid_pos.y), int(grid_pos.x) - return int(self.grid[grid_y, grid_x]) - return None - - def set_value(self, point: VectorLike, value: int = 0) -> bool: - grid_pos = self.world_to_grid(point) - - if 0 <= grid_pos.x < self.width and 0 <= grid_pos.y < self.height: - grid_y, grid_x = int(grid_pos.y), int(grid_pos.x) - self.grid[grid_y, grid_x] = value - return True - return False - - def smudge( - self, - kernel_size: int = 3, - iterations: int = 20, - decay_factor: float = 0.9, - threshold: int = 90, - preserve_unknown: bool = False, - ) -> "Costmap": - """ - Creates a new costmap with expanded obstacles (smudged). - - Args: - kernel_size: Size of the convolution kernel for dilation (must be odd) - iterations: Number of dilation iterations - decay_factor: Factor to reduce cost as distance increases (0.0-1.0) - threshold: Minimum cost value to consider as an obstacle for expansion - preserve_unknown: Whether to keep unknown (-1) cells as unknown - - Returns: - A new Costmap instance with expanded obstacles - """ - # Make sure kernel size is odd - if kernel_size % 2 == 0: - kernel_size += 1 - - # Create a copy of the grid for processing - grid_copy = self.grid.copy() - - # Create a mask of unknown cells if needed - unknown_mask = None - if preserve_unknown: - unknown_mask = grid_copy == -1 - # Temporarily replace unknown cells with 0 for processing - # This allows smudging to go over unknown areas - grid_copy[unknown_mask] = 0 - - # Create a mask of cells that are above the threshold - obstacle_mask = grid_copy >= threshold - - # Create a binary map of obstacles - binary_map = obstacle_mask.astype(np.uint8) * 100 - - # Create a circular kernel for dilation (instead of square) - y, x = np.ogrid[ - -kernel_size // 2 : kernel_size // 2 + 1, - -kernel_size // 2 : kernel_size // 2 + 1, - ] - kernel = (x * x + y * y <= (kernel_size // 2) * (kernel_size // 2)).astype(np.uint8) - - # Create distance map using dilation - # Each iteration adds one 'ring' of cells around obstacles - dilated_map = binary_map.copy() - - # Store each layer of dilation with decreasing values - layers = [] - - # First layer is the original obstacle cells - layers.append(binary_map.copy()) - - for i in range(iterations): - # Dilate the binary map - dilated = ndimage.binary_dilation( - dilated_map > 0, structure=kernel, iterations=1 - ).astype(np.uint8) - - # Calculate the new layer (cells that were just added in this iteration) - new_layer = (dilated - (dilated_map > 0).astype(np.uint8)) * 100 - - # Apply decay factor based on distance from obstacle - new_layer = new_layer * (decay_factor ** (i + 1)) - - # Add to layers list - layers.append(new_layer) - - # Update dilated map for next iteration - dilated_map = dilated * 100 - - # Combine all layers to create a distance-based cost map - smudged_map = np.zeros_like(grid_copy) - for layer in layers: - # For each cell, keep the maximum value across all layers - smudged_map = np.maximum(smudged_map, layer) - - # Preserve original obstacles - smudged_map[obstacle_mask] = grid_copy[obstacle_mask] - - # When preserve_unknown is true, restore all original unknown cells - # This overlays unknown cells on top of the smudged map - if preserve_unknown and unknown_mask is not None: - smudged_map[unknown_mask] = -1 - - # Ensure cost values are in valid range (0-100) except for unknown (-1) - if preserve_unknown and unknown_mask is not None: - valid_cells = ~unknown_mask - smudged_map[valid_cells] = np.clip(smudged_map[valid_cells], 0, 100) - else: - smudged_map = np.clip(smudged_map, 0, 100) - - # Create a new costmap with the smudged grid - return Costmap( - grid=smudged_map.astype(np.int8), - resolution=self.resolution, - origin=self.origin, - ) - - @property - def total_cells(self) -> int: - return self.width * self.height - - @property - def occupied_cells(self) -> int: - return np.sum(self.grid >= 0.1) - - @property - def unknown_cells(self) -> int: - return np.sum(self.grid == -1) - - @property - def free_cells(self) -> int: - return self.total_cells - self.occupied_cells - self.unknown_cells - - @property - def free_percent(self) -> float: - return (self.free_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 - - @property - def occupied_percent(self) -> float: - return (self.occupied_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 - - @property - def unknown_percent(self) -> float: - return (self.unknown_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 - - def __str__(self) -> str: - """ - Create a string representation of the Costmap. - - Returns: - A formatted string with key costmap information - """ - - cell_info = [ - "▦ Costmap", - f"{self.width}x{self.height}", - f"({self.width * self.resolution:.1f}x{self.height * self.resolution:.1f}m @", - f"{1 / self.resolution:.0f}cm res)", - f"Origin: ({x(self.origin):.2f}, {y(self.origin):.2f})", - f"▣ {self.occupied_percent:.1f}%", - f"□ {self.free_percent:.1f}%", - f"◌ {self.unknown_percent:.1f}%", - ] - - return " ".join(cell_info) - - @property - def o3d_geometry(self): - return self.pointcloud - - @property - def pointcloud(self, *, res: float = 0.25, origin=(0.0, 0.0), show_unknown: bool = False): - """ - Visualise a 2-D costmap (int8, −1…100) as an Open3D PointCloud. - - • −1 → ‘unknown’ (optionally drawn as mid-grey, or skipped) - • 0 → free - • 1-99→ graduated cost (turbo colour-ramp) - • 100 → lethal / obstacle (red end of ramp) - - Parameters - ---------- - res : float - Cell size in metres. - origin : (float, float) - World-space coord of costmap [row0,col0] centre. - show_unknown : bool - If true, draw unknown cells in grey; otherwise omit them. - """ - cost = np.asarray(self.grid, dtype=np.int16) - if cost.ndim != 2: - raise ValueError("cost map must be 2-D (H×W)") - - H, W = cost.shape - ys, xs = np.mgrid[0:H, 0:W] - - # ---------- flatten & mask -------------------------------------------------- - xs = xs.ravel() - ys = ys.ravel() - vals = cost.ravel() - - unknown_mask = vals == -1 - if not show_unknown: - keep = ~unknown_mask - xs, ys, vals = xs[keep], ys[keep], vals[keep] - - # ---------- 3-D points ------------------------------------------------------ - xyz = np.column_stack( - ( - (xs + 0.5) * res + origin[0], # X - (ys + 0.5) * res + origin[1], # Y - np.zeros_like(xs, dtype=np.float32), # Z = 0 - ) - ) - - # ---------- colours --------------------------------------------------------- - rgb = np.empty((len(vals), 3), dtype=np.float32) - - if show_unknown: - # mid-grey for unknown - rgb[unknown_mask[~unknown_mask if not show_unknown else slice(None)]] = ( - 0.4, - 0.4, - 0.4, - ) - - # normalise valid costs: 0…100 → 0…1 - norm = np.clip(vals.astype(np.float32), 0, 100) / 100.0 - rgb_valid = cm.turbo(norm)[:, :3] # type: ignore[attr-defined] # strip alpha - rgb[:] = rgb_valid # unknown already set if needed - - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(xyz) - pcd.colors = o3d.utility.Vector3dVector(rgb) - - return pcd diff --git a/dimos/robot/unitree_webrtc/type/lidar.py b/dimos/robot/unitree_webrtc/type/lidar.py index a0e081d4f1..11ccd03e8b 100644 --- a/dimos/robot/unitree_webrtc/type/lidar.py +++ b/dimos/robot/unitree_webrtc/type/lidar.py @@ -1,6 +1,7 @@ from dimos.robot.unitree_webrtc.testing.helpers import color from datetime import datetime from dimos.robot.unitree_webrtc.type.timeseries import Timestamped, to_datetime, to_human_readable +from dimos.types.costmap import Costmap, pointcloud_to_costmap from dimos.types.vector import Vector from dataclasses import dataclass, field from typing import List, TypedDict @@ -40,6 +41,7 @@ class LidarMessage(Timestamped): resolution: float pointcloud: o3d.geometry.PointCloud raw_msg: RawLidarMsg = field(repr=False, default=None) + _costmap: Costmap = field(init=False, repr=False, default=None) @classmethod def from_msg(cls, raw_message: RawLidarMsg) -> "LidarMessage": @@ -139,3 +141,10 @@ def get_color(color_choice): # Looks like we'll be displaying so might as well? self.estimate_normals() return self + + def costmap(self) -> Costmap: + if not self._costmap: + grid, origin_xy = pointcloud_to_costmap(self.pointcloud, resolution=self.resolution) + self._costmap = Costmap(grid=grid, origin=[*origin_xy, 0.0], resolution=self.resolution) + + return self._costmap diff --git a/dimos/robot/unitree_webrtc/type/map.py b/dimos/robot/unitree_webrtc/type/map.py index cbfe4eb903..5bed2fdb30 100644 --- a/dimos/robot/unitree_webrtc/type/map.py +++ b/dimos/robot/unitree_webrtc/type/map.py @@ -4,7 +4,7 @@ from typing import Tuple, Optional from dimos.robot.unitree_webrtc.type.lidar import LidarMessage -from dimos.robot.unitree_webrtc.type.costmap import Costmap +from dimos.types.costmap import Costmap, pointcloud_to_costmap from reactivex.observable import Observable import reactivex.operators as ops @@ -103,68 +103,3 @@ def _inflate_lethal(costmap: np.ndarray, radius: int, lethal_val: int = 100) -> out = costmap.copy() out[dilated] = lethal_val return out - - -def pointcloud_to_costmap( - pcd: o3d.geometry.PointCloud, - *, - resolution: float = 0.05, - ground_z: float = 0.0, - obs_min_height: float = 0.15, - max_height: Optional[float] = 0.5, - inflate_radius_m: Optional[float] = None, - default_unknown: int = -1, - cost_free: int = 0, - cost_lethal: int = 100, -) -> Tuple[np.ndarray, np.ndarray]: - """Rasterise *pcd* into a 2-D int8 cost-map with optional obstacle inflation. - - Grid origin is **aligned** to the `resolution` lattice so that when - `resolution == voxel_size` every voxel centroid lands squarely inside a cell - (no alternating blank lines). - """ - - pts = np.asarray(pcd.points, dtype=np.float32) - if pts.size == 0: - return np.full((1, 1), default_unknown, np.int8), np.zeros(2, np.float32) - - # 0. Ceiling filter -------------------------------------------------------- - if max_height is not None: - pts = pts[pts[:, 2] <= max_height] - if pts.size == 0: - return np.full((1, 1), default_unknown, np.int8), np.zeros(2, np.float32) - - # 1. Bounding box & aligned origin --------------------------------------- - xy_min = pts[:, :2].min(axis=0) - xy_max = pts[:, :2].max(axis=0) - - # Align origin to the resolution grid (anchor = 0,0) - origin = np.floor(xy_min / resolution) * resolution - - # Grid dimensions (inclusive) ------------------------------------------- - Nx, Ny = (np.ceil((xy_max - origin) / resolution).astype(int) + 1).tolist() - - # 2. Bin points ------------------------------------------------------------ - idx_xy = np.floor((pts[:, :2] - origin) / resolution).astype(np.int32) - np.clip(idx_xy[:, 0], 0, Nx - 1, out=idx_xy[:, 0]) - np.clip(idx_xy[:, 1], 0, Ny - 1, out=idx_xy[:, 1]) - - lin = idx_xy[:, 1] * Nx + idx_xy[:, 0] - z_max = np.full(Nx * Ny, -np.inf, np.float32) - np.maximum.at(z_max, lin, pts[:, 2]) - z_max = z_max.reshape(Ny, Nx) - - # 3. Cost rules ----------------------------------------------------------- - costmap = np.full_like(z_max, default_unknown, np.int8) - known = z_max != -np.inf - costmap[known] = cost_free - - lethal = z_max >= (ground_z + obs_min_height) - costmap[lethal] = cost_lethal - - # 4. Optional inflation ---------------------------------------------------- - if inflate_radius_m and inflate_radius_m > 0: - cells = int(np.ceil(inflate_radius_m / resolution)) - costmap = _inflate_lethal(costmap, cells, lethal_val=cost_lethal) - - return costmap, origin.astype(np.float32) diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 88c394236c..6328247182 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -44,7 +44,7 @@ def test_robot_vis(): def test_robot_mapping(): lidar_stream = SensorReplay("office_lidar", autocast=lambda x: LidarMessage.from_msg(x)) map = Map(voxel_size=0.5) - map.consume(lidar_stream.stream(rate_hz=100.0)).run() + map.consume(lidar_stream.stream(rate_hz=1000.0)).run() costmap = map.costmap diff --git a/dimos/robot/unitree_webrtc/type/test_odometry.py b/dimos/robot/unitree_webrtc/type/test_odometry.py index 4a48432257..1fe43dc1b4 100644 --- a/dimos/robot/unitree_webrtc/type/test_odometry.py +++ b/dimos/robot/unitree_webrtc/type/test_odometry.py @@ -1,237 +1,116 @@ +from __future__ import annotations + import math +import os +import threading +from typing import Iterable, Optional import pytest +from dotenv import load_dotenv + +from dimos.robot.unitree_webrtc.type.odometry import Odometry, RawOdometryMessage +from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 +from dimos.utils.testing import SensorReplay, SensorStorage -from dimos.robot.unitree_webrtc.type.odometry import Odometry -from dimos.utils.testing import SensorReplay +_REPLAY_NAME = "raw_odometry_rotate_walk" +_EXPECTED_MESSAGE_COUNT = 179 +_EXPECTED_TOTAL_DEGREES = -592.1696370412612 +_EXPECTED_LAST_YAW_RAD = -1.0452624875249261 -def quaternion_to_euler_z(x, y, z, w): - """Convert quaternion to Euler angles and return the Z (yaw) angle in radians.""" - # Calculate yaw (rotation around z-axis) +def _quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: + """Return yaw (rotation about Z) in *radians* from a quaternion.""" siny_cosp = 2 * (w * z + x * y) cosy_cosp = 1 - 2 * (y * y + z * z) - yaw = math.atan2(siny_cosp, cosy_cosp) - return yaw - - -def test_calculate_rotation_degrees(): - """Calculate total degrees of rotation change from the odometry stream.""" - messages = list( - SensorReplay( - name="raw_odometry_rotate_walk", - autocast=lambda x: x, # Keep raw messages - ).iterate() - ) - - if not messages: - print("No messages found in the replay data") - return - - print(f"Found {len(messages)} odometry messages") - - # Extract yaw angles from quaternions - yaw_angles = [] - timestamps = [] - - for msg in messages: - orientation = msg["data"]["pose"]["orientation"] - x, y, z, w = orientation["x"], orientation["y"], orientation["z"], orientation["w"] - - # Convert quaternion to yaw angle (rotation around z-axis) - yaw = quaternion_to_euler_z(x, y, z, w) - yaw_angles.append(yaw) - - # Extract timestamp - stamp = msg["data"]["header"]["stamp"] - timestamp = stamp["sec"] + stamp["nanosec"] / 1e9 - timestamps.append(timestamp) - - if len(yaw_angles) < 2: - print("Need at least 2 messages to calculate rotation change") - return - - # Calculate cumulative rotation change - total_rotation_radians = 0 - rotation_changes = [] - - for i in range(1, len(yaw_angles)): - # Calculate the difference, handling wrap-around at ±π - diff = yaw_angles[i] - yaw_angles[i - 1] - - # Normalize the difference to [-π, π] - while diff > math.pi: - diff -= 2 * math.pi - while diff < -math.pi: - diff += 2 * math.pi - - rotation_changes.append(diff) - total_rotation_radians += diff - - # Convert to degrees - total_rotation_degrees = math.degrees(total_rotation_radians) - - # Calculate time span - time_span = timestamps[-1] - timestamps[0] - - # Statistics - rotation_changes_degrees = [math.degrees(r) for r in rotation_changes] - max_rotation_rate = ( - max(abs(r) for r in rotation_changes_degrees) if rotation_changes_degrees else 0 - ) - avg_rotation_rate = ( - sum(abs(r) for r in rotation_changes_degrees) / len(rotation_changes_degrees) - if rotation_changes_degrees - else 0 - ) - - print("\n=== ROTATION ANALYSIS ===") - print(f"Total rotation change: {total_rotation_degrees:.2f} degrees") - print(f"Time span: {time_span:.2f} seconds") - print(f"Average rotation rate: {avg_rotation_rate:.2f} degrees per step") - print(f"Maximum rotation rate: {max_rotation_rate:.2f} degrees per step") - print(f"Number of rotation steps: {len(rotation_changes)}") - - if abs(total_rotation_degrees) > 360: - full_rotations = abs(total_rotation_degrees) // 360 - remaining_degrees = abs(total_rotation_degrees) % 360 - direction = "clockwise" if total_rotation_degrees < 0 else "counter-clockwise" - print( - f"That's {full_rotations:.0f} full rotation(s) + {remaining_degrees:.2f}° {direction}!" - ) - - # Show first few and last few rotation changes for inspection - print(f"\nFirst 5 rotation changes (degrees): {rotation_changes_degrees[:5]}") - print(f"Last 5 rotation changes (degrees): {rotation_changes_degrees[-5:]}") - - return total_rotation_degrees + return math.atan2(siny_cosp, cosy_cosp) -@pytest.mark.tool -def test_store_odometry_stream(): - import os - import threading - import time +def _replay_iter(autocast=None) -> Iterable[RawOdometryMessage]: + return SensorReplay(name=_REPLAY_NAME, autocast=autocast).iterate() - from dotenv import load_dotenv - from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 - from dimos.utils.testing import SensorStorage +def test_dataset_size() -> None: + """Ensure the replay contains the expected number of messages.""" + count = sum(1 for _ in _replay_iter()) + assert count == _EXPECTED_MESSAGE_COUNT - load_dotenv() - - robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") - robot.standup() - def streamlogger(element): - print(element) - return element +def test_odometry_conversion_and_count() -> None: + """Each replay entry converts to :class:`Odometry` and count is correct.""" + count = 0 + for raw in _replay_iter(): + odom = Odometry.from_msg(raw) + assert isinstance(raw, dict) + assert isinstance(odom, Odometry) + count += 1 - SensorStorage("raw_odometry_rotate_walk").save_stream(robot.raw_odom_stream()).subscribe(print) - - shutdown_event = threading.Event() - print("Robot is running. Press Ctrl+C to stop.") - - try: - # Keep the main thread alive while the robot loop runs in the background - while not shutdown_event.is_set(): - time.sleep(0.1) - except KeyboardInterrupt: - print("\nKeyboard interrupt received. Shutting down...") - finally: - print("Stopping robot...") - robot.liedown() - print("Robot stopped.") + assert count == _EXPECTED_MESSAGE_COUNT -def test_odometry_stream(): - counter = 0 - for message in SensorReplay( - name="raw_odometry_rotate_walk", autocast=lambda x: Odometry.from_msg(x) - ).iterate(): - counter += 1 - print(message) +def test_last_yaw_value() -> None: + """Verify yaw of the final message (regression guard).""" + last_msg: Optional[RawOdometryMessage] = None + for msg in _replay_iter(): + last_msg = msg # streaming – retain only the most recent + assert last_msg is not None, "Replay is empty" # pragma: no cover -def test_updated_odometry_rotation(): - """Test that the updated Odometry class now provides correct yaw angles in rot.z.""" - print("Testing updated Odometry class with proper quaternion conversion...") + o = last_msg["data"]["pose"]["orientation"] + yaw = _quaternion_to_yaw(o["x"], o["y"], o["z"], o["w"]) + assert yaw == pytest.approx(_EXPECTED_LAST_YAW_RAD, abs=1e-10) - # Test with a few messages - messages = list( - SensorReplay( - name="raw_odometry_rotate_walk", autocast=lambda x: Odometry.from_msg(x) - ).iterate() - ) - if len(messages) < 10: - print("Need at least 10 messages for testing") - return +def test_total_rotation_from_quaternion() -> None: + """Integrate yaw deltas computed directly from quaternions.""" + total_rad = 0.0 + prev_yaw: Optional[float] = None - print("\nFirst 5 Odometry objects with corrected rotation:") - for i, odom in enumerate(messages[:5]): - yaw_degrees = math.degrees(odom.rot.z) - print(f" {i + 1}: {odom}") + for msg in _replay_iter(): + o = msg["data"]["pose"]["orientation"] + yaw = _quaternion_to_yaw(o["x"], o["y"], o["z"], o["w"]) - # Calculate total rotation using the new rot.z values - total_rotation_radians = 0 - rotation_changes = [] + if prev_yaw is not None: + diff = yaw - prev_yaw + diff = (diff + math.pi) % (2 * math.pi) - math.pi # normalize + total_rad += diff + prev_yaw = yaw - for i in range(1, len(messages)): - # Now we can directly use rot.z which contains the yaw angle - diff = messages[i].rot.z - messages[i - 1].rot.z + assert math.degrees(total_rad) == pytest.approx(_EXPECTED_TOTAL_DEGREES, abs=1e-6) - # Normalize the difference to [-π, π] - while diff > math.pi: - diff -= 2 * math.pi - while diff < -math.pi: - diff += 2 * math.pi - rotation_changes.append(diff) - total_rotation_radians += diff +def test_total_rotation_from_odometry() -> None: + """`Odometry.rot.z` integration should match quaternion path.""" + total_rad = 0.0 + prev_yaw: Optional[float] = None - total_rotation_degrees = math.degrees(total_rotation_radians) + for odom in _replay_iter(Odometry.from_msg): + yaw = odom.rot.z + if prev_yaw is not None: + diff = yaw - prev_yaw + diff = (diff + math.pi) % (2 * math.pi) - math.pi + total_rad += diff + prev_yaw = yaw - print("\n=== SIMPLIFIED ROTATION ANALYSIS ===") - print(f"Total rotation using Odometry.rot.z: {total_rotation_degrees:.2f} degrees") - print(f"Number of messages: {len(messages)}") - print(f"First few rotation changes: {[math.degrees(r) for r in rotation_changes[:5]]}") + assert math.degrees(total_rad) == pytest.approx(_EXPECTED_TOTAL_DEGREES, abs=1e-6) - return total_rotation_degrees +# data collection tool +@pytest.mark.tool +def test_store_odometry_stream() -> None: + load_dotenv() -def test_compare_rotation_methods(): - """Compare quaternion-based rotation calculation with Odometry.rot values.""" - messages = list( - SensorReplay( - name="raw_odometry_rotate_walk", - autocast=lambda x: x, # Keep raw messages - ).iterate() - ) - - if not messages: - print("No messages found in the replay data") - return - - print("Analyzing first 10 messages to compare rotation methods...") - - for i, msg in enumerate(messages[:10]): - # Extract quaternion from raw message - orientation = msg["data"]["pose"]["orientation"] - qx, qy, qz, qw = orientation["x"], orientation["y"], orientation["z"], orientation["w"] - - # Convert to Odometry object (which only uses x,y,z components) - odom = Odometry.from_msg(msg) + robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") + robot.standup() - # Calculate yaw from full quaternion - yaw_from_quaternion = quaternion_to_euler_z(qx, qy, qz, qw) - yaw_degrees = math.degrees(yaw_from_quaternion) + storage = SensorStorage(_REPLAY_NAME) + storage.save_stream(robot.raw_odom_stream()) - # Show what Odometry.rot contains (just x,y,z components of quaternion) - rot_vector = odom.rot + shutdown = threading.Event() - print(f"\nMessage {i + 1}:") - print(f" Full quaternion: x={qx:.6f}, y={qy:.6f}, z={qz:.6f}, w={qw:.6f}") - print(f" Odometry.rot (x,y,z only): {rot_vector}") - print(f" Yaw from quaternion: {yaw_degrees:.2f}°") - print(f" Z-component magnitude: {abs(qz):.6f}") + try: + while not shutdown.wait(0.1): + pass + except KeyboardInterrupt: + shutdown.set() + finally: + robot.liedown() diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 13c5b32382..f002d28b61 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -11,6 +11,7 @@ from go2_webrtc_driver.constants import VUI_COLOR from dimos.robot.local_planner.vfh.vfh_local_planner import VFHPurePursuitPlanner from dimos.robot.local_planner.vfh.local_planner import navigate_path_local +from reactivex import operators as ops class Color(VUI_COLOR): ... @@ -31,9 +32,11 @@ def __init__( self.map = Map(voxel_size=0.5) self.map_stream = self.map.consume(self.lidar_stream()) + self.get_local_lidar_frame = getter_streaming(self.lidar_stream()) + self.get_local_costmap = lambda: self.get_local_lidar_frame().costmap() self.local_planner = SimplePlanner( - get_costmap=lambda: self.map.costmap, get_robot_pos=self.odom + get_costmap=self.get_local_costmap, get_robot_pos=self.odom ) def set_path(path, *args, **kwargs): diff --git a/dimos/types/costmap.py b/dimos/types/costmap.py index 177733ff03..b349af7fd5 100644 --- a/dimos/types/costmap.py +++ b/dimos/types/costmap.py @@ -19,6 +19,7 @@ from scipy import ndimage from nav_msgs.msg import OccupancyGrid from dimos.types.vector import Vector, VectorLike, x, y, to_vector +import open3d as o3d DTYPE2STR = { np.float32: "f32", @@ -49,8 +50,8 @@ class Costmap: def __init__( self, grid: np.ndarray, - origin_theta: float, origin: VectorLike, + origin_theta: float = 0, resolution: float = 0.05, ): """Initialize Costmap with its core attributes.""" @@ -184,8 +185,8 @@ def set_value(self, point: VectorLike, value: int = 0) -> bool: def smudge( self, - kernel_size: int = 6, - iterations: int = 20, + kernel_size: int = 7, + iterations: int = 25, decay_factor: float = 0.9, threshold: int = 90, preserve_unknown: bool = False, @@ -288,6 +289,34 @@ def smudge( origin_theta=self.origin_theta, ) + @property + def total_cells(self) -> int: + return self.width * self.height + + @property + def occupied_cells(self) -> int: + return np.sum(self.grid >= 0.1) + + @property + def unknown_cells(self) -> int: + return np.sum(self.grid == -1) + + @property + def free_cells(self) -> int: + return self.total_cells - self.occupied_cells - self.unknown_cells + + @property + def free_percent(self) -> float: + return (self.free_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + + @property + def occupied_percent(self) -> float: + return (self.occupied_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + + @property + def unknown_percent(self) -> float: + return (self.unknown_cells / self.total_cells) * 100 if self.total_cells > 0 else 0.0 + def __str__(self) -> str: """ Create a string representation of the Costmap. @@ -295,16 +324,6 @@ def __str__(self) -> str: Returns: A formatted string with key costmap information """ - # Calculate occupancy statistics - total_cells = self.width * self.height - occupied_cells = np.sum(self.grid >= 50) - unknown_cells = np.sum(self.grid == -1) - free_cells = total_cells - occupied_cells - unknown_cells - - # Calculate percentages - occupied_percent = (occupied_cells / total_cells) * 100 - unknown_percent = (unknown_cells / total_cells) * 100 - free_percent = (free_cells / total_cells) * 100 cell_info = [ "▦ Costmap", @@ -312,14 +331,97 @@ def __str__(self) -> str: f"({self.width * self.resolution:.1f}x{self.height * self.resolution:.1f}m @", f"{1 / self.resolution:.0f}cm res)", f"Origin: ({x(self.origin):.2f}, {y(self.origin):.2f})", - f"▣ {occupied_percent:.1f}%", - f"□ {free_percent:.1f}%", - f"◌ {unknown_percent:.1f}%", + f"▣ {self.occupied_percent:.1f}%", + f"□ {self.free_percent:.1f}%", + f"◌ {self.unknown_percent:.1f}%", ] return " ".join(cell_info) +def _inflate_lethal(costmap: np.ndarray, radius: int, lethal_val: int = 100) -> np.ndarray: + """Return *costmap* with lethal cells dilated by *radius* grid steps (circular).""" + if radius <= 0 or not np.any(costmap == lethal_val): + return costmap + + mask = costmap == lethal_val + dilated = mask.copy() + for dy in range(-radius, radius + 1): + for dx in range(-radius, radius + 1): + if dx * dx + dy * dy > radius * radius or (dx == 0 and dy == 0): + continue + dilated |= np.roll(mask, shift=(dy, dx), axis=(0, 1)) + + out = costmap.copy() + out[dilated] = lethal_val + return out + + +def pointcloud_to_costmap( + pcd: o3d.geometry.PointCloud, + *, + resolution: float = 0.05, + ground_z: float = 0.0, + obs_min_height: float = 0.15, + max_height: Optional[float] = 0.5, + inflate_radius_m: Optional[float] = None, + default_unknown: int = -1, + cost_free: int = 0, + cost_lethal: int = 100, +) -> tuple[np.ndarray, np.ndarray]: + """Rasterise *pcd* into a 2-D int8 cost-map with optional obstacle inflation. + + Grid origin is **aligned** to the `resolution` lattice so that when + `resolution == voxel_size` every voxel centroid lands squarely inside a cell + (no alternating blank lines). + """ + + pts = np.asarray(pcd.points, dtype=np.float32) + if pts.size == 0: + return np.full((1, 1), default_unknown, np.int8), np.zeros(2, np.float32) + + # 0. Ceiling filter -------------------------------------------------------- + if max_height is not None: + pts = pts[pts[:, 2] <= max_height] + if pts.size == 0: + return np.full((1, 1), default_unknown, np.int8), np.zeros(2, np.float32) + + # 1. Bounding box & aligned origin --------------------------------------- + xy_min = pts[:, :2].min(axis=0) + xy_max = pts[:, :2].max(axis=0) + + # Align origin to the resolution grid (anchor = 0,0) + origin = np.floor(xy_min / resolution) * resolution + + # Grid dimensions (inclusive) ------------------------------------------- + Nx, Ny = (np.ceil((xy_max - origin) / resolution).astype(int) + 1).tolist() + + # 2. Bin points ------------------------------------------------------------ + idx_xy = np.floor((pts[:, :2] - origin) / resolution).astype(np.int32) + np.clip(idx_xy[:, 0], 0, Nx - 1, out=idx_xy[:, 0]) + np.clip(idx_xy[:, 1], 0, Ny - 1, out=idx_xy[:, 1]) + + lin = idx_xy[:, 1] * Nx + idx_xy[:, 0] + z_max = np.full(Nx * Ny, -np.inf, np.float32) + np.maximum.at(z_max, lin, pts[:, 2]) + z_max = z_max.reshape(Ny, Nx) + + # 3. Cost rules ----------------------------------------------------------- + costmap = np.full_like(z_max, default_unknown, np.int8) + known = z_max != -np.inf + costmap[known] = cost_free + + lethal = z_max >= (ground_z + obs_min_height) + costmap[lethal] = cost_lethal + + # 4. Optional inflation ---------------------------------------------------- + if inflate_radius_m and inflate_radius_m > 0: + cells = int(np.ceil(inflate_radius_m / resolution)) + costmap = _inflate_lethal(costmap, cells, lethal_val=cost_lethal) + + return costmap, origin.astype(np.float32) + + if __name__ == "__main__": costmap = Costmap.from_pickle("costmapMsg.pickle") print(costmap) diff --git a/tests/run_webrtc.py b/tests/run_webrtc.py index fa84d800ae..065abb8992 100644 --- a/tests/run_webrtc.py +++ b/tests/run_webrtc.py @@ -30,6 +30,7 @@ websocket_vis = WebsocketVis() websocket_vis.start() websocket_vis.connect(robot.global_planner.vis_stream()) +websocket_vis.connect(robot.local_planner.vis_stream()) def msg_handler(msgtype, data): From 19cf46f5bf025730fc6167a4f9d0c4a1811dac2f Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 14:51:28 +0300 Subject: [PATCH 58/72] odometry tests --- .../robot/unitree_webrtc/type/test_odometry.py | 18 +++++++----------- dimos/utils/testing.py | 5 ++++- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/dimos/robot/unitree_webrtc/type/test_odometry.py b/dimos/robot/unitree_webrtc/type/test_odometry.py index 1fe43dc1b4..0be8a679c4 100644 --- a/dimos/robot/unitree_webrtc/type/test_odometry.py +++ b/dimos/robot/unitree_webrtc/type/test_odometry.py @@ -4,6 +4,7 @@ import os import threading from typing import Iterable, Optional +import reactivex.operators as ops import pytest from dotenv import load_dotenv @@ -25,20 +26,16 @@ def _quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: return math.atan2(siny_cosp, cosy_cosp) -def _replay_iter(autocast=None) -> Iterable[RawOdometryMessage]: - return SensorReplay(name=_REPLAY_NAME, autocast=autocast).iterate() - - def test_dataset_size() -> None: """Ensure the replay contains the expected number of messages.""" - count = sum(1 for _ in _replay_iter()) + count = sum(1 for _ in SensorReplay(name=_REPLAY_NAME).iterate()) assert count == _EXPECTED_MESSAGE_COUNT def test_odometry_conversion_and_count() -> None: """Each replay entry converts to :class:`Odometry` and count is correct.""" count = 0 - for raw in _replay_iter(): + for raw in SensorReplay(name=_REPLAY_NAME).iterate(): odom = Odometry.from_msg(raw) assert isinstance(raw, dict) assert isinstance(odom, Odometry) @@ -49,9 +46,8 @@ def test_odometry_conversion_and_count() -> None: def test_last_yaw_value() -> None: """Verify yaw of the final message (regression guard).""" - last_msg: Optional[RawOdometryMessage] = None - for msg in _replay_iter(): - last_msg = msg # streaming – retain only the most recent + + last_msg = SensorReplay(name=_REPLAY_NAME).stream(rate_hz=None).pipe(ops.last()).run() assert last_msg is not None, "Replay is empty" # pragma: no cover @@ -65,7 +61,7 @@ def test_total_rotation_from_quaternion() -> None: total_rad = 0.0 prev_yaw: Optional[float] = None - for msg in _replay_iter(): + for msg in SensorReplay(name=_REPLAY_NAME).iterate(): o = msg["data"]["pose"]["orientation"] yaw = _quaternion_to_yaw(o["x"], o["y"], o["z"], o["w"]) @@ -83,7 +79,7 @@ def test_total_rotation_from_odometry() -> None: total_rad = 0.0 prev_yaw: Optional[float] = None - for odom in _replay_iter(Odometry.from_msg): + for odom in SensorReplay(name=_REPLAY_NAME, autocast=Odometry.from_msg).iterate(): yaw = odom.rot.z if prev_yaw is not None: diff = yaw - prev_yaw diff --git a/dimos/utils/testing.py b/dimos/utils/testing.py index f3b9f9d162..986abc8c51 100644 --- a/dimos/utils/testing.py +++ b/dimos/utils/testing.py @@ -192,7 +192,10 @@ def iterate(self) -> Iterator[Union[T, Any]]: for file_path in sorted(glob.glob(pattern)): yield self.load_one(file_path) - def stream(self, rate_hz: float = 10.0) -> Observable[Union[T, Any]]: + def stream(self, rate_hz: Optional[float] = None) -> Observable[Union[T, Any]]: + if rate_hz is None: + return from_iterable(self.iterate()) + sleep_time = 1.0 / rate_hz return from_iterable(self.iterate()).pipe( From 3cbbaeb89f2098eadf0138f6f953b94f1e0cb2c6 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 14:52:52 +0300 Subject: [PATCH 59/72] map test cleanup --- dimos/robot/unitree_webrtc/type/test_map.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree_webrtc/type/test_map.py b/dimos/robot/unitree_webrtc/type/test_map.py index 6328247182..27c1a24150 100644 --- a/dimos/robot/unitree_webrtc/type/test_map.py +++ b/dimos/robot/unitree_webrtc/type/test_map.py @@ -42,10 +42,13 @@ def test_robot_vis(): def test_robot_mapping(): - lidar_stream = SensorReplay("office_lidar", autocast=lambda x: LidarMessage.from_msg(x)) + lidar_stream = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) map = Map(voxel_size=0.5) - map.consume(lidar_stream.stream(rate_hz=1000.0)).run() + # this will block until map has consumed the whole stream + map.consume(lidar_stream.stream()).run() + + # we investigate built map costmap = map.costmap assert costmap.grid.shape == (404, 276) From 5c5bbf53b508423a35602b36812afb167e08ae72 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 15:09:43 +0300 Subject: [PATCH 60/72] cleaned up vector --- dimos/types/vector.py | 88 +++++++++++++------------------------------ 1 file changed, 27 insertions(+), 61 deletions(-) diff --git a/dimos/types/vector.py b/dimos/types/vector.py index 1348d03698..6ecb77724b 100644 --- a/dimos/types/vector.py +++ b/dimos/types/vector.py @@ -116,36 +116,18 @@ def __eq__(self, other) -> bool: return np.allclose(self._data, other._data) def __add__(self: T, other) -> T: - if isinstance(other, Vector): - # Handle different dimensions by padding the smaller vector - if len(self._data) != len(other._data): - max_dim = max(len(self._data), len(other._data)) - return self.pad(max_dim) + other.pad(max_dim) - return self.__class__(self._data + other._data) - - # For non-Vector types, convert to array and handle potential dimension mismatch - other_arr = np.array(other, dtype=float) - if len(self._data) != len(other_arr): - max_dim = max(len(self._data), len(other_arr)) - other_vec = self.__class__(other_arr) - return self.pad(max_dim) + other_vec.pad(max_dim) - return self.__class__(self._data + other_arr) + other = to_vector(other) + if self.dim != other.dim: + max_dim = max(self.dim, other.dim) + return self.pad(max_dim) + other.pad(max_dim) + return self.__class__(self._data + other._data) def __sub__(self: T, other) -> T: - if isinstance(other, Vector): - # Handle different dimensions by padding the smaller vector - if len(self._data) != len(other._data): - max_dim = max(len(self._data), len(other._data)) - return self.pad(max_dim) - other.pad(max_dim) - return self.__class__(self._data - other._data) - - # For non-Vector types, convert to array and handle potential dimension mismatch - other_arr = np.array(other, dtype=float) - if len(self._data) != len(other_arr): - max_dim = max(len(self._data), len(other_arr)) - other_vec = self.__class__(other_arr) - return self.pad(max_dim) - other_vec.pad(max_dim) - return self.__class__(self._data - other_arr) + other = to_vector(other) + if self.dim != other.dim: + max_dim = max(self.dim, other.dim) + return self.pad(max_dim) - other.pad(max_dim) + return self.__class__(self._data - other._data) def __mul__(self: T, scalar: float) -> T: return self.__class__(self._data * scalar) @@ -161,24 +143,19 @@ def __neg__(self: T) -> T: def dot(self, other) -> float: """Compute dot product.""" - if isinstance(other, Vector): - return float(np.dot(self._data, other._data)) - return float(np.dot(self._data, np.array(other, dtype=float))) + other = to_vector(other) + return float(np.dot(self._data, other._data)) def cross(self: T, other) -> T: """Compute cross product (3D vectors only).""" if self.dim != 3: raise ValueError("Cross product is only defined for 3D vectors") - if isinstance(other, Vector): - other_data = other._data - else: - other_data = np.array(other, dtype=float) - - if len(other_data) != 3: + other = to_vector(other) + if other.dim != 3: raise ValueError("Cross product requires two 3D vectors") - return self.__class__(np.cross(self._data, other_data)) + return self.__class__(np.cross(self._data, other._data)) def length(self) -> float: """Compute the Euclidean length (magnitude) of the vector.""" @@ -213,31 +190,24 @@ def pad(self: T, dim: int) -> T: def distance(self, other) -> float: """Compute Euclidean distance to another vector.""" - if isinstance(other, Vector): - return float(np.linalg.norm(self._data - other._data)) - return float(np.linalg.norm(self._data - np.array(other, dtype=float))) + other = to_vector(other) + return float(np.linalg.norm(self._data - other._data)) def distance_squared(self, other) -> float: """Compute squared Euclidean distance to another vector (faster than distance()).""" - if isinstance(other, Vector): - diff = self._data - other._data - else: - diff = self._data - np.array(other, dtype=float) + other = to_vector(other) + diff = self._data - other._data return float(np.sum(diff * diff)) def angle(self, other) -> float: """Compute the angle (in radians) between this vector and another.""" - if self.length() < 1e-10 or (isinstance(other, Vector) and other.length() < 1e-10): + other = to_vector(other) + if self.length() < 1e-10 or other.length() < 1e-10: return 0.0 - if isinstance(other, Vector): - other_data = other._data - else: - other_data = np.array(other, dtype=float) - cos_angle = np.clip( - np.dot(self._data, other_data) - / (np.linalg.norm(self._data) * np.linalg.norm(other_data)), + np.dot(self._data, other._data) + / (np.linalg.norm(self._data) * np.linalg.norm(other._data)), -1.0, 1.0, ) @@ -245,17 +215,13 @@ def angle(self, other) -> float: def project(self: T, onto) -> T: """Project this vector onto another vector.""" - if isinstance(onto, Vector): - onto_data = onto._data - else: - onto_data = np.array(onto, dtype=float) - - onto_length_sq = np.sum(onto_data * onto_data) + onto = to_vector(onto) + onto_length_sq = np.sum(onto._data * onto._data) if onto_length_sq < 1e-10: return self.__class__(np.zeros_like(self._data)) - scalar_projection = np.dot(self._data, onto_data) / onto_length_sq - return self.__class__(scalar_projection * onto_data) + scalar_projection = np.dot(self._data, onto._data) / onto_length_sq + return self.__class__(scalar_projection * onto._data) # this is here to test ros_observable_topic # doesn't happen irl afaik that we want a vector from ros message From 5240413d3800f441b4174f2859ad8a7bd5785da0 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 15:10:53 +0300 Subject: [PATCH 61/72] vector typing --- dimos/types/vector.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/dimos/types/vector.py b/dimos/types/vector.py index 6ecb77724b..9b01c6a26a 100644 --- a/dimos/types/vector.py +++ b/dimos/types/vector.py @@ -115,14 +115,14 @@ def __eq__(self, other) -> bool: return False return np.allclose(self._data, other._data) - def __add__(self: T, other) -> T: + def __add__(self: T, other: VectorLike) -> T: other = to_vector(other) if self.dim != other.dim: max_dim = max(self.dim, other.dim) return self.pad(max_dim) + other.pad(max_dim) return self.__class__(self._data + other._data) - def __sub__(self: T, other) -> T: + def __sub__(self: T, other: VectorLike) -> T: other = to_vector(other) if self.dim != other.dim: max_dim = max(self.dim, other.dim) @@ -141,12 +141,12 @@ def __truediv__(self: T, scalar: float) -> T: def __neg__(self: T) -> T: return self.__class__(-self._data) - def dot(self, other) -> float: + def dot(self, other: VectorLike) -> float: """Compute dot product.""" other = to_vector(other) return float(np.dot(self._data, other._data)) - def cross(self: T, other) -> T: + def cross(self: T, other: VectorLike) -> T: """Compute cross product (3D vectors only).""" if self.dim != 3: raise ValueError("Cross product is only defined for 3D vectors") @@ -188,18 +188,18 @@ def pad(self: T, dim: int) -> T: padded[: len(self._data)] = self._data return self.__class__(padded) - def distance(self, other) -> float: + def distance(self, other: VectorLike) -> float: """Compute Euclidean distance to another vector.""" other = to_vector(other) return float(np.linalg.norm(self._data - other._data)) - def distance_squared(self, other) -> float: + def distance_squared(self, other: VectorLike) -> float: """Compute squared Euclidean distance to another vector (faster than distance()).""" other = to_vector(other) diff = self._data - other._data return float(np.sum(diff * diff)) - def angle(self, other) -> float: + def angle(self, other: VectorLike) -> float: """Compute the angle (in radians) between this vector and another.""" other = to_vector(other) if self.length() < 1e-10 or other.length() < 1e-10: @@ -213,7 +213,7 @@ def angle(self, other) -> float: ) return float(np.arccos(cos_angle)) - def project(self: T, onto) -> T: + def project(self: T, onto: VectorLike) -> T: """Project this vector onto another vector.""" onto = to_vector(onto) onto_length_sq = np.sum(onto._data * onto._data) From 2f59478cf410d18e9d17f758285e7d7e08052230 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 16:56:36 +0300 Subject: [PATCH 62/72] removed flake, small cleanup --- dimos/robot/local_planner/local_planner.py | 3 +- dimos/utils/test_testing.py | 2 +- flake.lock | 61 -------------- flake.nix | 95 ---------------------- 4 files changed, 3 insertions(+), 158 deletions(-) delete mode 100644 flake.lock delete mode 100644 flake.nix diff --git a/dimos/robot/local_planner/local_planner.py b/dimos/robot/local_planner/local_planner.py index 7e1ae601b3..3472825165 100644 --- a/dimos/robot/local_planner/local_planner.py +++ b/dimos/robot/local_planner/local_planner.py @@ -4,12 +4,13 @@ from reactivex import Observable from dimos.types.vector import Vector, VectorLike +from dimos.types.path import Path from dimos.web.websocket_vis.helpers import Visualizable class LocalPlanner(Visualizable): @abstractmethod - def set_goal(self, goal: VectorLike) -> bool: ... + def set_path(self, path: Path) -> bool: ... @abstractmethod def get_move_stream(self, frequency: Optional[float]) -> Observable[Vector]: ... diff --git a/dimos/utils/test_testing.py b/dimos/utils/test_testing.py index 0221abbe36..e08c70fa9e 100644 --- a/dimos/utils/test_testing.py +++ b/dimos/utils/test_testing.py @@ -124,7 +124,7 @@ def test_sensor_replay(): def test_sensor_replay_cast(): counter = 0 for message in testing.SensorReplay( - name="office_lidar", autocast=lambda x: LidarMessage.from_msg(x) + name="office_lidar", autocast=LidarMessage.from_msg ).iterate(): counter += 1 assert isinstance(message, LidarMessage) diff --git a/flake.lock b/flake.lock deleted file mode 100644 index faa26f56c7..0000000000 --- a/flake.lock +++ /dev/null @@ -1,61 +0,0 @@ -{ - "nodes": { - "flake-utils": { - "inputs": { - "systems": "systems" - }, - "locked": { - "lastModified": 1731533236, - "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", - "owner": "numtide", - "repo": "flake-utils", - "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", - "type": "github" - }, - "original": { - "owner": "numtide", - "repo": "flake-utils", - "type": "github" - } - }, - "nixpkgs": { - "locked": { - "lastModified": 1745526057, - "narHash": "sha256-ITSpPDwvLBZBnPRS2bUcHY3gZSwis/uTe255QgMtTLA=", - "owner": "NixOS", - "repo": "nixpkgs", - "rev": "f771eb401a46846c1aebd20552521b233dd7e18b", - "type": "github" - }, - "original": { - "owner": "NixOS", - "ref": "nixos-unstable", - "repo": "nixpkgs", - "type": "github" - } - }, - "root": { - "inputs": { - "flake-utils": "flake-utils", - "nixpkgs": "nixpkgs" - } - }, - "systems": { - "locked": { - "lastModified": 1681028828, - "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", - "owner": "nix-systems", - "repo": "default", - "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", - "type": "github" - }, - "original": { - "owner": "nix-systems", - "repo": "default", - "type": "github" - } - } - }, - "root": "root", - "version": 7 -} diff --git a/flake.nix b/flake.nix deleted file mode 100644 index 60751b156f..0000000000 --- a/flake.nix +++ /dev/null @@ -1,95 +0,0 @@ -{ - description = "Project dev environment as Nix shell + DockerTools layered image"; - - inputs = { - nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; - flake-utils.url = "github:numtide/flake-utils"; - }; - - outputs = { self, nixpkgs, flake-utils, ... }: - flake-utils.lib.eachDefaultSystem (system: - let - pkgs = import nixpkgs { inherit system; }; - - # ------------------------------------------------------------ - # 1. Shared package list (tool-chain + project deps) - # ------------------------------------------------------------ - devPackages = with pkgs; [ - ### Core shell & utils - bashInteractive coreutils gh - stdenv.cc.cc.lib - - ### Python + static analysis - python312 python312Packages.pip python312Packages.setuptools - python312Packages.virtualenv ruff mypy pre-commit - - ### Runtime deps - python312Packages.pyaudio portaudio ffmpeg_6 ffmpeg_6.dev - - ### Graphics / X11 stack - libGL libGLU mesa glfw - xorg.libX11 xorg.libXi xorg.libXext xorg.libXrandr xorg.libXinerama - xorg.libXcursor xorg.libXfixes xorg.libXrender xorg.libXdamage - xorg.libXcomposite xorg.libxcb xorg.libXScrnSaver xorg.libXxf86vm - - udev SDL2 SDL2.dev zlib - - ### GTK / OpenCV helpers - glib gtk3 gdk-pixbuf gobject-introspection - - ### Open3D & build-time - eigen cmake ninja jsoncpp libjpeg libpng - ]; - - # ------------------------------------------------------------ - # 2. Host interactive shell → `nix develop` - # ------------------------------------------------------------ - devShell = pkgs.mkShell { - packages = devPackages; - shellHook = '' - export LD_LIBRARY_PATH="${pkgs.lib.makeLibraryPath [ - pkgs.stdenv.cc.cc.lib pkgs.libGL pkgs.libGLU pkgs.mesa pkgs.glfw - pkgs.xorg.libX11 pkgs.xorg.libXi pkgs.xorg.libXext pkgs.xorg.libXrandr - pkgs.xorg.libXinerama pkgs.xorg.libXcursor pkgs.xorg.libXfixes - pkgs.xorg.libXrender pkgs.xorg.libXdamage pkgs.xorg.libXcomposite - pkgs.xorg.libxcb pkgs.xorg.libXScrnSaver pkgs.xorg.libXxf86vm - pkgs.udev pkgs.portaudio pkgs.SDL2.dev pkgs.zlib pkgs.glib pkgs.gtk3 - pkgs.gdk-pixbuf pkgs.gobject-introspection]}:$LD_LIBRARY_PATH" - - export DISPLAY=:0 - - PROJECT_ROOT=$(git rev-parse --show-toplevel 2>/dev/null || echo "$PWD") - if [ -f "$PROJECT_ROOT/env/bin/activate" ]; then - . "$PROJECT_ROOT/env/bin/activate" - fi - - [ -f "$PROJECT_ROOT/motd" ] && cat "$PROJECT_ROOT/motd" - [ -f "$PROJECT_ROOT/.pre-commit-config.yaml" ] && pre-commit install --install-hooks - ''; - }; - - # ------------------------------------------------------------ - # 3. Closure copied into the OCI image rootfs - # ------------------------------------------------------------ - imageRoot = pkgs.buildEnv { - name = "dimos-image-root"; - paths = devPackages; - pathsToLink = [ "/bin" ]; - }; - - in { - ## Local dev shell - devShells.default = devShell; - - ## Layered docker image with DockerTools - packages.devcontainer = pkgs.dockerTools.buildLayeredImage { - name = "dimensionalos/dimos-dev"; - tag = "latest"; - contents = [ imageRoot ]; - config = { - WorkingDir = "/workspace"; - Cmd = [ "bash" ]; - }; - }; - }); -} From ade5ea242943f238c6ee8631533aacff414e067e Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 17:00:53 +0300 Subject: [PATCH 63/72] removed simple planner --- dimos/robot/local_planner/__init__.py | 7 + dimos/robot/local_planner/local_planner.py | 1278 ++++++++++++++++- .../robot/local_planner/vfh_local_planner.py | 416 ++++++ 3 files changed, 1694 insertions(+), 7 deletions(-) create mode 100644 dimos/robot/local_planner/__init__.py create mode 100644 dimos/robot/local_planner/vfh_local_planner.py diff --git a/dimos/robot/local_planner/__init__.py b/dimos/robot/local_planner/__init__.py new file mode 100644 index 0000000000..472b58dcd2 --- /dev/null +++ b/dimos/robot/local_planner/__init__.py @@ -0,0 +1,7 @@ +from dimos.robot.local_planner.local_planner import ( + BaseLocalPlanner, + navigate_to_goal_local, + navigate_path_local, +) + +from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner diff --git a/dimos/robot/local_planner/local_planner.py b/dimos/robot/local_planner/local_planner.py index 3472825165..d7af8a80d3 100644 --- a/dimos/robot/local_planner/local_planner.py +++ b/dimos/robot/local_planner/local_planner.py @@ -1,16 +1,1280 @@ -from abc import abstractmethod -from typing import Optional +#!/usr/bin/env python3 +import math +import numpy as np +from typing import Dict, Tuple, Optional, Callable +from abc import ABC, abstractmethod +import cv2 from reactivex import Observable +from reactivex.subject import Subject +import threading +import time +import logging +from collections import deque +from dimos.utils.logging_config import setup_logger +from dimos.utils.ros_utils import normalize_angle, distance_angle_to_goal_xy -from dimos.types.vector import Vector, VectorLike +from dimos.types.vector import VectorLike, Vector, to_tuple from dimos.types.path import Path -from dimos.web.websocket_vis.helpers import Visualizable +from nav_msgs.msg import OccupancyGrid +logger = setup_logger("dimos.robot.unitree.local_planner", level=logging.DEBUG) + + +class BaseLocalPlanner(ABC): + """ + Abstract base class for local planners that handle obstacle avoidance and path following. + + This class defines the common interface and shared functionality that all local planners + must implement, regardless of the specific algorithm used. + """ + + def __init__( + self, + get_costmap: Callable[[], Optional[OccupancyGrid]], + transform: object, + move_vel_control: Callable[[float, float, float], None], + safety_threshold: float = 0.5, + max_linear_vel: float = 0.8, + max_angular_vel: float = 1.0, + lookahead_distance: float = 1.0, + goal_tolerance: float = 0.75, + angle_tolerance: float = 0.15, + robot_width: float = 0.5, + robot_length: float = 0.7, + visualization_size: int = 400, + control_frequency: float = 10.0, + safe_goal_distance: float = 1.5, + ): # Control frequency in Hz + """ + Initialize the base local planner. + + Args: + get_costmap: Function to get the latest local costmap + transform: Object with transform methods (transform_point, transform_rot, etc.) + move_vel_control: Function to send velocity commands + safety_threshold: Distance to maintain from obstacles (meters) + max_linear_vel: Maximum linear velocity (m/s) + max_angular_vel: Maximum angular velocity (rad/s) + lookahead_distance: Lookahead distance for path following (meters) + goal_tolerance: Distance at which the goal is considered reached (meters) + angle_tolerance: Angle at which the goal orientation is considered reached (radians) + robot_width: Width of the robot for visualization (meters) + robot_length: Length of the robot for visualization (meters) + visualization_size: Size of the visualization image in pixels + control_frequency: Frequency at which the planner is called (Hz) + safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) + """ + # Store callables for robot interactions + self.get_costmap = get_costmap + self.transform = transform + self.move_vel_control = move_vel_control + + # Store parameters + self.safety_threshold = safety_threshold + self.max_linear_vel = max_linear_vel + self.max_angular_vel = max_angular_vel + self.lookahead_distance = lookahead_distance + self.goal_tolerance = goal_tolerance + self.angle_tolerance = angle_tolerance + self.robot_width = robot_width + self.robot_length = robot_length + self.visualization_size = visualization_size + self.control_frequency = control_frequency + self.control_period = 1.0 / control_frequency # Period in seconds + self.safe_goal_distance = safe_goal_distance # Distance to ignore obstacles at goal + self.ignore_obstacles = False # Flag for derived classes to check + + # Goal and Waypoint Tracking + self.goal_xy: Optional[Tuple[float, float]] = None # Current target for planning + self.goal_theta: Optional[float] = None # Goal orientation in odom frame + self.position_reached: bool = False # Flag indicating if position goal is reached + self.waypoints: Optional[Path] = None # Full path if following waypoints + self.waypoints_in_odom: Optional[Path] = None # Full path in odom frame + self.waypoint_frame: Optional[str] = None # Frame of the waypoints + self.current_waypoint_index: int = 0 # Index of the next waypoint to reach + self.final_goal_reached: bool = False # Flag indicating if the final waypoint is reached + + # Stuck detection + self.stuck_detection_window_seconds = 8.0 # Time window for stuck detection (seconds) + self.position_history_size = int(self.stuck_detection_window_seconds * control_frequency) + self.position_history = deque( + maxlen=self.position_history_size + ) # History of recent positions + self.stuck_distance_threshold = 0.1 # Distance threshold for stuck detection (meters) + self.unstuck_distance_threshold = 0.5 # Distance threshold for unstuck detection (meters) + self.stuck_time_threshold = 4.0 # Time threshold for stuck detection (seconds) + self.is_recovery_active = False # Whether recovery behavior is active + self.recovery_start_time = 0.0 # When recovery behavior started + self.recovery_duration = 8.0 # How long to run recovery before giving up (seconds) + self.last_update_time = time.time() # Last time position was updated + self.navigation_failed = False # Flag indicating if navigation should be terminated + + def reset(self): + """ + Reset all navigation and state tracking variables. + Should be called whenever a new goal is set. + """ + # Reset stuck detection state + self.position_history.clear() + self.is_recovery_active = False + self.recovery_start_time = 0.0 + self.last_update_time = time.time() + + # Reset navigation state flags + self.navigation_failed = False + self.position_reached = False + self.final_goal_reached = False + self.ignore_obstacles = False + + logger.info("Local planner state has been reset") + + def set_goal( + self, goal_xy: VectorLike, frame: str = "odom", goal_theta: Optional[float] = None + ): + """Set a single goal position, converting to odom frame if necessary. + This clears any existing waypoints being followed. + + Args: + goal_xy: The goal position to set. + frame: The frame of the goal position. + goal_theta: Optional goal orientation in radians (in the specified frame) + """ + # Reset all state variables + self.reset() + + # Clear waypoint following state + self.waypoints = None + self.current_waypoint_index = 0 + self.goal_xy = None # Clear previous goal + self.goal_theta = None # Clear previous goal orientation + + target_goal_xy: Optional[Tuple[float, float]] = None + + target_goal_xy = self.transform.transform_point( + goal_xy, source_frame=frame, target_frame="odom" + ).to_tuple() + + logger.info( + f"Goal set directly in odom frame: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" + ) + + # Check if goal is valid (in bounds and not colliding) + if not self.is_goal_in_costmap_bounds(target_goal_xy) or self.check_goal_collision( + target_goal_xy + ): + logger.warning( + "Goal is in collision or out of bounds. Adjusting goal to valid position." + ) + self.goal_xy = self.adjust_goal_to_valid_position(target_goal_xy) + else: + self.goal_xy = target_goal_xy # Set the adjusted or original valid goal + + # Set goal orientation if provided + if goal_theta is not None: + transformed_rot = self.transform.transform_rot( + Vector(0.0, 0.0, goal_theta), source_frame=frame, target_frame="odom" + ) + self.goal_theta = transformed_rot[2] + + def set_goal_waypoints( + self, waypoints: Path, frame: str = "map", goal_theta: Optional[float] = None + ): + """Sets a path of waypoints for the robot to follow. + + Args: + waypoints: A list of waypoints to follow. Each waypoint is a tuple of (x, y) coordinates in odom frame. + frame: The frame of the waypoints. + goal_theta: Optional final orientation in radians (in the specified frame) + """ + # Reset all state variables + self.reset() + + if not isinstance(waypoints, Path) or len(waypoints) == 0: + logger.warning("Invalid or empty path provided to set_goal_waypoints. Ignoring.") + self.waypoints = None + self.waypoint_frame = None + self.goal_xy = None + self.goal_theta = None + self.current_waypoint_index = 0 + return + + logger.info(f"Setting goal waypoints with {len(waypoints)} points.") + self.waypoints = waypoints + self.waypoint_frame = frame + self.current_waypoint_index = 0 + + # Transform waypoints to odom frame + self.waypoints_in_odom = self.transform.transform_path( + self.waypoints, source_frame=frame, target_frame="odom" + ) + + # Set the initial target to the first waypoint, adjusting if necessary + first_waypoint = self.waypoints_in_odom[0] + if not self.is_goal_in_costmap_bounds(first_waypoint) or self.check_goal_collision( + first_waypoint + ): + logger.warning("First waypoint is invalid. Adjusting...") + self.goal_xy = self.adjust_goal_to_valid_position(first_waypoint) + else: + self.goal_xy = to_tuple(first_waypoint) # Initial target + + # Set goal orientation if provided + if goal_theta is not None: + transformed_rot = self.transform.transform_rot( + Vector(0.0, 0.0, goal_theta), source_frame=frame, target_frame="odom" + ) + self.goal_theta = transformed_rot[2] + + def _get_robot_pose(self) -> Tuple[Tuple[float, float], float]: + """ + Get the current robot position and orientation. + + Returns: + Tuple containing: + - position as (x, y) tuple + - orientation (theta) in radians + """ + [pos, rot] = self.transform.transform_euler("base_link", "odom") + return (pos[0], pos[1]), rot[2] + + def _get_final_goal_position(self) -> Optional[Tuple[float, float]]: + """ + Get the final goal position (either last waypoint or direct goal). + + Returns: + Tuple (x, y) of the final goal, or None if no goal is set + """ + if self.waypoints_in_odom is not None and len(self.waypoints_in_odom) > 0: + return to_tuple(self.waypoints_in_odom[-1]) + elif self.goal_xy is not None: + return self.goal_xy + return None + + def _distance_to_position(self, target_position: Tuple[float, float]) -> float: + """ + Calculate distance from the robot to a target position. + + Args: + target_position: Target (x, y) position + + Returns: + Distance in meters + """ + robot_pos, _ = self._get_robot_pose() + return np.linalg.norm( + [target_position[0] - robot_pos[0], target_position[1] - robot_pos[1]] + ) + + def plan(self) -> Dict[str, float]: + """ + Main planning method that computes velocity commands. + This includes common planning logic like waypoint following, + with algorithm-specific calculations delegated to subclasses. + + Returns: + Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys + """ + # If goal orientation is specified, rotate to match it + if ( + self.position_reached + and self.goal_theta is not None + and not self._is_goal_orientation_reached() + ): + logger.info("Position goal reached. Rotating to target orientation.") + return self._rotate_to_goal_orientation() + + # Check if the robot is stuck and handle accordingly + if self.check_if_stuck() and not self.position_reached: + # Check if we're stuck but close to our goal + final_goal_pos = self._get_final_goal_position() + + # If we have a goal position, check distance to it + if final_goal_pos is not None: + distance_to_goal = self._distance_to_position(final_goal_pos) + + # If we're stuck but within 2x safe_goal_distance of the goal, consider it a success + if distance_to_goal < 2.0 * self.safe_goal_distance: + logger.info( + f"Robot is stuck but within {distance_to_goal:.2f}m of goal (< {2.0 * self.safe_goal_distance:.2f}m). Considering navigation successful." + ) + self.position_reached = True + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Otherwise, execute normal recovery behavior + logger.warning("Robot is stuck - executing recovery behavior") + return self.execute_recovery_behavior() + + # Reset obstacle ignore flag + self.ignore_obstacles = False + + # --- Waypoint Following Mode --- + if self.waypoints is not None: + if self.final_goal_reached: + logger.info("Final waypoint reached. Stopping.") + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Get current robot pose + robot_pos, robot_theta = self._get_robot_pose() + robot_pos_np = np.array(robot_pos) + + # Check if close to final waypoint + if self.waypoints_in_odom is not None and len(self.waypoints_in_odom) > 0: + final_waypoint = self.waypoints_in_odom[-1] + dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) + + # If we're close to the final waypoint, adjust it and ignore obstacles + if dist_to_final < self.safe_goal_distance: + final_wp_tuple = to_tuple(final_waypoint) + adjusted_goal = self.adjust_goal_to_valid_position(final_wp_tuple) + # Create a new Path with the adjusted final waypoint + new_waypoints = self.waypoints_in_odom[:-1] # Get all but the last waypoint + new_waypoints.append(adjusted_goal) # Append the adjusted goal + self.waypoints_in_odom = new_waypoints + self.ignore_obstacles = True + logger.debug("Within safe distance of final waypoint. Ignoring obstacles.") + + # Update the target goal based on waypoint progression + just_reached_final = self._update_waypoint_target(robot_pos_np) + + # If the helper indicates the final goal was just reached, stop immediately + if just_reached_final: + return {"x_vel": 0.0, "angular_vel": 0.0} + + # --- Single Goal or Current Waypoint Target Set --- + if self.goal_xy is None: + # If no goal is set (e.g., empty path or rejected goal), stop. + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Get necessary data for planning + costmap = self.get_costmap() + if costmap is None: + logger.warning("Local costmap is None. Cannot plan.") + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Check if close to single goal mode goal + if self.waypoints is None: + # Get distance to goal + goal_distance = self._distance_to_position(self.goal_xy) + + # If within safe distance of goal, adjust it and ignore obstacles + if goal_distance < self.safe_goal_distance: + self.goal_xy = self.adjust_goal_to_valid_position(self.goal_xy) + self.ignore_obstacles = True + logger.debug("Within safe distance of goal. Ignoring obstacles.") + + # First check position + if goal_distance < self.goal_tolerance or self.position_reached: + self.position_reached = True + + else: + self.position_reached = False + + # Call the algorithm-specific planning implementation + return self._compute_velocity_commands() + + @abstractmethod + def _compute_velocity_commands(self) -> Dict[str, float]: + """ + Algorithm-specific method to compute velocity commands. + Must be implemented by derived classes. + + Returns: + Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys + """ + pass + + def _rotate_to_goal_orientation(self) -> Dict[str, float]: + """Compute velocity commands to rotate to the goal orientation. + + Returns: + Dict[str, float]: Velocity commands with zero linear velocity + """ + # Get current robot orientation + _, robot_theta = self._get_robot_pose() + + # Calculate the angle difference + angle_diff = normalize_angle(self.goal_theta - robot_theta) + + # Determine rotation direction and speed + if abs(angle_diff) < self.angle_tolerance: + # Already at correct orientation + return {"x_vel": 0.0, "angular_vel": 0.0} + + # Calculate rotation speed - proportional to the angle difference + # but capped at max_angular_vel + direction = 1.0 if angle_diff > 0 else -1.0 + angular_vel = direction * min(abs(angle_diff) * 2.0, self.max_angular_vel) + + # logger.debug(f"Rotating to goal orientation: angle_diff={angle_diff:.4f}, angular_vel={angular_vel:.4f}") + return {"x_vel": 0.0, "angular_vel": angular_vel} + + def _is_goal_orientation_reached(self) -> bool: + """Check if the current robot orientation matches the goal orientation. + + Returns: + bool: True if orientation is reached or no orientation goal is set + """ + if self.goal_theta is None: + return True # No orientation goal set + + # Get current robot orientation + _, robot_theta = self._get_robot_pose() + + # Calculate the angle difference and normalize + angle_diff = abs(normalize_angle(self.goal_theta - robot_theta)) + + logger.debug( + f"Orientation error: {angle_diff:.4f} rad, tolerance: {self.angle_tolerance:.4f} rad" + ) + return angle_diff <= self.angle_tolerance + + def _update_waypoint_target(self, robot_pos_np: np.ndarray) -> bool: + """Helper function to manage waypoint progression and update the target goal. + + Args: + robot_pos_np: Current robot position as a numpy array [x, y]. + + Returns: + bool: True if the final waypoint has just been reached, False otherwise. + """ + if self.waypoints is None or len(self.waypoints) == 0: + return False # Not in waypoint mode or empty path + + self.waypoints_in_odom = self.transform.transform_path( + self.waypoints, source_frame=self.waypoint_frame, target_frame="odom" + ) + + # Check if final goal is reached + final_waypoint = self.waypoints_in_odom[-1] + dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) + + if dist_to_final < self.goal_tolerance: + self.position_reached = True + self.goal_xy = to_tuple(final_waypoint) + + # If goal orientation is not specified or achieved, consider fully reached + if self.goal_theta is None or self._is_goal_orientation_reached(): + self.final_goal_reached = True + logger.info("Reached final waypoint with correct orientation.") + return True + else: + logger.info("Reached final waypoint position, rotating to target orientation.") + return False + + # Always find the lookahead point + lookahead_point = None + for i in range(self.current_waypoint_index, len(self.waypoints_in_odom)): + wp = self.waypoints_in_odom[i] + dist_to_wp = np.linalg.norm(robot_pos_np - wp) + if dist_to_wp >= self.lookahead_distance: + lookahead_point = wp + # Update current waypoint index to this point + self.current_waypoint_index = i + break + + # If no point is far enough, target the final waypoint + if lookahead_point is None: + lookahead_point = self.waypoints_in_odom[-1] + self.current_waypoint_index = len(self.waypoints_in_odom) - 1 + + # Set the lookahead point as the immediate target, adjusting if needed + if not self.is_goal_in_costmap_bounds(lookahead_point) or self.check_goal_collision( + lookahead_point + ): + logger.debug("Lookahead point is invalid. Adjusting...") + adjusted_lookahead = self.adjust_goal_to_valid_position(lookahead_point) + # Only update if adjustment didn't fail completely + if adjusted_lookahead is not None: + self.goal_xy = adjusted_lookahead + else: + self.goal_xy = to_tuple(lookahead_point) + + return False # Final goal not reached in this update cycle -class LocalPlanner(Visualizable): @abstractmethod - def set_path(self, path: Path) -> bool: ... + def update_visualization(self) -> np.ndarray: + """ + Generate visualization of the planning state. + Must be implemented by derived classes. + + Returns: + np.ndarray: Visualization image as numpy array + """ + pass + + def create_stream(self, frequency_hz: float = None) -> Observable: + """ + Create an Observable stream that emits the visualization image at a fixed frequency. + + Args: + frequency_hz: Optional frequency override (defaults to 1/4 of control_frequency if None) + + Returns: + Observable: Stream of visualization frames + """ + # Default to 1/4 of control frequency if not specified (to reduce CPU usage) + if frequency_hz is None: + frequency_hz = self.control_frequency / 4.0 + + subject = Subject() + sleep_time = 1.0 / frequency_hz + + def frame_emitter(): + while True: + try: + # Generate the frame using the updated method + frame = self.update_visualization() + subject.on_next(frame) + except Exception as e: + logger.error(f"Error in frame emitter thread: {e}") + # Optionally, emit an error frame or simply skip + # subject.on_error(e) # This would terminate the stream + time.sleep(sleep_time) + + emitter_thread = threading.Thread(target=frame_emitter, daemon=True) + emitter_thread.start() + logger.info(f"Started visualization frame emitter thread at {frequency_hz:.1f} Hz") + return subject @abstractmethod - def get_move_stream(self, frequency: Optional[float]) -> Observable[Vector]: ... + def check_collision(self, direction: float) -> bool: + """ + Check if there's a collision in the given direction. + Must be implemented by derived classes. + + Args: + direction: Direction to check for collision in radians + + Returns: + bool: True if collision detected, False otherwise + """ + pass + + def is_goal_reached(self) -> bool: + """Check if the final goal (single or last waypoint) is reached, including orientation.""" + if self.waypoints is not None: + # Waypoint mode: check if the final waypoint and orientation have been reached + return self.final_goal_reached + else: + # Single goal mode: check distance to the single goal and orientation + if self.goal_xy is None: + return False # No goal set + + return self.position_reached and self._is_goal_orientation_reached() + + def check_goal_collision(self, goal_xy: VectorLike) -> bool: + """Check if the current goal is in collision with obstacles in the costmap. + + Returns: + bool: True if goal is in collision, False if goal is safe or cannot be checked + """ + + costmap = self.get_costmap() + if costmap is None: + logger.warning("Cannot check collision: No costmap available") + return False + + # Check if the position is occupied + collision_threshold = 80 # Consider values above 80 as obstacles + + # Use Costmap's is_occupied method + return costmap.is_occupied(goal_xy, threshold=collision_threshold) + + def is_goal_in_costmap_bounds(self, goal_xy: VectorLike) -> bool: + """Check if the goal position is within the bounds of the costmap. + + Args: + goal_xy: Goal position (x, y) in odom frame + + Returns: + bool: True if the goal is within the costmap bounds, False otherwise + """ + costmap = self.get_costmap() + if costmap is None: + logger.warning("Cannot check bounds: No costmap available") + return False + + # Get goal position in grid coordinates + goal_point = costmap.world_to_grid(goal_xy) + goal_cell_x, goal_cell_y = goal_point.x, goal_point.y + + # Check if goal is within the costmap bounds + is_in_bounds = 0 <= goal_cell_x < costmap.width and 0 <= goal_cell_y < costmap.height + + if not is_in_bounds: + logger.warning(f"Goal ({goal_xy[0]:.2f}, {goal_xy[1]:.2f}) is outside costmap bounds") + + return is_in_bounds + + def adjust_goal_to_valid_position( + self, goal_xy: VectorLike, clearance: float = 0.5 + ) -> Tuple[float, float]: + """Find a valid (non-colliding) goal position by moving it towards the robot. + + Args: + goal_xy: Original goal position (x, y) in odom frame + clearance: Additional distance to move back from obstacles for better clearance (meters) + + Returns: + Tuple[float, float]: A valid goal position, or the original goal if already valid + """ + [pos, rot] = self.transform.transform_euler("base_link", "odom") + + robot_x, robot_y = pos[0], pos[1] + + # Original goal + goal_x, goal_y = to_tuple(goal_xy) + + if not self.check_goal_collision((goal_x, goal_y)): + return (goal_x, goal_y) + + # Calculate vector from goal to robot + dx = robot_x - goal_x + dy = robot_y - goal_y + distance = np.sqrt(dx * dx + dy * dy) + + if distance < 0.001: # Goal is at robot position + return to_tuple(goal_xy) + + # Normalize direction vector + dx /= distance + dy /= distance + + # Step size + step_size = 0.25 # meters + + # Move goal towards robot step by step + current_x, current_y = goal_x, goal_y + steps = 0 + max_steps = 50 # Safety limit + + # Variables to store the first valid position found + valid_found = False + valid_x, valid_y = None, None + + while steps < max_steps: + # Move towards robot + current_x += dx * step_size + current_y += dy * step_size + steps += 1 + + # Check if we've reached or passed the robot + new_distance = np.sqrt((current_x - robot_x) ** 2 + (current_y - robot_y) ** 2) + if new_distance < step_size: + # We've reached the robot without finding a valid point + # Move back one step from robot to avoid self-collision + current_x = robot_x - dx * step_size + current_y = robot_y - dy * step_size + break + + # Check if this position is valid + if not self.check_goal_collision( + (current_x, current_y) + ) and self.is_goal_in_costmap_bounds((current_x, current_y)): + # Store the first valid position + if not valid_found: + valid_found = True + valid_x, valid_y = current_x, current_y + + # If clearance is requested, continue searching for a better position + if clearance > 0: + continue + + # Calculate position with additional clearance + if clearance > 0: + # Calculate clearance position + clearance_x = current_x + dx * clearance + clearance_y = current_y + dy * clearance + + logger.info( + f"Checking clearance position at ({clearance_x:.2f}, {clearance_y:.2f})" + ) + + # Check if the clearance position is also valid + if not self.check_goal_collision( + (clearance_x, clearance_y) + ) and self.is_goal_in_costmap_bounds((clearance_x, clearance_y)): + logger.info( + f"Found valid goal with clearance at ({clearance_x:.2f}, {clearance_y:.2f})" + ) + return (clearance_x, clearance_y) + + # Return the valid position without clearance + logger.info(f"Found valid goal at ({current_x:.2f}, {current_y:.2f})") + return (current_x, current_y) + + # If we found a valid position earlier but couldn't add clearance + if valid_found: + logger.info(f"Using valid goal found at ({valid_x:.2f}, {valid_y:.2f})") + return (valid_x, valid_y) + + logger.warning( + f"Could not find valid goal after {steps} steps, using closest point to robot" + ) + return (current_x, current_y) + + def check_if_stuck(self) -> bool: + """ + Check if the robot is stuck by analyzing movement history. + + Returns: + bool: True if the robot is determined to be stuck, False otherwise + """ + # Get current position and time + current_time = time.time() + + # Get current robot position + [pos, _] = self.transform.transform_euler("base_link", "odom") + current_position = (pos[0], pos[1], current_time) + + # Add current position to history (newest is appended at the end) + self.position_history.append(current_position) + + # Need enough history to make a determination + min_history_size = self.stuck_detection_window_seconds * self.control_frequency + if len(self.position_history) < min_history_size: + return False + + # Find positions within our detection window (positions are already in order from oldest to newest) + window_start_time = current_time - self.stuck_detection_window_seconds + window_positions = [] + + # Collect positions within the window (newest entries will be at the end) + for pos_x, pos_y, timestamp in self.position_history: + if timestamp >= window_start_time: + window_positions.append((pos_x, pos_y, timestamp)) + + # Need at least a few positions in the window + if len(window_positions) < 3: + return False + + # Ensure correct order: oldest to newest + window_positions.sort(key=lambda p: p[2]) + + # Get the oldest and newest positions in the window + oldest_x, oldest_y, oldest_time = window_positions[0] + newest_x, newest_y, newest_time = window_positions[-1] + + # Calculate time range in the window (should always be positive) + time_range = newest_time - oldest_time + + # Calculate displacement from oldest to newest position + displacement = np.sqrt((newest_x - oldest_x) ** 2 + (newest_y - oldest_y) ** 2) + + # Check if we're stuck - moved less than threshold over minimum time + # Only consider it if the time range makes sense (positive and sufficient) + is_currently_stuck = ( + time_range >= self.stuck_time_threshold + and time_range <= self.stuck_detection_window_seconds + and displacement < self.stuck_distance_threshold + ) + + if is_currently_stuck: + logger.warning( + f"Robot appears to be stuck! Displacement {displacement:.3f}m over {time_range:.1f}s" + ) + + # Don't trigger recovery if it's already active + if not self.is_recovery_active: + self.is_recovery_active = True + self.recovery_start_time = current_time + return True + + # Check if we've been trying to recover for too long + elif current_time - self.recovery_start_time > self.recovery_duration: + logger.error( + f"Recovery behavior has been active for {self.recovery_duration}s without success" + ) + # Reset recovery state - maybe a different behavior will work + self.is_recovery_active = False + self.recovery_start_time = current_time + + # If we've moved enough, we're not stuck anymore + elif self.is_recovery_active and displacement > self.unstuck_distance_threshold: + logger.info(f"Robot has escaped from stuck state (moved {displacement:.3f}m)") + self.is_recovery_active = False + + return self.is_recovery_active + + def execute_recovery_behavior(self) -> Dict[str, float]: + """ + Execute a recovery behavior when the robot is stuck. + + Returns: + Dict[str, float]: Velocity commands for the recovery behavior + """ + # Calculate how long we've been in recovery + recovery_time = time.time() - self.recovery_start_time + + # Calculate recovery phases based on control frequency + backup_phase_time = 3.0 # seconds + rotate_phase_time = 2.0 # seconds + + # Simple recovery behavior state machine + if recovery_time < backup_phase_time: + # First try backing up + logger.info("Recovery: backing up") + return {"x_vel": -0.2, "angular_vel": 0.0} + elif recovery_time < backup_phase_time + rotate_phase_time: + # Then try rotating + logger.info("Recovery: rotating to find new path") + rotation_direction = 1.0 if np.random.random() > 0.5 else -1.0 + return {"x_vel": 0.0, "angular_vel": rotation_direction * self.max_angular_vel * 0.7} + else: + # If we're still stuck after backup and rotation, terminate navigation + logger.error("Recovery failed after backup and rotation. Navigation terminated.") + # Set a flag to indicate navigation should terminate + self.navigation_failed = True + # Stop the robot + return {"x_vel": 0.0, "angular_vel": 0.0} + + +def navigate_to_goal_local( + robot, + goal_xy_robot: Tuple[float, float], + goal_theta: Optional[float] = None, + distance: float = 0.0, + timeout: float = 60.0, + stop_event: Optional[threading.Event] = None, +) -> bool: + """ + Navigates the robot to a goal specified in the robot's local frame + using the local planner. + + Args: + robot: Robot instance to control + goal_xy_robot: Tuple (x, y) representing the goal position relative + to the robot's current position and orientation. + distance: Desired distance to maintain from the goal in meters. + If non-zero, the robot will stop this far away from the goal. + timeout: Maximum time (in seconds) allowed to reach the goal. + stop_event: Optional threading.Event to signal when navigation should stop + + Returns: + bool: True if the goal was reached within the timeout, False otherwise. + """ + logger.info( + f"Starting navigation to local goal {goal_xy_robot} with distance {distance}m and timeout {timeout}s." + ) + + goal_x, goal_y = goal_xy_robot + + # Calculate goal orientation to face the target + if goal_theta is None: + goal_theta = np.arctan2(goal_y, goal_x) + + # If distance is non-zero, adjust the goal to stop at the desired distance + if distance > 0: + # Calculate magnitude of the goal vector + goal_distance = np.sqrt(goal_x**2 + goal_y**2) + + # Only adjust if goal is further than the desired distance + if goal_distance > distance: + goal_x, goal_y = distance_angle_to_goal_xy(goal_distance - distance, goal_theta) + + # Set the goal in the robot's frame with orientation to face the original target + robot.local_planner.set_goal((goal_x, goal_y), frame="base_link", goal_theta=goal_theta) + + # Get control period from robot's local planner for consistent timing + control_period = 1.0 / robot.local_planner.control_frequency + + start_time = time.time() + goal_reached = False + + try: + while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): + # Check if goal has been reached + if robot.local_planner.is_goal_reached(): + logger.info("Goal reached successfully.") + goal_reached = True + break + + # Check if navigation failed flag is set + if robot.local_planner.navigation_failed: + logger.error("Navigation aborted due to repeated recovery failures.") + goal_reached = False + break + + # Get planned velocity towards the goal + vel_command = robot.local_planner.plan() + x_vel = vel_command.get("x_vel", 0.0) + angular_vel = vel_command.get("angular_vel", 0.0) + + # Send velocity command + robot.local_planner.move_vel_control(x=x_vel, y=0, yaw=angular_vel) + + # Control loop frequency - use robot's control frequency + time.sleep(control_period) + + if not goal_reached: + logger.warning(f"Navigation timed out after {timeout} seconds before reaching goal.") + + except KeyboardInterrupt: + logger.info("Navigation to local goal interrupted by user.") + goal_reached = False # Consider interruption as failure + except Exception as e: + logger.error(f"Error during navigation to local goal: {e}") + goal_reached = False # Consider error as failure + finally: + logger.info("Stopping robot after navigation attempt.") + robot.local_planner.move_vel_control(0, 0, 0) # Stop the robot + + return goal_reached + + +def navigate_path_local( + robot, + path: Path, + timeout: float = 120.0, + goal_theta: Optional[float] = None, + stop_event: Optional[threading.Event] = None, +) -> bool: + """ + Navigates the robot along a path of waypoints using the waypoint following capability + of the local planner. + + Args: + robot: Robot instance to control + path: Path object containing waypoints in odom/map frame + timeout: Maximum time (in seconds) allowed to follow the complete path + goal_theta: Optional final orientation in radians + stop_event: Optional threading.Event to signal when navigation should stop + + Returns: + bool: True if the entire path was successfully followed, False otherwise + """ + logger.info( + f"Starting navigation along path with {len(path)} waypoints and timeout {timeout}s." + ) + + # Set the path in the local planner + robot.local_planner.set_goal_waypoints(path, goal_theta=goal_theta) + + # Get control period from robot's local planner for consistent timing + control_period = 1.0 / robot.local_planner.control_frequency + + start_time = time.time() + path_completed = False + + try: + while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): + # Check if the entire path has been traversed + if robot.local_planner.is_goal_reached(): + logger.info("Path traversed successfully.") + path_completed = True + break + + # Check if navigation failed flag is set + if robot.local_planner.navigation_failed: + logger.error("Navigation aborted due to repeated recovery failures.") + path_completed = False + break + + # Get planned velocity towards the current waypoint target + vel_command = robot.local_planner.plan() + x_vel = vel_command.get("x_vel", 0.0) + angular_vel = vel_command.get("angular_vel", 0.0) + + # Send velocity command + robot.local_planner.move_vel_control(x=x_vel, y=0, yaw=angular_vel) + + # Control loop frequency - use robot's control frequency + time.sleep(control_period) + + if not path_completed: + logger.warning( + f"Path following timed out after {timeout} seconds before completing the path." + ) + + except KeyboardInterrupt: + logger.info("Path navigation interrupted by user.") + path_completed = False + except Exception as e: + logger.error(f"Error during path navigation: {e}") + path_completed = False + finally: + logger.info("Stopping robot after path navigation attempt.") + robot.local_planner.move_vel_control(0, 0, 0) # Stop the robot + + return path_completed + + +def visualize_local_planner_state( + occupancy_grid: np.ndarray, + grid_resolution: float, + grid_origin: Tuple[float, float, float], + robot_pose: Tuple[float, float, float], + visualization_size: int = 400, + robot_width: float = 0.5, + robot_length: float = 0.7, + map_size_meters: float = 10.0, + goal_xy: Optional[Tuple[float, float]] = None, + goal_theta: Optional[float] = None, + histogram: Optional[np.ndarray] = None, + selected_direction: Optional[float] = None, + waypoints: Optional["Path"] = None, + current_waypoint_index: Optional[int] = None, +) -> np.ndarray: + """Generate a bird's eye view visualization of the local costmap. + Optionally includes VFH histogram, selected direction, and waypoints path. + + Args: + occupancy_grid: 2D numpy array of the occupancy grid + grid_resolution: Resolution of the grid in meters/cell + grid_origin: Tuple (x, y, theta) of the grid origin in the odom frame + robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame + visualization_size: Size of the visualization image in pixels + robot_width: Width of the robot in meters + robot_length: Length of the robot in meters + map_size_meters: Size of the map to visualize in meters + goal_xy: Optional tuple (x, y) of the goal position in the odom frame + goal_theta: Optional goal orientation in radians (in odom frame) + histogram: Optional numpy array of the VFH histogram + selected_direction: Optional selected direction angle in radians + waypoints: Optional Path object containing waypoints to visualize + current_waypoint_index: Optional index of the current target waypoint + """ + + robot_x, robot_y, robot_theta = robot_pose + grid_origin_x, grid_origin_y, _ = grid_origin + vis_size = visualization_size + scale = vis_size / map_size_meters + + vis_img = np.ones((vis_size, vis_size, 3), dtype=np.uint8) * 255 + center_x = vis_size // 2 + center_y = vis_size // 2 + + grid_height, grid_width = occupancy_grid.shape + + # Calculate robot position relative to grid origin + robot_rel_x = robot_x - grid_origin_x + robot_rel_y = robot_y - grid_origin_y + robot_cell_x = int(robot_rel_x / grid_resolution) + robot_cell_y = int(robot_rel_y / grid_resolution) + + half_size_cells = int(map_size_meters / grid_resolution / 2) + + # Draw grid cells (using standard occupancy coloring) + for y in range( + max(0, robot_cell_y - half_size_cells), min(grid_height, robot_cell_y + half_size_cells) + ): + for x in range( + max(0, robot_cell_x - half_size_cells), min(grid_width, robot_cell_x + half_size_cells) + ): + cell_rel_x_meters = (x - robot_cell_x) * grid_resolution + cell_rel_y_meters = (y - robot_cell_y) * grid_resolution + + img_x = int(center_x + cell_rel_x_meters * scale) + img_y = int(center_y - cell_rel_y_meters * scale) # Flip y-axis + + if 0 <= img_x < vis_size and 0 <= img_y < vis_size: + cell_value = occupancy_grid[y, x] + if cell_value == -1: + color = (200, 200, 200) # Unknown (Light gray) + elif cell_value == 0: + color = (255, 255, 255) # Free (White) + else: # Occupied + # Scale darkness based on occupancy value (0-100) + darkness = 255 - int(155 * (cell_value / 100)) - 100 + color = (darkness, darkness, darkness) # Shades of gray/black + + cell_size_px = max(1, int(grid_resolution * scale)) + cv2.rectangle( + vis_img, + (img_x - cell_size_px // 2, img_y - cell_size_px // 2), + (img_x + cell_size_px // 2, img_y + cell_size_px // 2), + color, + -1, + ) + + # Draw waypoints path if provided + if waypoints is not None and len(waypoints) > 0: + try: + path_points = [] + for i, waypoint in enumerate(waypoints): + # Convert waypoint from odom frame to visualization frame + wp_x, wp_y = waypoint[0], waypoint[1] + wp_rel_x = wp_x - robot_x + wp_rel_y = wp_y - robot_y + + wp_img_x = int(center_x + wp_rel_x * scale) + wp_img_y = int(center_y - wp_rel_y * scale) # Flip y-axis + + if 0 <= wp_img_x < vis_size and 0 <= wp_img_y < vis_size: + path_points.append((wp_img_x, wp_img_y)) + + # Draw each waypoint as a small circle + cv2.circle(vis_img, (wp_img_x, wp_img_y), 3, (0, 128, 0), -1) # Dark green dots + + # Highlight current target waypoint + if current_waypoint_index is not None and i == current_waypoint_index: + cv2.circle(vis_img, (wp_img_x, wp_img_y), 6, (0, 0, 255), 2) # Red circle + + # Connect waypoints with lines to show the path + if len(path_points) > 1: + for i in range(len(path_points) - 1): + cv2.line( + vis_img, path_points[i], path_points[i + 1], (0, 200, 0), 1 + ) # Green line + except Exception as e: + logger.error(f"Error drawing waypoints: {e}") + + # Draw histogram + if histogram is not None: + num_bins = len(histogram) + # Find absolute maximum value (ignoring any negative debug values) + abs_histogram = np.abs(histogram) + max_hist_value = np.max(abs_histogram) if np.max(abs_histogram) > 0 else 1.0 + hist_scale = (vis_size / 2) * 0.8 # Scale histogram lines to 80% of half the viz size + + for i in range(num_bins): + # Angle relative to robot's forward direction + angle_relative_to_robot = (i / num_bins) * 2 * math.pi - math.pi + # Angle in the visualization frame (relative to image +X axis) + vis_angle = angle_relative_to_robot + robot_theta + + # Get the value and check if it's a special debug value (negative) + hist_val = histogram[i] + is_debug_value = hist_val < 0 + + # Use absolute value for line length + normalized_val = min(1.0, abs(hist_val) / max_hist_value) + line_length = normalized_val * hist_scale + + # Calculate endpoint using the visualization angle + end_x = int(center_x + line_length * math.cos(vis_angle)) + end_y = int(center_y - line_length * math.sin(vis_angle)) # Flipped Y + + # Color based on value and whether it's a debug value + if is_debug_value: + # Use green for debug values (minimum cost bin) + color = (0, 255, 0) # Green + line_width = 2 # Thicker line for emphasis + else: + # Regular coloring for normal values (blue to red gradient based on obstacle density) + blue = max(0, 255 - int(normalized_val * 255)) + red = min(255, int(normalized_val * 255)) + color = (blue, 0, red) # BGR format: obstacles are redder, clear areas are bluer + line_width = 1 + + cv2.line(vis_img, (center_x, center_y), (end_x, end_y), color, line_width) + + # Draw robot + robot_length_px = int(robot_length * scale) + robot_width_px = int(robot_width * scale) + robot_pts = np.array( + [ + [-robot_length_px / 2, -robot_width_px / 2], + [robot_length_px / 2, -robot_width_px / 2], + [robot_length_px / 2, robot_width_px / 2], + [-robot_length_px / 2, robot_width_px / 2], + ], + dtype=np.float32, + ) + rotation_matrix = np.array( + [ + [math.cos(robot_theta), -math.sin(robot_theta)], + [math.sin(robot_theta), math.cos(robot_theta)], + ] + ) + robot_pts = np.dot(robot_pts, rotation_matrix.T) + robot_pts[:, 0] += center_x + robot_pts[:, 1] = center_y - robot_pts[:, 1] # Flip y-axis + cv2.fillPoly( + vis_img, [robot_pts.reshape((-1, 1, 2)).astype(np.int32)], (0, 0, 255) + ) # Red robot + + # Draw robot direction line + front_x = int(center_x + (robot_length_px / 2) * math.cos(robot_theta)) + front_y = int(center_y - (robot_length_px / 2) * math.sin(robot_theta)) + cv2.line(vis_img, (center_x, center_y), (front_x, front_y), (255, 0, 0), 2) # Blue line + + # Draw selected direction + if selected_direction is not None: + # selected_direction is relative to robot frame + # Angle in the visualization frame (relative to image +X axis) + vis_angle_selected = selected_direction + robot_theta + + # Make slightly longer than max histogram line + sel_dir_line_length = (vis_size / 2) * 0.9 + + sel_end_x = int(center_x + sel_dir_line_length * math.cos(vis_angle_selected)) + sel_end_y = int(center_y - sel_dir_line_length * math.sin(vis_angle_selected)) # Flipped Y + + cv2.line( + vis_img, (center_x, center_y), (sel_end_x, sel_end_y), (0, 165, 255), 2 + ) # BGR for Orange + + # Draw goal + if goal_xy is not None: + goal_x, goal_y = goal_xy + goal_rel_x_map = goal_x - robot_x + goal_rel_y_map = goal_y - robot_y + goal_img_x = int(center_x + goal_rel_x_map * scale) + goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis + if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: + cv2.circle(vis_img, (goal_img_x, goal_img_y), 5, (0, 255, 0), -1) # Green circle + cv2.circle(vis_img, (goal_img_x, goal_img_y), 8, (0, 0, 0), 1) # Black outline + + # Draw goal orientation + if goal_theta is not None and goal_xy is not None: + # For waypoint mode, only draw orientation at the final waypoint + if waypoints is not None and len(waypoints) > 0: + # Use the final waypoint position + final_waypoint = waypoints[-1] + goal_x, goal_y = final_waypoint[0], final_waypoint[1] + else: + # Use the current goal position + goal_x, goal_y = goal_xy + + goal_rel_x_map = goal_x - robot_x + goal_rel_y_map = goal_y - robot_y + goal_img_x = int(center_x + goal_rel_x_map * scale) + goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis + + # Calculate goal orientation vector direction in visualization frame + # goal_theta is already in odom frame, need to adjust for visualization orientation + goal_dir_length = 30 # Length of direction indicator in pixels + goal_dir_end_x = int(goal_img_x + goal_dir_length * math.cos(goal_theta)) + goal_dir_end_y = int(goal_img_y - goal_dir_length * math.sin(goal_theta)) # Flip y-axis + + # Draw goal orientation arrow + if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: + cv2.arrowedLine( + vis_img, + (goal_img_x, goal_img_y), + (goal_dir_end_x, goal_dir_end_y), + (255, 0, 255), + 4, + ) # Magenta arrow + + # Add scale bar + scale_bar_length_px = int(1.0 * scale) + scale_bar_x = vis_size - scale_bar_length_px - 10 + scale_bar_y = vis_size - 20 + cv2.line( + vis_img, + (scale_bar_x, scale_bar_y), + (scale_bar_x + scale_bar_length_px, scale_bar_y), + (0, 0, 0), + 2, + ) + cv2.putText( + vis_img, "1m", (scale_bar_x, scale_bar_y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 0), 1 + ) + + # Add status info + status_text = [] + if waypoints is not None: + if current_waypoint_index is not None: + status_text.append(f"WP: {current_waypoint_index}/{len(waypoints)}") + else: + status_text.append(f"WPs: {len(waypoints)}") + + y_pos = 20 + for text in status_text: + cv2.putText(vis_img, text, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) + y_pos += 20 + + return vis_img diff --git a/dimos/robot/local_planner/vfh_local_planner.py b/dimos/robot/local_planner/vfh_local_planner.py new file mode 100644 index 0000000000..d133c3ade8 --- /dev/null +++ b/dimos/robot/local_planner/vfh_local_planner.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python3 + +import numpy as np +from typing import Dict, Tuple, Optional, Callable +import cv2 +import logging + +from dimos.utils.logging_config import setup_logger +from dimos.utils.ros_utils import normalize_angle + +from dimos.robot.local_planner.local_planner import BaseLocalPlanner, visualize_local_planner_state +from dimos.types.costmap import Costmap +from nav_msgs.msg import OccupancyGrid + +logger = setup_logger("dimos.robot.unitree.vfh_local_planner", level=logging.DEBUG) + + +class VFHPurePursuitPlanner(BaseLocalPlanner): + """ + A local planner that combines Vector Field Histogram (VFH) for obstacle avoidance + with Pure Pursuit for goal tracking. + """ + + def __init__( + self, + get_costmap: Callable[[], Optional[OccupancyGrid]], + transform: object, + move_vel_control: Callable[[float, float, float], None], + safety_threshold: float = 0.8, + histogram_bins: int = 144, + max_linear_vel: float = 0.8, + max_angular_vel: float = 1.0, + lookahead_distance: float = 1.0, + goal_tolerance: float = 0.2, + angle_tolerance: float = 0.1, # ~5.7 degrees + robot_width: float = 0.5, + robot_length: float = 0.7, + visualization_size: int = 400, + control_frequency: float = 10.0, + safe_goal_distance: float = 1.0, + ): + """ + Initialize the VFH + Pure Pursuit planner. + + Args: + get_costmap: Function to get the latest local costmap + transform: Object with transform methods (transform_point, transform_rot, etc.) + move_vel_control: Function to send velocity commands + safety_threshold: Distance to maintain from obstacles (meters) + histogram_bins: Number of directional bins in the polar histogram + max_linear_vel: Maximum linear velocity (m/s) + max_angular_vel: Maximum angular velocity (rad/s) + lookahead_distance: Lookahead distance for pure pursuit (meters) + goal_tolerance: Distance at which the goal is considered reached (meters) + angle_tolerance: Angle at which the goal orientation is considered reached (radians) + robot_width: Width of the robot for visualization (meters) + robot_length: Length of the robot for visualization (meters) + visualization_size: Size of the visualization image in pixels + control_frequency: Frequency at which the planner is called (Hz) + safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) + """ + # Initialize base class + super().__init__( + get_costmap=get_costmap, + transform=transform, + move_vel_control=move_vel_control, + safety_threshold=safety_threshold, + max_linear_vel=max_linear_vel, + max_angular_vel=max_angular_vel, + lookahead_distance=lookahead_distance, + goal_tolerance=goal_tolerance, + angle_tolerance=angle_tolerance, + robot_width=robot_width, + robot_length=robot_length, + visualization_size=visualization_size, + control_frequency=control_frequency, + safe_goal_distance=safe_goal_distance, + ) + + # VFH specific parameters + self.histogram_bins = histogram_bins + self.histogram = None + self.selected_direction = None + + # VFH tuning parameters + self.alpha = 0.2 # Histogram smoothing factor + self.obstacle_weight = 10.0 + self.goal_weight = 1.0 + self.prev_direction_weight = 0.5 + self.prev_selected_angle = 0.0 + self.prev_linear_vel = 0.0 + self.linear_vel_filter_factor = 0.4 + self.low_speed_nudge = 0.1 + + # Add after other initialization + self.angle_mapping = np.linspace(-np.pi, np.pi, self.histogram_bins, endpoint=False) + self.smoothing_kernel = np.array([self.alpha, (1 - 2 * self.alpha), self.alpha]) + + def _compute_velocity_commands(self) -> Dict[str, float]: + """ + VFH + Pure Pursuit specific implementation of velocity command computation. + + Returns: + Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys + """ + # Get necessary data for planning + costmap = self.get_costmap() + if costmap is None: + logger.warning("No costmap available for planning") + return {"x_vel": 0.0, "angular_vel": 0.0} + + [pos, rot] = self.transform.transform_euler("base_link", "odom") + robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + robot_pose = (robot_x, robot_y, robot_theta) + + # Calculate goal-related parameters + goal_x, goal_y = self.goal_xy + dx = goal_x - robot_x + dy = goal_y - robot_y + goal_distance = np.linalg.norm([dx, dy]) + goal_direction = np.arctan2(dy, dx) - robot_theta + goal_direction = normalize_angle(goal_direction) + + self.histogram = self.build_polar_histogram(costmap, robot_pose) + + # If we're ignoring obstacles near the goal, zero out the histogram + if self.ignore_obstacles: + self.histogram = np.zeros_like(self.histogram) + + self.selected_direction = self.select_direction( + self.goal_weight, + self.obstacle_weight, + self.prev_direction_weight, + self.histogram, + goal_direction, + ) + + # Calculate Pure Pursuit Velocities + linear_vel, angular_vel = self.compute_pure_pursuit(goal_distance, self.selected_direction) + + # Slow down when turning sharply + if abs(self.selected_direction) > 0.25: # ~15 degrees + # Scale from 1.0 (small turn) to 0.5 (sharp turn at 90 degrees or more) + turn_factor = max(0.25, 1.0 - (abs(self.selected_direction) / (np.pi / 2))) + linear_vel *= turn_factor + + # Apply Collision Avoidance Stop - skip if ignoring obstacles + if not self.ignore_obstacles and self.check_collision( + self.selected_direction, safety_threshold=0.5 + ): + # Re-select direction prioritizing obstacle avoidance if colliding + self.selected_direction = self.select_direction( + self.goal_weight * 0.2, + self.obstacle_weight, + self.prev_direction_weight * 0.2, + self.histogram, + goal_direction, + ) + linear_vel, angular_vel = self.compute_pure_pursuit( + goal_distance, self.selected_direction + ) + + if self.check_collision(0.0, safety_threshold=self.safety_threshold): + logger.warning("Collision detected ahead. Stopping.") + linear_vel = 0.0 + + self.prev_linear_vel = linear_vel + filtered_linear_vel = self.prev_linear_vel * self.linear_vel_filter_factor + linear_vel * ( + 1 - self.linear_vel_filter_factor + ) + + return {"x_vel": filtered_linear_vel, "angular_vel": angular_vel} + + def _smooth_histogram(self, histogram: np.ndarray) -> np.ndarray: + """ + Apply advanced smoothing to the polar histogram to better identify valleys + and reduce noise. + + Args: + histogram: Raw histogram to smooth + + Returns: + np.ndarray: Smoothed histogram + """ + # Apply a windowed average with variable width based on obstacle density + smoothed = np.zeros_like(histogram) + bins = len(histogram) + + # First pass: basic smoothing with a 5-point kernel + # This uses a wider window than the original 3-point smoother + for i in range(bins): + # Compute indices with wrap-around + indices = [(i + j) % bins for j in range(-2, 3)] + # Apply weighted average (more weight to the center) + weights = [0.1, 0.2, 0.4, 0.2, 0.1] # Sum = 1.0 + smoothed[i] = sum(histogram[idx] * weight for idx, weight in zip(indices, weights)) + + # Second pass: peak and valley enhancement + enhanced = np.zeros_like(smoothed) + for i in range(bins): + # Check neighboring values + prev_idx = (i - 1) % bins + next_idx = (i + 1) % bins + + # Enhance valleys (low values) + if smoothed[i] < smoothed[prev_idx] and smoothed[i] < smoothed[next_idx]: + # It's a local minimum - make it even lower + enhanced[i] = smoothed[i] * 0.8 + # Enhance peaks (high values) + elif smoothed[i] > smoothed[prev_idx] and smoothed[i] > smoothed[next_idx]: + # It's a local maximum - make it even higher + enhanced[i] = min(1.0, smoothed[i] * 1.2) + else: + enhanced[i] = smoothed[i] + + return enhanced + + def build_polar_histogram(self, costmap: Costmap, robot_pose: Tuple[float, float, float]): + """ + Build a polar histogram of obstacle densities around the robot. + + Args: + costmap: Costmap object with grid and metadata + robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame + + Returns: + np.ndarray: Polar histogram of obstacle densities + """ + + # Get grid and find all obstacle cells + occupancy_grid = costmap.grid + y_indices, x_indices = np.where(occupancy_grid > 0) + if len(y_indices) == 0: # No obstacles + return np.zeros(self.histogram_bins) + + # Get robot position in grid coordinates + robot_x, robot_y, robot_theta = robot_pose + robot_point = costmap.world_to_grid((robot_x, robot_y)) + robot_cell_x, robot_cell_y = robot_point.x, robot_point.y + + # Vectorized distance and angle calculation + dx_cells = x_indices - robot_cell_x + dy_cells = y_indices - robot_cell_y + distances = np.sqrt(dx_cells**2 + dy_cells**2) * costmap.resolution + angles_grid = np.arctan2(dy_cells, dx_cells) + angles_robot = normalize_angle(angles_grid - robot_theta) + + # Convert to bin indices + bin_indices = ((angles_robot + np.pi) / (2 * np.pi) * self.histogram_bins).astype( + int + ) % self.histogram_bins + + # Get obstacle values + obstacle_values = occupancy_grid[y_indices, x_indices] / 100.0 + + # Build histogram + histogram = np.zeros(self.histogram_bins) + mask = distances > 0 + # Weight obstacles by inverse square of distance and cell value + np.add.at(histogram, bin_indices[mask], obstacle_values[mask] / (distances[mask] ** 2)) + + # Apply the enhanced smoothing + return self._smooth_histogram(histogram) + + def select_direction( + self, goal_weight, obstacle_weight, prev_direction_weight, histogram, goal_direction + ): + """ + Select best direction based on a simple weighted cost function. + + Args: + goal_weight: Weight for the goal direction component + obstacle_weight: Weight for the obstacle avoidance component + prev_direction_weight: Weight for previous direction consistency + histogram: Polar histogram of obstacle density + goal_direction: Desired direction to goal + + Returns: + float: Selected direction in radians + """ + # Normalize histogram if needed + if np.max(histogram) > 0: + histogram = histogram / np.max(histogram) + + # Calculate costs for each possible direction + angle_diffs = np.abs(normalize_angle(self.angle_mapping - goal_direction)) + prev_diffs = np.abs(normalize_angle(self.angle_mapping - self.prev_selected_angle)) + + # Combine costs with weights + obstacle_costs = obstacle_weight * histogram + goal_costs = goal_weight * angle_diffs + prev_costs = prev_direction_weight * prev_diffs + + total_costs = obstacle_costs + goal_costs + prev_costs + + # Select direction with lowest cost + min_cost_idx = np.argmin(total_costs) + selected_angle = self.angle_mapping[min_cost_idx] + + # Update history for next iteration + self.prev_selected_angle = selected_angle + + return selected_angle + + def compute_pure_pursuit( + self, goal_distance: float, goal_direction: float + ) -> Tuple[float, float]: + """Compute pure pursuit velocities.""" + if goal_distance < self.goal_tolerance: + return 0.0, 0.0 + + lookahead = min(self.lookahead_distance, goal_distance) + linear_vel = min(self.max_linear_vel, goal_distance) + angular_vel = 2.0 * np.sin(goal_direction) / lookahead + angular_vel = max(-self.max_angular_vel, min(angular_vel, self.max_angular_vel)) + + return linear_vel, angular_vel + + def check_collision(self, selected_direction: float, safety_threshold: float = 1.0) -> bool: + """Check if there's an obstacle in the selected direction within safety threshold.""" + # Skip collision check if ignoring obstacles + if self.ignore_obstacles: + return False + + # Get the latest costmap and robot pose + costmap = self.get_costmap() + if costmap is None: + return False # No costmap available + + [pos, rot] = self.transform.transform_euler("base_link", "odom") + robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + + # Direction in world frame + direction_world = robot_theta + selected_direction + + # Safety distance in cells + safety_cells = int(safety_threshold / costmap.resolution) + + # Get robot position in grid coordinates + robot_point = costmap.world_to_grid((robot_x, robot_y)) + robot_cell_x, robot_cell_y = robot_point.x, robot_point.y + + # Check for obstacles along the selected direction + for dist in range(1, safety_cells + 1): + # Calculate cell position + cell_x = robot_cell_x + int(dist * np.cos(direction_world)) + cell_y = robot_cell_y + int(dist * np.sin(direction_world)) + + # Check if cell is within grid bounds + if not (0 <= cell_x < costmap.width and 0 <= cell_y < costmap.height): + continue + + # Check if cell contains an obstacle (threshold at 50) + if costmap.grid[int(cell_y), int(cell_x)] > 50: + return True + + return False # No collision detected + + def update_visualization(self) -> np.ndarray: + """Generate visualization of the planning state.""" + try: + costmap = self.get_costmap() + if costmap is None: + raise ValueError("Costmap is None") + + [pos, rot] = self.transform.transform_euler("base_link", "odom") + robot_x, robot_y, robot_theta = pos[0], pos[1], rot[2] + robot_pose = (robot_x, robot_y, robot_theta) + + goal_xy = self.goal_xy # This could be a lookahead point or final goal + + # Get the latest histogram and selected direction, if available + histogram = getattr(self, "histogram", None) + selected_direction = getattr(self, "selected_direction", None) + + # Get waypoint data if in waypoint mode + waypoints_to_draw = self.waypoints_in_odom + current_wp_index_to_draw = ( + self.current_waypoint_index if self.waypoints_in_odom is not None else None + ) + # Ensure index is valid before passing + if waypoints_to_draw is not None and current_wp_index_to_draw is not None: + if not (0 <= current_wp_index_to_draw < len(waypoints_to_draw)): + current_wp_index_to_draw = None # Invalidate index if out of bounds + + return visualize_local_planner_state( + occupancy_grid=costmap.grid, + grid_resolution=costmap.resolution, + grid_origin=(costmap.origin.x, costmap.origin.y, costmap.origin_theta), + robot_pose=robot_pose, + goal_xy=goal_xy, # Current target (lookahead or final) + goal_theta=self.goal_theta, # Pass goal orientation if available + visualization_size=self.visualization_size, + robot_width=self.robot_width, + robot_length=self.robot_length, + histogram=histogram, + selected_direction=selected_direction, + waypoints=waypoints_to_draw, # Pass the full path + current_waypoint_index=current_wp_index_to_draw, # Pass the target index + ) + except Exception as e: + logger.error(f"Error during visualization update: {e}") + # Return a blank image with error text + blank = ( + np.ones((self.visualization_size, self.visualization_size, 3), dtype=np.uint8) * 255 + ) + cv2.putText( + blank, + "Viz Error", + (self.visualization_size // 4, self.visualization_size // 2), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (0, 0, 0), + 2, + ) + return blank From 7c44d47e796240aa8dbc41bd72515402f04574ba Mon Sep 17 00:00:00 2001 From: leshy <681516+leshy@users.noreply.github.com> Date: Tue, 3 Jun 2025 14:06:15 +0000 Subject: [PATCH 64/72] CI code cleanup --- dimos/robot/local_planner/simple.py | 14 ++++++++++++++ dimos/robot/local_planner/vfh/local_planner.py | 14 ++++++++++++++ dimos/robot/local_planner/vfh/vfh_local_planner.py | 14 ++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py index 43632d71ca..a3dc372ded 100644 --- a/dimos/robot/local_planner/simple.py +++ b/dimos/robot/local_planner/simple.py @@ -1,3 +1,17 @@ +# 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. + import math import time from datetime import datetime diff --git a/dimos/robot/local_planner/vfh/local_planner.py b/dimos/robot/local_planner/vfh/local_planner.py index 70760e164d..43594351ce 100644 --- a/dimos/robot/local_planner/vfh/local_planner.py +++ b/dimos/robot/local_planner/vfh/local_planner.py @@ -1,5 +1,19 @@ #!/usr/bin/env python3 +# 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. + import math import numpy as np from typing import Dict, Tuple, Optional, List, Any, Callable, Protocol diff --git a/dimos/robot/local_planner/vfh/vfh_local_planner.py b/dimos/robot/local_planner/vfh/vfh_local_planner.py index 09d77a9743..4c3d4d427a 100644 --- a/dimos/robot/local_planner/vfh/vfh_local_planner.py +++ b/dimos/robot/local_planner/vfh/vfh_local_planner.py @@ -1,5 +1,19 @@ #!/usr/bin/env python3 +# 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. + import math import numpy as np from typing import Dict, Tuple, Optional, Callable, Any From 96369df70dfe033da02c92f2be6a504f5a60f327 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 17:07:12 +0300 Subject: [PATCH 65/72] removed moved_out vfh planner --- dimos/robot/local_planner/vfh/__init__.py | 7 - .../robot/local_planner/vfh/local_planner.py | 1297 ----------------- .../local_planner/vfh/vfh_local_planner.py | 422 ------ 3 files changed, 1726 deletions(-) delete mode 100644 dimos/robot/local_planner/vfh/__init__.py delete mode 100644 dimos/robot/local_planner/vfh/local_planner.py delete mode 100644 dimos/robot/local_planner/vfh/vfh_local_planner.py diff --git a/dimos/robot/local_planner/vfh/__init__.py b/dimos/robot/local_planner/vfh/__init__.py deleted file mode 100644 index 22bf61986c..0000000000 --- a/dimos/robot/local_planner/vfh/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from dimos.robot.local_planner.vfh.local_planner import ( - BaseLocalPlanner, - navigate_to_goal_local, - navigate_path_local, -) - -from dimos.robot.local_planner.vfh.vfh_local_planner import VFHPurePursuitPlanner diff --git a/dimos/robot/local_planner/vfh/local_planner.py b/dimos/robot/local_planner/vfh/local_planner.py deleted file mode 100644 index 70760e164d..0000000000 --- a/dimos/robot/local_planner/vfh/local_planner.py +++ /dev/null @@ -1,1297 +0,0 @@ -#!/usr/bin/env python3 - -import math -import numpy as np -from typing import Dict, Tuple, Optional, List, Any, Callable, Protocol -from abc import ABC, abstractmethod -import cv2 -from reactivex import Observable -from reactivex.subject import Subject -import threading -import time -import logging -from collections import deque -from dimos.utils.logging_config import setup_logger -from dimos.utils.ros_utils import normalize_angle, distance_angle_to_goal_xy - -from dimos.robot.robot import Robot -from dimos.types.vector import VectorLike, Vector, to_tuple -from dimos.types.path import Path -from dimos.types.costmap import Costmap -from dimos.robot.global_planner.algo import astar -from nav_msgs.msg import OccupancyGrid - -logger = setup_logger("dimos.robot.unitree.local_planner", level=logging.DEBUG) - - -class BaseLocalPlanner(ABC): - """ - Abstract base class for local planners that handle obstacle avoidance and path following. - - This class defines the common interface and shared functionality that all local planners - must implement, regardless of the specific algorithm used. - """ - - def __init__( - self, - get_costmap: Callable[[], Optional[OccupancyGrid]], - get_robot_pose: Callable[[], Any], - move: Callable[[Vector], None], - safety_threshold: float = 0.5, - max_linear_vel: float = 0.8, - max_angular_vel: float = 1.0, - lookahead_distance: float = 1.0, - goal_tolerance: float = 0.75, - angle_tolerance: float = 0.15, - robot_width: float = 0.5, - robot_length: float = 0.7, - visualization_size: int = 400, - control_frequency: float = 10.0, - safe_goal_distance: float = 1.5, - ): # Control frequency in Hz - """ - Initialize the base local planner. - - Args: - get_costmap: Function to get the latest local costmap - get_robot_pose: Function to get the latest robot pose (returning odom object) - move: Function to send velocity commands - safety_threshold: Distance to maintain from obstacles (meters) - max_linear_vel: Maximum linear velocity (m/s) - max_angular_vel: Maximum angular velocity (rad/s) - lookahead_distance: Lookahead distance for path following (meters) - goal_tolerance: Distance at which the goal is considered reached (meters) - angle_tolerance: Angle at which the goal orientation is considered reached (radians) - robot_width: Width of the robot for visualization (meters) - robot_length: Length of the robot for visualization (meters) - visualization_size: Size of the visualization image in pixels - control_frequency: Frequency at which the planner is called (Hz) - safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) - """ - # Store callables for robot interactions - self.get_costmap = get_costmap - self.get_robot_pose = get_robot_pose - self.move = move - - # Store parameters - self.safety_threshold = safety_threshold - self.max_linear_vel = max_linear_vel - self.max_angular_vel = max_angular_vel - self.lookahead_distance = lookahead_distance - self.goal_tolerance = goal_tolerance - self.angle_tolerance = angle_tolerance - self.robot_width = robot_width - self.robot_length = robot_length - self.visualization_size = visualization_size - self.control_frequency = control_frequency - self.control_period = 1.0 / control_frequency # Period in seconds - self.safe_goal_distance = safe_goal_distance # Distance to ignore obstacles at goal - self.ignore_obstacles = False # Flag for derived classes to check - - # Goal and Waypoint Tracking - self.goal_xy: Optional[Tuple[float, float]] = None # Current target for planning - self.goal_theta: Optional[float] = None # Goal orientation in odom frame - self.position_reached: bool = False # Flag indicating if position goal is reached - self.waypoints: Optional[Path] = None # Full path if following waypoints - self.waypoints_in_absolute: Optional[Path] = None # Full path in absolute frame - self.waypoint_is_relative: bool = False # Whether waypoints are in relative frame - self.current_waypoint_index: int = 0 # Index of the next waypoint to reach - self.final_goal_reached: bool = False # Flag indicating if the final waypoint is reached - - # Stuck detection - self.stuck_detection_window_seconds = 8.0 # Time window for stuck detection (seconds) - self.position_history_size = int(self.stuck_detection_window_seconds * control_frequency) - self.position_history = deque( - maxlen=self.position_history_size - ) # History of recent positions - self.stuck_distance_threshold = 0.1 # Distance threshold for stuck detection (meters) - self.unstuck_distance_threshold = 0.5 # Distance threshold for unstuck detection (meters) - self.stuck_time_threshold = 4.0 # Time threshold for stuck detection (seconds) - self.is_recovery_active = False # Whether recovery behavior is active - self.recovery_start_time = 0.0 # When recovery behavior started - self.recovery_duration = 8.0 # How long to run recovery before giving up (seconds) - self.last_update_time = time.time() # Last time position was updated - self.navigation_failed = False # Flag indicating if navigation should be terminated - - def reset(self): - """ - Reset all navigation and state tracking variables. - Should be called whenever a new goal is set. - """ - # Reset stuck detection state - self.position_history.clear() - self.is_recovery_active = False - self.recovery_start_time = 0.0 - self.last_update_time = time.time() - - # Reset navigation state flags - self.navigation_failed = False - self.position_reached = False - self.final_goal_reached = False - self.ignore_obstacles = False - - logger.info("Local planner state has been reset") - - def set_goal( - self, goal_xy: VectorLike, is_relative: bool = False, goal_theta: Optional[float] = None - ): - """Set a single goal position, converting to absolute frame if necessary. - This clears any existing waypoints being followed. - - Args: - goal_xy: The goal position to set. - is_relative: Whether the goal is in the robot's relative frame. - goal_theta: Optional goal orientation in radians - """ - # Reset all state variables - self.reset() - - # Clear waypoint following state - self.waypoints = None - self.current_waypoint_index = 0 - self.goal_xy = None # Clear previous goal - self.goal_theta = None # Clear previous goal orientation - - target_goal_xy: Optional[Tuple[float, float]] = None - - # Transform goal to absolute frame if it's relative - if is_relative: - # Get current robot pose - odom = self.get_robot_pose() - robot_pos, robot_rot = odom.pos, odom.rot - - # Extract current position and orientation - robot_x, robot_y = robot_pos.x, robot_pos.y - robot_theta = robot_rot.z # Assuming rotation is euler angles - - # Transform the relative goal into absolute coordinates - goal_x, goal_y = to_tuple(goal_xy) - # Rotate - abs_x = goal_x * math.cos(robot_theta) - goal_y * math.sin(robot_theta) - abs_y = goal_x * math.sin(robot_theta) + goal_y * math.cos(robot_theta) - # Translate - target_goal_xy = (robot_x + abs_x, robot_y + abs_y) - - logger.info( - f"Goal set in relative frame, converted to absolute: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" - ) - else: - target_goal_xy = to_tuple(goal_xy) - logger.info( - f"Goal set directly in absolute frame: ({target_goal_xy[0]:.2f}, {target_goal_xy[1]:.2f})" - ) - - # Check if goal is valid (in bounds and not colliding) - if not self.is_goal_in_costmap_bounds(target_goal_xy) or self.check_goal_collision( - target_goal_xy - ): - logger.warning( - "Goal is in collision or out of bounds. Adjusting goal to valid position." - ) - self.goal_xy = self.adjust_goal_to_valid_position(target_goal_xy) - else: - self.goal_xy = target_goal_xy # Set the adjusted or original valid goal - - # Set goal orientation if provided - if goal_theta is not None: - if is_relative: - # Transform the orientation to absolute frame - odom = self.get_robot_pose() - robot_theta = odom.rot.z - self.goal_theta = normalize_angle(goal_theta + robot_theta) - else: - self.goal_theta = goal_theta - - def set_goal_waypoints(self, waypoints: Path, goal_theta: Optional[float] = None): - """Sets a path of waypoints for the robot to follow. - - Args: - waypoints: A list of waypoints to follow. Each waypoint is a tuple of (x, y) coordinates in absolute frame. - goal_theta: Optional final orientation in radians - """ - # Reset all state variables - self.reset() - - if not isinstance(waypoints, Path) or len(waypoints) == 0: - logger.warning("Invalid or empty path provided to set_goal_waypoints. Ignoring.") - self.waypoints = None - self.waypoint_is_relative = False - self.goal_xy = None - self.goal_theta = None - self.current_waypoint_index = 0 - return - - logger.info(f"Setting goal waypoints with {len(waypoints)} points.") - self.waypoints = waypoints - self.waypoint_is_relative = False - self.current_waypoint_index = 0 - - # Waypoints are always in absolute frame - self.waypoints_in_absolute = waypoints - - # Set the initial target to the first waypoint, adjusting if necessary - first_waypoint = self.waypoints_in_absolute[0] - if not self.is_goal_in_costmap_bounds(first_waypoint) or self.check_goal_collision( - first_waypoint - ): - logger.warning("First waypoint is invalid. Adjusting...") - self.goal_xy = self.adjust_goal_to_valid_position(first_waypoint) - else: - self.goal_xy = to_tuple(first_waypoint) # Initial target - - # Set goal orientation if provided - if goal_theta is not None: - self.goal_theta = goal_theta - - def _get_robot_pose(self) -> Tuple[Tuple[float, float], float]: - """ - Get the current robot position and orientation. - - Returns: - Tuple containing: - - position as (x, y) tuple - - orientation (theta) in radians - """ - odom = self.get_robot_pose() - pos, rot = odom.pos, odom.rot - return (pos.x, pos.y), rot.z - - def _get_final_goal_position(self) -> Optional[Tuple[float, float]]: - """ - Get the final goal position (either last waypoint or direct goal). - - Returns: - Tuple (x, y) of the final goal, or None if no goal is set - """ - if self.waypoints_in_absolute is not None and len(self.waypoints_in_absolute) > 0: - return to_tuple(self.waypoints_in_absolute[-1]) - elif self.goal_xy is not None: - return self.goal_xy - return None - - def _distance_to_position(self, target_position: Tuple[float, float]) -> float: - """ - Calculate distance from the robot to a target position. - - Args: - target_position: Target (x, y) position - - Returns: - Distance in meters - """ - robot_pos, _ = self._get_robot_pose() - return np.linalg.norm( - [target_position[0] - robot_pos[0], target_position[1] - robot_pos[1]] - ) - - def plan(self) -> Dict[str, float]: - """ - Main planning method that computes velocity commands. - This includes common planning logic like waypoint following, - with algorithm-specific calculations delegated to subclasses. - - Returns: - Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys - """ - # If goal orientation is specified, rotate to match it - if ( - self.position_reached - and self.goal_theta is not None - and not self._is_goal_orientation_reached() - ): - logger.info("Position goal reached. Rotating to target orientation.") - return self._rotate_to_goal_orientation() - - # Check if the robot is stuck and handle accordingly - if self.check_if_stuck() and not self.position_reached: - # Check if we're stuck but close to our goal - final_goal_pos = self._get_final_goal_position() - - # If we have a goal position, check distance to it - if final_goal_pos is not None: - distance_to_goal = self._distance_to_position(final_goal_pos) - - # If we're stuck but within 2x safe_goal_distance of the goal, consider it a success - if distance_to_goal < 2.0 * self.safe_goal_distance: - logger.info( - f"Robot is stuck but within {distance_to_goal:.2f}m of goal (< {2.0 * self.safe_goal_distance:.2f}m). Considering navigation successful." - ) - self.position_reached = True - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Otherwise, execute normal recovery behavior - logger.warning("Robot is stuck - executing recovery behavior") - return self.execute_recovery_behavior() - - # Reset obstacle ignore flag - self.ignore_obstacles = False - - # --- Waypoint Following Mode --- - if self.waypoints is not None: - if self.final_goal_reached: - logger.info("Final waypoint reached. Stopping.") - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Get current robot pose - robot_pos, robot_theta = self._get_robot_pose() - robot_pos_np = np.array(robot_pos) - - # Check if close to final waypoint - if self.waypoints_in_absolute is not None and len(self.waypoints_in_absolute) > 0: - final_waypoint = self.waypoints_in_absolute[-1] - dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) - - # If we're close to the final waypoint, adjust it and ignore obstacles - if dist_to_final < self.safe_goal_distance: - final_wp_tuple = to_tuple(final_waypoint) - adjusted_goal = self.adjust_goal_to_valid_position(final_wp_tuple) - # Create a new Path with the adjusted final waypoint - new_waypoints = self.waypoints_in_absolute[:-1] # Get all but the last waypoint - new_waypoints.append(adjusted_goal) # Append the adjusted goal - self.waypoints_in_absolute = new_waypoints - self.ignore_obstacles = True - logger.debug(f"Within safe distance of final waypoint. Ignoring obstacles.") - - # Update the target goal based on waypoint progression - just_reached_final = self._update_waypoint_target(robot_pos_np) - - # If the helper indicates the final goal was just reached, stop immediately - if just_reached_final: - return {"x_vel": 0.0, "angular_vel": 0.0} - - # --- Single Goal or Current Waypoint Target Set --- - if self.goal_xy is None: - # If no goal is set (e.g., empty path or rejected goal), stop. - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Get necessary data for planning - costmap = self.get_costmap() - if costmap is None: - logger.warning("Local costmap is None. Cannot plan.") - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Check if close to single goal mode goal - if self.waypoints is None: - # Get distance to goal - goal_distance = self._distance_to_position(self.goal_xy) - - # If within safe distance of goal, adjust it and ignore obstacles - if goal_distance < self.safe_goal_distance: - self.goal_xy = self.adjust_goal_to_valid_position(self.goal_xy) - self.ignore_obstacles = True - logger.debug(f"Within safe distance of goal. Ignoring obstacles.") - - # First check position - if goal_distance < self.goal_tolerance or self.position_reached: - self.position_reached = True - - else: - self.position_reached = False - - # Call the algorithm-specific planning implementation - return self._compute_velocity_commands() - - @abstractmethod - def _compute_velocity_commands(self) -> Dict[str, float]: - """ - Algorithm-specific method to compute velocity commands. - Must be implemented by derived classes. - - Returns: - Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys - """ - pass - - def _rotate_to_goal_orientation(self) -> Dict[str, float]: - """Compute velocity commands to rotate to the goal orientation. - - Returns: - Dict[str, float]: Velocity commands with zero linear velocity - """ - # Get current robot orientation - _, robot_theta = self._get_robot_pose() - - # Calculate the angle difference - angle_diff = normalize_angle(self.goal_theta - robot_theta) - - # Determine rotation direction and speed - if abs(angle_diff) < self.angle_tolerance: - # Already at correct orientation - return {"x_vel": 0.0, "angular_vel": 0.0} - - # Calculate rotation speed - proportional to the angle difference - # but capped at max_angular_vel - direction = 1.0 if angle_diff > 0 else -1.0 - angular_vel = direction * min(abs(angle_diff) * 2.0, self.max_angular_vel) - - # logger.debug(f"Rotating to goal orientation: angle_diff={angle_diff:.4f}, angular_vel={angular_vel:.4f}") - return {"x_vel": 0.0, "angular_vel": angular_vel} - - def _is_goal_orientation_reached(self) -> bool: - """Check if the current robot orientation matches the goal orientation. - - Returns: - bool: True if orientation is reached or no orientation goal is set - """ - if self.goal_theta is None: - return True # No orientation goal set - - # Get current robot orientation - _, robot_theta = self._get_robot_pose() - - # Calculate the angle difference and normalize - angle_diff = abs(normalize_angle(self.goal_theta - robot_theta)) - - logger.debug( - f"Orientation error: {angle_diff:.4f} rad, tolerance: {self.angle_tolerance:.4f} rad" - ) - return angle_diff <= self.angle_tolerance - - def _update_waypoint_target(self, robot_pos_np: np.ndarray) -> bool: - """Helper function to manage waypoint progression and update the target goal. - - Args: - robot_pos_np: Current robot position as a numpy array [x, y]. - - Returns: - bool: True if the final waypoint has just been reached, False otherwise. - """ - if self.waypoints is None or len(self.waypoints) == 0: - return False # Not in waypoint mode or empty path - - # Waypoints are always in absolute frame - self.waypoints_in_absolute = self.waypoints - - # Check if final goal is reached - final_waypoint = self.waypoints_in_absolute[-1] - dist_to_final = np.linalg.norm(robot_pos_np - final_waypoint) - - if dist_to_final < self.goal_tolerance: - self.position_reached = True - self.goal_xy = to_tuple(final_waypoint) - - # If goal orientation is not specified or achieved, consider fully reached - if self.goal_theta is None or self._is_goal_orientation_reached(): - self.final_goal_reached = True - logger.info("Reached final waypoint with correct orientation.") - return True - else: - logger.info("Reached final waypoint position, rotating to target orientation.") - return False - - # Always find the lookahead point - lookahead_point = None - for i in range(self.current_waypoint_index, len(self.waypoints_in_absolute)): - wp = self.waypoints_in_absolute[i] - dist_to_wp = np.linalg.norm(robot_pos_np - wp) - if dist_to_wp >= self.lookahead_distance: - lookahead_point = wp - # Update current waypoint index to this point - self.current_waypoint_index = i - break - - # If no point is far enough, target the final waypoint - if lookahead_point is None: - lookahead_point = self.waypoints_in_absolute[-1] - self.current_waypoint_index = len(self.waypoints_in_absolute) - 1 - - # Set the lookahead point as the immediate target, adjusting if needed - if not self.is_goal_in_costmap_bounds(lookahead_point) or self.check_goal_collision( - lookahead_point - ): - logger.debug("Lookahead point is invalid. Adjusting...") - adjusted_lookahead = self.adjust_goal_to_valid_position(lookahead_point) - # Only update if adjustment didn't fail completely - if adjusted_lookahead is not None: - self.goal_xy = adjusted_lookahead - else: - self.goal_xy = to_tuple(lookahead_point) - - return False # Final goal not reached in this update cycle - - @abstractmethod - def update_visualization(self) -> np.ndarray: - """ - Generate visualization of the planning state. - Must be implemented by derived classes. - - Returns: - np.ndarray: Visualization image as numpy array - """ - pass - - def create_stream(self, frequency_hz: float = None) -> Observable: - """ - Create an Observable stream that emits the visualization image at a fixed frequency. - - Args: - frequency_hz: Optional frequency override (defaults to 1/4 of control_frequency if None) - - Returns: - Observable: Stream of visualization frames - """ - # Default to 1/4 of control frequency if not specified (to reduce CPU usage) - if frequency_hz is None: - frequency_hz = self.control_frequency / 4.0 - - subject = Subject() - sleep_time = 1.0 / frequency_hz - - def frame_emitter(): - while True: - try: - # Generate the frame using the updated method - frame = self.update_visualization() - subject.on_next(frame) - except Exception as e: - logger.error(f"Error in frame emitter thread: {e}") - # Optionally, emit an error frame or simply skip - # subject.on_error(e) # This would terminate the stream - time.sleep(sleep_time) - - emitter_thread = threading.Thread(target=frame_emitter, daemon=True) - emitter_thread.start() - logger.info(f"Started visualization frame emitter thread at {frequency_hz:.1f} Hz") - return subject - - @abstractmethod - def check_collision(self, direction: float) -> bool: - """ - Check if there's a collision in the given direction. - Must be implemented by derived classes. - - Args: - direction: Direction to check for collision in radians - - Returns: - bool: True if collision detected, False otherwise - """ - pass - - def is_goal_reached(self) -> bool: - """Check if the final goal (single or last waypoint) is reached, including orientation.""" - if self.waypoints is not None: - # Waypoint mode: check if the final waypoint and orientation have been reached - return self.final_goal_reached - else: - # Single goal mode: check distance to the single goal and orientation - if self.goal_xy is None: - return False # No goal set - - return self.position_reached and self._is_goal_orientation_reached() - - def check_goal_collision(self, goal_xy: VectorLike) -> bool: - """Check if the current goal is in collision with obstacles in the costmap. - - Returns: - bool: True if goal is in collision, False if goal is safe or cannot be checked - """ - - costmap = self.get_costmap() - if costmap is None: - logger.warning("Cannot check collision: No costmap available") - return False - - # Check if the position is occupied - collision_threshold = 80 # Consider values above 80 as obstacles - - # Use Costmap's is_occupied method - return costmap.is_occupied(goal_xy, threshold=collision_threshold) - - def is_goal_in_costmap_bounds(self, goal_xy: VectorLike) -> bool: - """Check if the goal position is within the bounds of the costmap. - - Args: - goal_xy: Goal position (x, y) in odom frame - - Returns: - bool: True if the goal is within the costmap bounds, False otherwise - """ - costmap = self.get_costmap() - if costmap is None: - logger.warning("Cannot check bounds: No costmap available") - return False - - # Get goal position in grid coordinates - goal_point = costmap.world_to_grid(goal_xy) - goal_cell_x, goal_cell_y = goal_point.x, goal_point.y - - # Check if goal is within the costmap bounds - is_in_bounds = 0 <= goal_cell_x < costmap.width and 0 <= goal_cell_y < costmap.height - - if not is_in_bounds: - logger.warning(f"Goal ({goal_xy[0]:.2f}, {goal_xy[1]:.2f}) is outside costmap bounds") - - return is_in_bounds - - def adjust_goal_to_valid_position( - self, goal_xy: VectorLike, clearance: float = 0.5 - ) -> Tuple[float, float]: - """Find a valid (non-colliding) goal position by moving it towards the robot. - - Args: - goal_xy: Original goal position (x, y) in odom frame - clearance: Additional distance to move back from obstacles for better clearance (meters) - - Returns: - Tuple[float, float]: A valid goal position, or the original goal if already valid - """ - [pos, rot] = self._get_robot_pose() - - robot_x, robot_y = pos[0], pos[1] - - # Original goal - goal_x, goal_y = to_tuple(goal_xy) - - if not self.check_goal_collision((goal_x, goal_y)): - return (goal_x, goal_y) - - # Calculate vector from goal to robot - dx = robot_x - goal_x - dy = robot_y - goal_y - distance = np.sqrt(dx * dx + dy * dy) - - if distance < 0.001: # Goal is at robot position - return to_tuple(goal_xy) - - # Normalize direction vector - dx /= distance - dy /= distance - - # Step size - step_size = 0.25 # meters - - # Move goal towards robot step by step - current_x, current_y = goal_x, goal_y - steps = 0 - max_steps = 50 # Safety limit - - # Variables to store the first valid position found - valid_found = False - valid_x, valid_y = None, None - - while steps < max_steps: - # Move towards robot - current_x += dx * step_size - current_y += dy * step_size - steps += 1 - - # Check if we've reached or passed the robot - new_distance = np.sqrt((current_x - robot_x) ** 2 + (current_y - robot_y) ** 2) - if new_distance < step_size: - # We've reached the robot without finding a valid point - # Move back one step from robot to avoid self-collision - current_x = robot_x - dx * step_size - current_y = robot_y - dy * step_size - break - - # Check if this position is valid - if not self.check_goal_collision( - (current_x, current_y) - ) and self.is_goal_in_costmap_bounds((current_x, current_y)): - # Store the first valid position - if not valid_found: - valid_found = True - valid_x, valid_y = current_x, current_y - - # If clearance is requested, continue searching for a better position - if clearance > 0: - continue - - # Calculate position with additional clearance - if clearance > 0: - # Calculate clearance position - clearance_x = current_x + dx * clearance - clearance_y = current_y + dy * clearance - - logger.info( - f"Checking clearance position at ({clearance_x:.2f}, {clearance_y:.2f})" - ) - - # Check if the clearance position is also valid - if not self.check_goal_collision( - (clearance_x, clearance_y) - ) and self.is_goal_in_costmap_bounds((clearance_x, clearance_y)): - logger.info( - f"Found valid goal with clearance at ({clearance_x:.2f}, {clearance_y:.2f})" - ) - return (clearance_x, clearance_y) - - # Return the valid position without clearance - logger.info(f"Found valid goal at ({current_x:.2f}, {current_y:.2f})") - return (current_x, current_y) - - # If we found a valid position earlier but couldn't add clearance - if valid_found: - logger.info(f"Using valid goal found at ({valid_x:.2f}, {valid_y:.2f})") - return (valid_x, valid_y) - - logger.warning( - f"Could not find valid goal after {steps} steps, using closest point to robot" - ) - return (current_x, current_y) - - def check_if_stuck(self) -> bool: - """ - Check if the robot is stuck by analyzing movement history. - - Returns: - bool: True if the robot is determined to be stuck, False otherwise - """ - # Get current position and time - current_time = time.time() - - # Get current robot position - [pos, _] = self._get_robot_pose() - current_position = (pos[0], pos[1], current_time) - - # Add current position to history (newest is appended at the end) - self.position_history.append(current_position) - - # Need enough history to make a determination - min_history_size = self.stuck_detection_window_seconds * self.control_frequency - if len(self.position_history) < min_history_size: - return False - - # Find positions within our detection window (positions are already in order from oldest to newest) - window_start_time = current_time - self.stuck_detection_window_seconds - window_positions = [] - - # Collect positions within the window (newest entries will be at the end) - for pos_x, pos_y, timestamp in self.position_history: - if timestamp >= window_start_time: - window_positions.append((pos_x, pos_y, timestamp)) - - # Need at least a few positions in the window - if len(window_positions) < 3: - return False - - # Ensure correct order: oldest to newest - window_positions.sort(key=lambda p: p[2]) - - # Get the oldest and newest positions in the window - oldest_x, oldest_y, oldest_time = window_positions[0] - newest_x, newest_y, newest_time = window_positions[-1] - - # Calculate time range in the window (should always be positive) - time_range = newest_time - oldest_time - - # Calculate displacement from oldest to newest position - displacement = np.sqrt((newest_x - oldest_x) ** 2 + (newest_y - oldest_y) ** 2) - - # Check if we're stuck - moved less than threshold over minimum time - # Only consider it if the time range makes sense (positive and sufficient) - is_currently_stuck = ( - time_range >= self.stuck_time_threshold - and time_range <= self.stuck_detection_window_seconds - and displacement < self.stuck_distance_threshold - ) - - if is_currently_stuck: - logger.warning( - f"Robot appears to be stuck! Displacement {displacement:.3f}m over {time_range:.1f}s" - ) - - # Don't trigger recovery if it's already active - if not self.is_recovery_active: - self.is_recovery_active = True - self.recovery_start_time = current_time - return True - - # Check if we've been trying to recover for too long - elif current_time - self.recovery_start_time > self.recovery_duration: - logger.error( - f"Recovery behavior has been active for {self.recovery_duration}s without success" - ) - # Reset recovery state - maybe a different behavior will work - self.is_recovery_active = False - self.recovery_start_time = current_time - - # If we've moved enough, we're not stuck anymore - elif self.is_recovery_active and displacement > self.unstuck_distance_threshold: - logger.info(f"Robot has escaped from stuck state (moved {displacement:.3f}m)") - self.is_recovery_active = False - - return self.is_recovery_active - - def execute_recovery_behavior(self) -> Dict[str, float]: - """ - Execute a recovery behavior when the robot is stuck. - - Returns: - Dict[str, float]: Velocity commands for the recovery behavior - """ - # Calculate how long we've been in recovery - recovery_time = time.time() - self.recovery_start_time - - # Calculate recovery phases based on control frequency - backup_phase_time = 3.0 # seconds - rotate_phase_time = 2.0 # seconds - - # Simple recovery behavior state machine - if recovery_time < backup_phase_time: - # First try backing up - logger.info("Recovery: backing up") - return {"x_vel": -0.2, "angular_vel": 0.0} - elif recovery_time < backup_phase_time + rotate_phase_time: - # Then try rotating - logger.info("Recovery: rotating to find new path") - rotation_direction = 1.0 if np.random.random() > 0.5 else -1.0 - return {"x_vel": 0.0, "angular_vel": rotation_direction * self.max_angular_vel * 0.7} - else: - # If we're still stuck after backup and rotation, terminate navigation - logger.error("Recovery failed after backup and rotation. Navigation terminated.") - # Set a flag to indicate navigation should terminate - self.navigation_failed = True - # Stop the robot - return {"x_vel": 0.0, "angular_vel": 0.0} - - -def navigate_to_goal_local( - robot, - goal_xy_robot: Tuple[float, float], - goal_theta: Optional[float] = None, - distance: float = 0.0, - timeout: float = 60.0, - stop_event: Optional[threading.Event] = None, -) -> bool: - """ - Navigates the robot to a goal specified in the robot's local frame - using the local planner. - - Args: - robot: Robot instance to control - goal_xy_robot: Tuple (x, y) representing the goal position relative - to the robot's current position and orientation. - distance: Desired distance to maintain from the goal in meters. - If non-zero, the robot will stop this far away from the goal. - timeout: Maximum time (in seconds) allowed to reach the goal. - stop_event: Optional threading.Event to signal when navigation should stop - - Returns: - bool: True if the goal was reached within the timeout, False otherwise. - """ - logger.info( - f"Starting navigation to local goal {goal_xy_robot} with distance {distance}m and timeout {timeout}s." - ) - - goal_x, goal_y = goal_xy_robot - - # Calculate goal orientation to face the target - if goal_theta is None: - goal_theta = np.arctan2(goal_y, goal_x) - - # If distance is non-zero, adjust the goal to stop at the desired distance - if distance > 0: - # Calculate magnitude of the goal vector - goal_distance = np.sqrt(goal_x**2 + goal_y**2) - - # Only adjust if goal is further than the desired distance - if goal_distance > distance: - goal_x, goal_y = distance_angle_to_goal_xy(goal_distance - distance, goal_theta) - - # Set the goal in the robot's frame with orientation to face the original target - robot.local_planner.set_goal((goal_x, goal_y), is_relative=True, goal_theta=goal_theta) - - # Get control period from robot's local planner for consistent timing - control_period = 1.0 / robot.local_planner.control_frequency - - start_time = time.time() - goal_reached = False - - try: - while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): - # Check if goal has been reached - if robot.local_planner.is_goal_reached(): - logger.info("Goal reached successfully.") - goal_reached = True - break - - # Check if navigation failed flag is set - if robot.local_planner.navigation_failed: - logger.error("Navigation aborted due to repeated recovery failures.") - goal_reached = False - break - - # Get planned velocity towards the goal - vel_command = robot.local_planner.plan() - x_vel = vel_command.get("x_vel", 0.0) - angular_vel = vel_command.get("angular_vel", 0.0) - - # Send velocity command - robot.local_planner.move(Vector(x_vel, 0, angular_vel)) - - # Control loop frequency - use robot's control frequency - time.sleep(control_period) - - if not goal_reached: - logger.warning(f"Navigation timed out after {timeout} seconds before reaching goal.") - - except KeyboardInterrupt: - logger.info("Navigation to local goal interrupted by user.") - goal_reached = False # Consider interruption as failure - except Exception as e: - logger.error(f"Error during navigation to local goal: {e}") - goal_reached = False # Consider error as failure - finally: - logger.info("Stopping robot after navigation attempt.") - robot.local_planner.move(Vector(0, 0, 0)) # Stop the robot - - return goal_reached - - -def navigate_path_local( - robot, - path: Path, - timeout: float = 120.0, - goal_theta: Optional[float] = None, - stop_event: Optional[threading.Event] = None, -) -> bool: - """ - Navigates the robot along a path of waypoints using the waypoint following capability - of the local planner. - - Args: - robot: Robot instance to control - path: Path object containing waypoints in absolute frame - timeout: Maximum time (in seconds) allowed to follow the complete path - goal_theta: Optional final orientation in radians - stop_event: Optional threading.Event to signal when navigation should stop - - Returns: - bool: True if the entire path was successfully followed, False otherwise - """ - logger.info( - f"Starting navigation along path with {len(path)} waypoints and timeout {timeout}s." - ) - - # Set the path in the local planner - robot.local_planner.set_goal_waypoints(path, goal_theta=goal_theta) - - # Get control period from robot's local planner for consistent timing - control_period = 1.0 / robot.local_planner.control_frequency - - start_time = time.time() - path_completed = False - - try: - while time.time() - start_time < timeout and not (stop_event and stop_event.is_set()): - # Check if the entire path has been traversed - if robot.local_planner.is_goal_reached(): - logger.info("Path traversed successfully.") - path_completed = True - break - - # Check if navigation failed flag is set - if robot.local_planner.navigation_failed: - logger.error("Navigation aborted due to repeated recovery failures.") - path_completed = False - break - - # Get planned velocity towards the current waypoint target - vel_command = robot.local_planner.plan() - x_vel = vel_command.get("x_vel", 0.0) - angular_vel = vel_command.get("angular_vel", 0.0) - - # Send velocity command - robot.local_planner.move(Vector(x_vel, 0, angular_vel)) - - # Control loop frequency - use robot's control frequency - time.sleep(control_period) - - if not path_completed: - logger.warning( - f"Path following timed out after {timeout} seconds before completing the path." - ) - - except KeyboardInterrupt: - logger.info("Path navigation interrupted by user.") - path_completed = False - except Exception as e: - logger.error(f"Error during path navigation: {e}") - path_completed = False - finally: - logger.info("Stopping robot after path navigation attempt.") - robot.local_planner.move(Vector(0, 0, 0)) # Stop the robot - - return path_completed - - -def visualize_local_planner_state( - occupancy_grid: np.ndarray, - grid_resolution: float, - grid_origin: Tuple[float, float], - robot_pose: Tuple[float, float, float], - visualization_size: int = 400, - robot_width: float = 0.5, - robot_length: float = 0.7, - map_size_meters: float = 10.0, - goal_xy: Optional[Tuple[float, float]] = None, - goal_theta: Optional[float] = None, - histogram: Optional[np.ndarray] = None, - selected_direction: Optional[float] = None, - waypoints: Optional["Path"] = None, - current_waypoint_index: Optional[int] = None, -) -> np.ndarray: - """Generate a bird's eye view visualization of the local costmap. - Optionally includes VFH histogram, selected direction, and waypoints path. - - Args: - occupancy_grid: 2D numpy array of the occupancy grid - grid_resolution: Resolution of the grid in meters/cell - grid_origin: Tuple (x, y) of the grid origin in the odom frame - robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame - visualization_size: Size of the visualization image in pixels - robot_width: Width of the robot in meters - robot_length: Length of the robot in meters - map_size_meters: Size of the map to visualize in meters - goal_xy: Optional tuple (x, y) of the goal position in the odom frame - goal_theta: Optional goal orientation in radians (in odom frame) - histogram: Optional numpy array of the VFH histogram - selected_direction: Optional selected direction angle in radians - waypoints: Optional Path object containing waypoints to visualize - current_waypoint_index: Optional index of the current target waypoint - """ - - robot_x, robot_y, robot_theta = robot_pose - grid_origin_x, grid_origin_y = grid_origin - vis_size = visualization_size - scale = vis_size / map_size_meters - - vis_img = np.ones((vis_size, vis_size, 3), dtype=np.uint8) * 255 - center_x = vis_size // 2 - center_y = vis_size // 2 - - grid_height, grid_width = occupancy_grid.shape - - # Calculate robot position relative to grid origin - robot_rel_x = robot_x - grid_origin_x - robot_rel_y = robot_y - grid_origin_y - robot_cell_x = int(robot_rel_x / grid_resolution) - robot_cell_y = int(robot_rel_y / grid_resolution) - - half_size_cells = int(map_size_meters / grid_resolution / 2) - - # Draw grid cells (using standard occupancy coloring) - for y in range( - max(0, robot_cell_y - half_size_cells), min(grid_height, robot_cell_y + half_size_cells) - ): - for x in range( - max(0, robot_cell_x - half_size_cells), min(grid_width, robot_cell_x + half_size_cells) - ): - cell_rel_x_meters = (x - robot_cell_x) * grid_resolution - cell_rel_y_meters = (y - robot_cell_y) * grid_resolution - - img_x = int(center_x + cell_rel_x_meters * scale) - img_y = int(center_y - cell_rel_y_meters * scale) # Flip y-axis - - if 0 <= img_x < vis_size and 0 <= img_y < vis_size: - cell_value = occupancy_grid[y, x] - if cell_value == -1: - color = (200, 200, 200) # Unknown (Light gray) - elif cell_value == 0: - color = (255, 255, 255) # Free (White) - else: # Occupied - # Scale darkness based on occupancy value (0-100) - darkness = 255 - int(155 * (cell_value / 100)) - 100 - color = (darkness, darkness, darkness) # Shades of gray/black - - cell_size_px = max(1, int(grid_resolution * scale)) - cv2.rectangle( - vis_img, - (img_x - cell_size_px // 2, img_y - cell_size_px // 2), - (img_x + cell_size_px // 2, img_y + cell_size_px // 2), - color, - -1, - ) - - # Draw waypoints path if provided - if waypoints is not None and len(waypoints) > 0: - try: - path_points = [] - for i, waypoint in enumerate(waypoints): - # Convert waypoint from odom frame to visualization frame - wp_x, wp_y = waypoint[0], waypoint[1] - wp_rel_x = wp_x - robot_x - wp_rel_y = wp_y - robot_y - - wp_img_x = int(center_x + wp_rel_x * scale) - wp_img_y = int(center_y - wp_rel_y * scale) # Flip y-axis - - if 0 <= wp_img_x < vis_size and 0 <= wp_img_y < vis_size: - path_points.append((wp_img_x, wp_img_y)) - - # Draw each waypoint as a small circle - cv2.circle(vis_img, (wp_img_x, wp_img_y), 3, (0, 128, 0), -1) # Dark green dots - - # Highlight current target waypoint - if current_waypoint_index is not None and i == current_waypoint_index: - cv2.circle(vis_img, (wp_img_x, wp_img_y), 6, (0, 0, 255), 2) # Red circle - - # Connect waypoints with lines to show the path - if len(path_points) > 1: - for i in range(len(path_points) - 1): - cv2.line( - vis_img, path_points[i], path_points[i + 1], (0, 200, 0), 1 - ) # Green line - except Exception as e: - logger.error(f"Error drawing waypoints: {e}") - - # Draw histogram - if histogram is not None: - num_bins = len(histogram) - # Find absolute maximum value (ignoring any negative debug values) - abs_histogram = np.abs(histogram) - max_hist_value = np.max(abs_histogram) if np.max(abs_histogram) > 0 else 1.0 - hist_scale = (vis_size / 2) * 0.8 # Scale histogram lines to 80% of half the viz size - - for i in range(num_bins): - # Angle relative to robot's forward direction - angle_relative_to_robot = (i / num_bins) * 2 * math.pi - math.pi - # Angle in the visualization frame (relative to image +X axis) - vis_angle = angle_relative_to_robot + robot_theta - - # Get the value and check if it's a special debug value (negative) - hist_val = histogram[i] - is_debug_value = hist_val < 0 - - # Use absolute value for line length - normalized_val = min(1.0, abs(hist_val) / max_hist_value) - line_length = normalized_val * hist_scale - - # Calculate endpoint using the visualization angle - end_x = int(center_x + line_length * math.cos(vis_angle)) - end_y = int(center_y - line_length * math.sin(vis_angle)) # Flipped Y - - # Color based on value and whether it's a debug value - if is_debug_value: - # Use green for debug values (minimum cost bin) - color = (0, 255, 0) # Green - line_width = 2 # Thicker line for emphasis - else: - # Regular coloring for normal values (blue to red gradient based on obstacle density) - blue = max(0, 255 - int(normalized_val * 255)) - red = min(255, int(normalized_val * 255)) - color = (blue, 0, red) # BGR format: obstacles are redder, clear areas are bluer - line_width = 1 - - cv2.line(vis_img, (center_x, center_y), (end_x, end_y), color, line_width) - - # Draw robot - robot_length_px = int(robot_length * scale) - robot_width_px = int(robot_width * scale) - robot_pts = np.array( - [ - [-robot_length_px / 2, -robot_width_px / 2], - [robot_length_px / 2, -robot_width_px / 2], - [robot_length_px / 2, robot_width_px / 2], - [-robot_length_px / 2, robot_width_px / 2], - ], - dtype=np.float32, - ) - rotation_matrix = np.array( - [ - [math.cos(robot_theta), -math.sin(robot_theta)], - [math.sin(robot_theta), math.cos(robot_theta)], - ] - ) - robot_pts = np.dot(robot_pts, rotation_matrix.T) - robot_pts[:, 0] += center_x - robot_pts[:, 1] = center_y - robot_pts[:, 1] # Flip y-axis - cv2.fillPoly( - vis_img, [robot_pts.reshape((-1, 1, 2)).astype(np.int32)], (0, 0, 255) - ) # Red robot - - # Draw robot direction line - front_x = int(center_x + (robot_length_px / 2) * math.cos(robot_theta)) - front_y = int(center_y - (robot_length_px / 2) * math.sin(robot_theta)) - cv2.line(vis_img, (center_x, center_y), (front_x, front_y), (255, 0, 0), 2) # Blue line - - # Draw selected direction - if selected_direction is not None: - # selected_direction is relative to robot frame - # Angle in the visualization frame (relative to image +X axis) - vis_angle_selected = selected_direction + robot_theta - - # Make slightly longer than max histogram line - sel_dir_line_length = (vis_size / 2) * 0.9 - - sel_end_x = int(center_x + sel_dir_line_length * math.cos(vis_angle_selected)) - sel_end_y = int(center_y - sel_dir_line_length * math.sin(vis_angle_selected)) # Flipped Y - - cv2.line( - vis_img, (center_x, center_y), (sel_end_x, sel_end_y), (0, 165, 255), 2 - ) # BGR for Orange - - # Draw goal - if goal_xy is not None: - goal_x, goal_y = goal_xy - goal_rel_x_map = goal_x - robot_x - goal_rel_y_map = goal_y - robot_y - goal_img_x = int(center_x + goal_rel_x_map * scale) - goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis - if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: - cv2.circle(vis_img, (goal_img_x, goal_img_y), 5, (0, 255, 0), -1) # Green circle - cv2.circle(vis_img, (goal_img_x, goal_img_y), 8, (0, 0, 0), 1) # Black outline - - # Draw goal orientation - if goal_theta is not None and goal_xy is not None: - # For waypoint mode, only draw orientation at the final waypoint - if waypoints is not None and len(waypoints) > 0: - # Use the final waypoint position - final_waypoint = waypoints[-1] - goal_x, goal_y = final_waypoint[0], final_waypoint[1] - else: - # Use the current goal position - goal_x, goal_y = goal_xy - - goal_rel_x_map = goal_x - robot_x - goal_rel_y_map = goal_y - robot_y - goal_img_x = int(center_x + goal_rel_x_map * scale) - goal_img_y = int(center_y - goal_rel_y_map * scale) # Flip y-axis - - # Calculate goal orientation vector direction in visualization frame - # goal_theta is already in odom frame, need to adjust for visualization orientation - goal_dir_length = 30 # Length of direction indicator in pixels - goal_dir_end_x = int(goal_img_x + goal_dir_length * math.cos(goal_theta)) - goal_dir_end_y = int(goal_img_y - goal_dir_length * math.sin(goal_theta)) # Flip y-axis - - # Draw goal orientation arrow - if 0 <= goal_img_x < vis_size and 0 <= goal_img_y < vis_size: - cv2.arrowedLine( - vis_img, - (goal_img_x, goal_img_y), - (goal_dir_end_x, goal_dir_end_y), - (255, 0, 255), - 4, - ) # Magenta arrow - - # Add scale bar - scale_bar_length_px = int(1.0 * scale) - scale_bar_x = vis_size - scale_bar_length_px - 10 - scale_bar_y = vis_size - 20 - cv2.line( - vis_img, - (scale_bar_x, scale_bar_y), - (scale_bar_x + scale_bar_length_px, scale_bar_y), - (0, 0, 0), - 2, - ) - cv2.putText( - vis_img, "1m", (scale_bar_x, scale_bar_y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 0), 1 - ) - - # Add status info - status_text = [] - if waypoints is not None: - if current_waypoint_index is not None: - status_text.append(f"WP: {current_waypoint_index}/{len(waypoints)}") - else: - status_text.append(f"WPs: {len(waypoints)}") - - y_pos = 20 - for text in status_text: - cv2.putText(vis_img, text, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1) - y_pos += 20 - - return vis_img diff --git a/dimos/robot/local_planner/vfh/vfh_local_planner.py b/dimos/robot/local_planner/vfh/vfh_local_planner.py deleted file mode 100644 index 09d77a9743..0000000000 --- a/dimos/robot/local_planner/vfh/vfh_local_planner.py +++ /dev/null @@ -1,422 +0,0 @@ -#!/usr/bin/env python3 - -import math -import numpy as np -from typing import Dict, Tuple, Optional, Callable, Any -import cv2 -import logging -import time - -from dimos.utils.logging_config import setup_logger -from dimos.utils.ros_utils import normalize_angle - -from dimos.robot.local_planner.vfh.local_planner import ( - BaseLocalPlanner, - visualize_local_planner_state, -) -from dimos.types.costmap import Costmap -from dimos.types.vector import Vector -from nav_msgs.msg import OccupancyGrid - -logger = setup_logger("dimos.robot.unitree.vfh_local_planner", level=logging.DEBUG) - - -class VFHPurePursuitPlanner(BaseLocalPlanner): - """ - A local planner that combines Vector Field Histogram (VFH) for obstacle avoidance - with Pure Pursuit for goal tracking. - """ - - def __init__( - self, - get_costmap: Callable[[], Optional[OccupancyGrid]], - get_robot_pose: Callable[[], Any], - move: Callable[[Vector], None], - safety_threshold: float = 0.8, - histogram_bins: int = 144, - max_linear_vel: float = 0.8, - max_angular_vel: float = 1.0, - lookahead_distance: float = 1.0, - goal_tolerance: float = 0.2, - angle_tolerance: float = 0.1, # ~5.7 degrees - robot_width: float = 0.5, - robot_length: float = 0.7, - visualization_size: int = 400, - control_frequency: float = 10.0, - safe_goal_distance: float = 1.0, - ): - """ - Initialize the VFH + Pure Pursuit planner. - - Args: - get_costmap: Function to get the latest local costmap - get_robot_pose: Function to get the latest robot pose (returning odom object) - move: Function to send velocity commands - safety_threshold: Distance to maintain from obstacles (meters) - histogram_bins: Number of directional bins in the polar histogram - max_linear_vel: Maximum linear velocity (m/s) - max_angular_vel: Maximum angular velocity (rad/s) - lookahead_distance: Lookahead distance for pure pursuit (meters) - goal_tolerance: Distance at which the goal is considered reached (meters) - angle_tolerance: Angle at which the goal orientation is considered reached (radians) - robot_width: Width of the robot for visualization (meters) - robot_length: Length of the robot for visualization (meters) - visualization_size: Size of the visualization image in pixels - control_frequency: Frequency at which the planner is called (Hz) - safe_goal_distance: Distance at which to adjust the goal and ignore obstacles (meters) - """ - # Initialize base class - super().__init__( - get_costmap=get_costmap, - get_robot_pose=get_robot_pose, - move=move, - safety_threshold=safety_threshold, - max_linear_vel=max_linear_vel, - max_angular_vel=max_angular_vel, - lookahead_distance=lookahead_distance, - goal_tolerance=goal_tolerance, - angle_tolerance=angle_tolerance, - robot_width=robot_width, - robot_length=robot_length, - visualization_size=visualization_size, - control_frequency=control_frequency, - safe_goal_distance=safe_goal_distance, - ) - - # VFH specific parameters - self.histogram_bins = histogram_bins - self.histogram = None - self.selected_direction = None - - # VFH tuning parameters - self.alpha = 0.2 # Histogram smoothing factor - self.obstacle_weight = 10.0 - self.goal_weight = 1.0 - self.prev_direction_weight = 0.5 - self.prev_selected_angle = 0.0 - self.prev_linear_vel = 0.0 - self.linear_vel_filter_factor = 0.4 - self.low_speed_nudge = 0.1 - - # Add after other initialization - self.angle_mapping = np.linspace(-np.pi, np.pi, self.histogram_bins, endpoint=False) - self.smoothing_kernel = np.array([self.alpha, (1 - 2 * self.alpha), self.alpha]) - - def _compute_velocity_commands(self) -> Dict[str, float]: - """ - VFH + Pure Pursuit specific implementation of velocity command computation. - - Returns: - Dict[str, float]: Velocity commands with 'x_vel' and 'angular_vel' keys - """ - # Get necessary data for planning - costmap = self.get_costmap() - if costmap is None: - logger.warning("No costmap available for planning") - return {"x_vel": 0.0, "angular_vel": 0.0} - - robot_pos, robot_theta = self._get_robot_pose() - robot_x, robot_y = robot_pos - robot_pose = (robot_x, robot_y, robot_theta) - - # Calculate goal-related parameters - goal_x, goal_y = self.goal_xy - dx = goal_x - robot_x - dy = goal_y - robot_y - goal_distance = np.linalg.norm([dx, dy]) - goal_direction = np.arctan2(dy, dx) - robot_theta - goal_direction = normalize_angle(goal_direction) - - self.histogram = self.build_polar_histogram(costmap, robot_pose) - - # If we're ignoring obstacles near the goal, zero out the histogram - if self.ignore_obstacles: - self.histogram = np.zeros_like(self.histogram) - - self.selected_direction = self.select_direction( - self.goal_weight, - self.obstacle_weight, - self.prev_direction_weight, - self.histogram, - goal_direction, - ) - - # Calculate Pure Pursuit Velocities - linear_vel, angular_vel = self.compute_pure_pursuit(goal_distance, self.selected_direction) - - # Slow down when turning sharply - if abs(self.selected_direction) > 0.25: # ~15 degrees - # Scale from 1.0 (small turn) to 0.5 (sharp turn at 90 degrees or more) - turn_factor = max(0.25, 1.0 - (abs(self.selected_direction) / (np.pi / 2))) - linear_vel *= turn_factor - - # Apply Collision Avoidance Stop - skip if ignoring obstacles - if not self.ignore_obstacles and self.check_collision( - self.selected_direction, safety_threshold=0.5 - ): - # Re-select direction prioritizing obstacle avoidance if colliding - self.selected_direction = self.select_direction( - self.goal_weight * 0.2, - self.obstacle_weight, - self.prev_direction_weight * 0.2, - self.histogram, - goal_direction, - ) - linear_vel, angular_vel = self.compute_pure_pursuit( - goal_distance, self.selected_direction - ) - - if self.check_collision(0.0, safety_threshold=self.safety_threshold): - logger.warning("Collision detected ahead. Stopping.") - linear_vel = 0.0 - - self.prev_linear_vel = linear_vel - filtered_linear_vel = self.prev_linear_vel * self.linear_vel_filter_factor + linear_vel * ( - 1 - self.linear_vel_filter_factor - ) - - return {"x_vel": filtered_linear_vel, "angular_vel": angular_vel} - - def _smooth_histogram(self, histogram: np.ndarray) -> np.ndarray: - """ - Apply advanced smoothing to the polar histogram to better identify valleys - and reduce noise. - - Args: - histogram: Raw histogram to smooth - - Returns: - np.ndarray: Smoothed histogram - """ - # Apply a windowed average with variable width based on obstacle density - smoothed = np.zeros_like(histogram) - bins = len(histogram) - - # First pass: basic smoothing with a 5-point kernel - # This uses a wider window than the original 3-point smoother - for i in range(bins): - # Compute indices with wrap-around - indices = [(i + j) % bins for j in range(-2, 3)] - # Apply weighted average (more weight to the center) - weights = [0.1, 0.2, 0.4, 0.2, 0.1] # Sum = 1.0 - smoothed[i] = sum(histogram[idx] * weight for idx, weight in zip(indices, weights)) - - # Second pass: peak and valley enhancement - enhanced = np.zeros_like(smoothed) - for i in range(bins): - # Check neighboring values - prev_idx = (i - 1) % bins - next_idx = (i + 1) % bins - - # Enhance valleys (low values) - if smoothed[i] < smoothed[prev_idx] and smoothed[i] < smoothed[next_idx]: - # It's a local minimum - make it even lower - enhanced[i] = smoothed[i] * 0.8 - # Enhance peaks (high values) - elif smoothed[i] > smoothed[prev_idx] and smoothed[i] > smoothed[next_idx]: - # It's a local maximum - make it even higher - enhanced[i] = min(1.0, smoothed[i] * 1.2) - else: - enhanced[i] = smoothed[i] - - return enhanced - - def build_polar_histogram(self, costmap: Costmap, robot_pose: Tuple[float, float, float]): - """ - Build a polar histogram of obstacle densities around the robot. - - Args: - costmap: Costmap object with grid and metadata - robot_pose: Tuple (x, y, theta) of the robot pose in the odom frame - - Returns: - np.ndarray: Polar histogram of obstacle densities - """ - - # Get grid and find all obstacle cells - occupancy_grid = costmap.grid - y_indices, x_indices = np.where(occupancy_grid > 0) - if len(y_indices) == 0: # No obstacles - return np.zeros(self.histogram_bins) - - # Get robot position in grid coordinates - robot_x, robot_y, robot_theta = robot_pose - robot_point = costmap.world_to_grid((robot_x, robot_y)) - robot_cell_x, robot_cell_y = robot_point.x, robot_point.y - - # Vectorized distance and angle calculation - dx_cells = x_indices - robot_cell_x - dy_cells = y_indices - robot_cell_y - distances = np.sqrt(dx_cells**2 + dy_cells**2) * costmap.resolution - angles_grid = np.arctan2(dy_cells, dx_cells) - angles_robot = normalize_angle(angles_grid - robot_theta) - - # Convert to bin indices - bin_indices = ((angles_robot + np.pi) / (2 * np.pi) * self.histogram_bins).astype( - int - ) % self.histogram_bins - - # Get obstacle values - obstacle_values = occupancy_grid[y_indices, x_indices] / 100.0 - - # Build histogram - histogram = np.zeros(self.histogram_bins) - mask = distances > 0 - # Weight obstacles by inverse square of distance and cell value - np.add.at(histogram, bin_indices[mask], obstacle_values[mask] / (distances[mask] ** 2)) - - # Apply the enhanced smoothing - return self._smooth_histogram(histogram) - - def select_direction( - self, goal_weight, obstacle_weight, prev_direction_weight, histogram, goal_direction - ): - """ - Select best direction based on a simple weighted cost function. - - Args: - goal_weight: Weight for the goal direction component - obstacle_weight: Weight for the obstacle avoidance component - prev_direction_weight: Weight for previous direction consistency - histogram: Polar histogram of obstacle density - goal_direction: Desired direction to goal - - Returns: - float: Selected direction in radians - """ - # Normalize histogram if needed - if np.max(histogram) > 0: - histogram = histogram / np.max(histogram) - - # Calculate costs for each possible direction - angle_diffs = np.abs(normalize_angle(self.angle_mapping - goal_direction)) - prev_diffs = np.abs(normalize_angle(self.angle_mapping - self.prev_selected_angle)) - - # Combine costs with weights - obstacle_costs = obstacle_weight * histogram - goal_costs = goal_weight * angle_diffs - prev_costs = prev_direction_weight * prev_diffs - - total_costs = obstacle_costs + goal_costs + prev_costs - - # Select direction with lowest cost - min_cost_idx = np.argmin(total_costs) - selected_angle = self.angle_mapping[min_cost_idx] - - # Update history for next iteration - self.prev_selected_angle = selected_angle - - return selected_angle - - def compute_pure_pursuit( - self, goal_distance: float, goal_direction: float - ) -> Tuple[float, float]: - """Compute pure pursuit velocities.""" - if goal_distance < self.goal_tolerance: - return 0.0, 0.0 - - lookahead = min(self.lookahead_distance, goal_distance) - linear_vel = min(self.max_linear_vel, goal_distance) - angular_vel = 2.0 * np.sin(goal_direction) / lookahead - angular_vel = max(-self.max_angular_vel, min(angular_vel, self.max_angular_vel)) - - return linear_vel, angular_vel - - def check_collision(self, selected_direction: float, safety_threshold: float = 1.0) -> bool: - """Check if there's an obstacle in the selected direction within safety threshold.""" - # Skip collision check if ignoring obstacles - if self.ignore_obstacles: - return False - - # Get the latest costmap and robot pose - costmap = self.get_costmap() - if costmap is None: - return False # No costmap available - - robot_pos, robot_theta = self._get_robot_pose() - robot_x, robot_y = robot_pos - - # Direction in world frame - direction_world = robot_theta + selected_direction - - # Safety distance in cells - safety_cells = int(safety_threshold / costmap.resolution) - - # Get robot position in grid coordinates - robot_point = costmap.world_to_grid((robot_x, robot_y)) - robot_cell_x, robot_cell_y = robot_point.x, robot_point.y - - # Check for obstacles along the selected direction - for dist in range(1, safety_cells + 1): - # Calculate cell position - cell_x = robot_cell_x + int(dist * np.cos(direction_world)) - cell_y = robot_cell_y + int(dist * np.sin(direction_world)) - - # Check if cell is within grid bounds - if not (0 <= cell_x < costmap.width and 0 <= cell_y < costmap.height): - continue - - # Check if cell contains an obstacle (threshold at 50) - if costmap.grid[int(cell_y), int(cell_x)] > 50: - return True - - return False # No collision detected - - def update_visualization(self) -> np.ndarray: - """Generate visualization of the planning state.""" - try: - costmap = self.get_costmap() - if costmap is None: - raise ValueError("Costmap is None") - - robot_pos, robot_theta = self._get_robot_pose() - robot_x, robot_y = robot_pos - robot_pose = (robot_x, robot_y, robot_theta) - - goal_xy = self.goal_xy # This could be a lookahead point or final goal - - # Get the latest histogram and selected direction, if available - histogram = getattr(self, "histogram", None) - selected_direction = getattr(self, "selected_direction", None) - - # Get waypoint data if in waypoint mode - waypoints_to_draw = self.waypoints_in_absolute - current_wp_index_to_draw = ( - self.current_waypoint_index if self.waypoints_in_absolute is not None else None - ) - # Ensure index is valid before passing - if waypoints_to_draw is not None and current_wp_index_to_draw is not None: - if not (0 <= current_wp_index_to_draw < len(waypoints_to_draw)): - current_wp_index_to_draw = None # Invalidate index if out of bounds - - return visualize_local_planner_state( - occupancy_grid=costmap.grid, - grid_resolution=costmap.resolution, - grid_origin=(costmap.origin.x, costmap.origin.y), - robot_pose=robot_pose, - goal_xy=goal_xy, # Current target (lookahead or final) - goal_theta=self.goal_theta, # Pass goal orientation if available - visualization_size=self.visualization_size, - robot_width=self.robot_width, - robot_length=self.robot_length, - histogram=histogram, - selected_direction=selected_direction, - waypoints=waypoints_to_draw, # Pass the full path - current_waypoint_index=current_wp_index_to_draw, # Pass the target index - ) - except Exception as e: - logger.error(f"Error during visualization update: {e}") - # Return a blank image with error text - blank = ( - np.ones((self.visualization_size, self.visualization_size, 3), dtype=np.uint8) * 255 - ) - cv2.putText( - blank, - "Viz Error", - (self.visualization_size // 4, self.visualization_size // 2), - cv2.FONT_HERSHEY_SIMPLEX, - 1, - (0, 0, 0), - 2, - ) - return blank From 25ae5f4a3efeb2da6da43d99d9820139107c591b Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 17:07:36 +0300 Subject: [PATCH 66/72] removed simple planner --- dimos/robot/local_planner/simple.py | 92 ----------------------------- 1 file changed, 92 deletions(-) delete mode 100644 dimos/robot/local_planner/simple.py diff --git a/dimos/robot/local_planner/simple.py b/dimos/robot/local_planner/simple.py deleted file mode 100644 index 43632d71ca..0000000000 --- a/dimos/robot/local_planner/simple.py +++ /dev/null @@ -1,92 +0,0 @@ -import math -import time -from datetime import datetime -from dataclasses import dataclass -from typing import Callable, Optional - -import reactivex as rx -from plum import dispatch -from reactivex import operators as ops - -from dimos.robot.local_planner.local_planner import LocalPlanner -from dimos.types.costmap import Costmap -from dimos.types.path import Path -from dimos.types.position import Position -from dimos.types.vector import Vector, VectorLike, to_vector -from dimos.utils.logging_config import setup_logger -from dimos.utils.threadpool import get_scheduler - -logger = setup_logger("dimos.robot.unitree.global_planner") - - -# experimental stream based vector based planner -# not production ready - just a sketch -@dataclass -class SimplePlanner(LocalPlanner): - get_costmap: Callable[[], Costmap] - get_robot_pos: Callable[[], Position] - goal: Optional[Vector] = None - path: Optional[Path] = None - speed: float = 0.5 - slowdown_range = 0.5 - - def set_path(self, path: Path) -> bool: - self.path = path - if self.path: - self.set_goal(path.head()) - - def set_goal(self, goal: VectorLike, stop_event=None, goal_theta=None) -> bool: - self.goal = to_vector(goal).to_2d() - self.vis("local_goal", self.goal) - logger.info(f"Setting goal: {self.goal}") - - def speed_ctrl(self, direction: Vector): - dist = direction.length() - if dist >= self.slowdown_range: - f = 1.0 - else: - f = dist / self.slowdown_range - - return self.speed * f - - def yaw_ctrl(self, direction, k_p: float = 0.3, omega_max: float = 1): - alpha = math.atan2(-direction.y, direction.x) - omega = max(-omega_max, min(omega_max, k_p * alpha)) - return Vector(0.0, 0.0, omega) - - def pop_path(self): - self.set_path(self.path.tail()) - return False - - # dumb path popping filter (needs change) - def filter(self, direction): - length = direction.length() - if self.path: - if length < 0.5: - self.pop_path() - return False - else: - if length < 0.2: - return False - - return direction - - def ctrl(self, direction): - jaw = self.yaw_ctrl(direction) - if jaw.length() > 0.25: - return jaw - else: - return (direction.normalize() * self.speed_ctrl(direction)) + jaw - - def get_move_stream(self, frequency: float = 30.0) -> rx.Observable: - return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe( - ops.filter(lambda _: (self.goal is not None) or (self.path is not None)), - ops.map(lambda _: self.get_robot_pos()), - ops.map(lambda odom: odom.vector_to(self.goal)), - ops.filter(self.filter), - ops.map(self.ctrl), - # somewhere here we should detect potential obstacles in direction of travel - # ops.map(lambda direction: direction.normalize() * self.speed_ctrl(direction)), - # ops.map(lambda direction: direction + self.yaw_ctrl(direction)), - ops.map(lambda direction: Vector(-direction.y, direction.x, direction.z)), - ) From 0c840477ffe13a8127c9bd8b5e20b17c5c33fd37 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 17:09:19 +0300 Subject: [PATCH 67/72] reverted run_webrtc --- tests/run_webrtc.py | 49 +++++++++++++++------------------------------ 1 file changed, 16 insertions(+), 33 deletions(-) diff --git a/tests/run_webrtc.py b/tests/run_webrtc.py index 065abb8992..ff96bed14e 100644 --- a/tests/run_webrtc.py +++ b/tests/run_webrtc.py @@ -11,16 +11,20 @@ # 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 cv2 import os -import threading -import time - -import reactivex.operators as ops +import asyncio from dotenv import load_dotenv - -from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 -from dimos.types.vector import Vector +from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2, Color +from dimos.robot.unitree_webrtc.testing.helpers import show3d_stream from dimos.web.websocket_vis.server import WebsocketVis +from dimos.types.vector import Vector +import logging +import open3d as o3d +import reactivex.operators as ops +import numpy as np +import time +import threading # logging.basicConfig(level=logging.DEBUG) @@ -30,13 +34,11 @@ websocket_vis = WebsocketVis() websocket_vis.start() websocket_vis.connect(robot.global_planner.vis_stream()) -websocket_vis.connect(robot.local_planner.vis_stream()) def msg_handler(msgtype, data): if msgtype == "click": try: - print(data) robot.global_planner.set_goal(Vector(data["position"])) except Exception as e: print(f"Error setting goal: {e}") @@ -53,7 +55,6 @@ def threaded_msg_handler(msgtype, data): print("standing up") robot.standup() - print("robot is up") @@ -64,29 +65,11 @@ def newmap(msg): websocket_vis.connect(robot.map_stream.pipe(ops.map(newmap))) websocket_vis.connect(robot.odom_stream().pipe(ops.map(lambda pos: ["robot_pos", pos.pos.to_2d()]))) -# Keep the script running and handle graceful shutdown -shutdown_event = threading.Event() - - -# def signal_handler(signum, frame): -# print("\nShutdown signal received. Cleaning up...") -# shutdown_event.set() - - -# # Register signal handlers for graceful shutdown -# signal.signal(signal.SIGINT, signal_handler) -# signal.signal(signal.SIGTERM, signal_handler) - - -print("Robot is running. Press Ctrl+C to stop.") - try: - # Keep the main thread alive while the robot loop runs in the background - while not shutdown_event.is_set(): - time.sleep(0.1) + while True: + # robot.move_vel(Vector(0.1, 0.1, 0.1)) + time.sleep(0.01) + except KeyboardInterrupt: - print("\nKeyboard interrupt received. Shutting down...") -finally: - print("Stopping robot...") + print("Stopping robot") robot.liedown() - print("Robot stopped.") From 09bd561d726fa96613dcadb2c6133890540275b5 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 18:00:03 +0300 Subject: [PATCH 68/72] documentation for stream replay --- .../unitree_webrtc/type/test_odometry.py | 78 ++++----- docs/testing_stream_reply.md | 156 ++++++++++++++++++ 2 files changed, 187 insertions(+), 47 deletions(-) create mode 100644 docs/testing_stream_reply.md diff --git a/dimos/robot/unitree_webrtc/type/test_odometry.py b/dimos/robot/unitree_webrtc/type/test_odometry.py index 08808e5bfb..74fa512177 100644 --- a/dimos/robot/unitree_webrtc/type/test_odometry.py +++ b/dimos/robot/unitree_webrtc/type/test_odometry.py @@ -14,7 +14,9 @@ from __future__ import annotations +from functools import reduce import math +from operator import sub, add import os import threading from typing import Iterable, Optional @@ -27,81 +29,63 @@ from dimos.robot.unitree_webrtc.unitree_go2 import UnitreeGo2 from dimos.utils.testing import SensorReplay, SensorStorage -_REPLAY_NAME = "raw_odometry_rotate_walk" -_EXPECTED_MESSAGE_COUNT = 179 -_EXPECTED_TOTAL_DEGREES = -592.1696370412612 -_EXPECTED_LAST_YAW_RAD = -1.0452624875249261 - - -def _quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: - """Return yaw (rotation about Z) in *radians* from a quaternion.""" - siny_cosp = 2 * (w * z + x * y) - cosy_cosp = 1 - 2 * (y * y + z * z) - return math.atan2(siny_cosp, cosy_cosp) +_EXPECTED_TOTAL_RAD = -4.05212 def test_dataset_size() -> None: """Ensure the replay contains the expected number of messages.""" - count = sum(1 for _ in SensorReplay(name=_REPLAY_NAME).iterate()) - assert count == _EXPECTED_MESSAGE_COUNT + assert sum(1 for _ in SensorReplay(name="raw_odometry_rotate_walk").iterate()) == 179 def test_odometry_conversion_and_count() -> None: """Each replay entry converts to :class:`Odometry` and count is correct.""" - count = 0 - for raw in SensorReplay(name=_REPLAY_NAME).iterate(): + for raw in SensorReplay(name="raw_odometry_rotate_walk").iterate(): odom = Odometry.from_msg(raw) assert isinstance(raw, dict) assert isinstance(odom, Odometry) - count += 1 - - assert count == _EXPECTED_MESSAGE_COUNT def test_last_yaw_value() -> None: """Verify yaw of the final message (regression guard).""" + last_msg = SensorReplay(name="raw_odometry_rotate_walk").stream().pipe(ops.last()).run() - last_msg = SensorReplay(name=_REPLAY_NAME).stream(rate_hz=None).pipe(ops.last()).run() - - assert last_msg is not None, "Replay is empty" # pragma: no cover + assert last_msg is not None, "Replay is empty" + assert last_msg["data"]["pose"]["orientation"] == { + "x": 0.01077, + "y": 0.008505, + "z": 0.499171, + "w": -0.866395, + } - o = last_msg["data"]["pose"]["orientation"] - yaw = _quaternion_to_yaw(o["x"], o["y"], o["z"], o["w"]) - assert yaw == pytest.approx(_EXPECTED_LAST_YAW_RAD, abs=1e-10) - -def test_total_rotation_from_quaternion() -> None: - """Integrate yaw deltas computed directly from quaternions.""" +def test_total_rotation_travel_iterate() -> None: total_rad = 0.0 prev_yaw: Optional[float] = None - for msg in SensorReplay(name=_REPLAY_NAME).iterate(): - o = msg["data"]["pose"]["orientation"] - yaw = _quaternion_to_yaw(o["x"], o["y"], o["z"], o["w"]) - + for odom in SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg).iterate(): + yaw = odom.rot.z if prev_yaw is not None: diff = yaw - prev_yaw - diff = (diff + math.pi) % (2 * math.pi) - math.pi # normalize total_rad += diff prev_yaw = yaw - assert math.degrees(total_rad) == pytest.approx(_EXPECTED_TOTAL_DEGREES, abs=1e-6) + assert total_rad == pytest.approx(_EXPECTED_TOTAL_RAD, abs=0.001) -def test_total_rotation_from_odometry() -> None: - """`Odometry.rot.z` integration should match quaternion path.""" - total_rad = 0.0 - prev_yaw: Optional[float] = None - - for odom in SensorReplay(name=_REPLAY_NAME, autocast=Odometry.from_msg).iterate(): - yaw = odom.rot.z - if prev_yaw is not None: - diff = yaw - prev_yaw - diff = (diff + math.pi) % (2 * math.pi) - math.pi - total_rad += diff - prev_yaw = yaw +def test_total_rotation_travel_rxpy() -> None: + total_rad = ( + SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg) + .stream() + .pipe( + ops.map(lambda odom: odom.rot.z), + ops.pairwise(), # [1,2,3,4] -> [[1,2], [2,3], [3,4]] + ops.starmap(sub), # [sub(1,2), sub(2,3), sub(3,4)] + ops.reduce(add), + ) + .run() + ) - assert math.degrees(total_rad) == pytest.approx(_EXPECTED_TOTAL_DEGREES, abs=1e-6) + assert total_rad == pytest.approx(4.05, abs=0.01) # data collection tool @@ -112,7 +96,7 @@ def test_store_odometry_stream() -> None: robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") robot.standup() - storage = SensorStorage(_REPLAY_NAME) + storage = SensorStorage("raw_odometry_rotate_walk") storage.save_stream(robot.raw_odom_stream()) shutdown = threading.Event() diff --git a/docs/testing_stream_reply.md b/docs/testing_stream_reply.md new file mode 100644 index 0000000000..8841ca7890 --- /dev/null +++ b/docs/testing_stream_reply.md @@ -0,0 +1,156 @@ +# Stream replying for testing + + +## Goals +- super easy binary data delivery for testing, no data stored in git but lfs +- super easy data stream storage + +## Usage + +you get this SensorReply class that you can use to stream previously stored rxpy streams. +(example below will auto download and extract `raw_odometry_rotate_walk` directory and load & reply a stream stored within it: + +to simply print all messages: + +```python + SensorReplay(name="raw_odometry_rotate_walk").iterate(print) +``` + +or to get an rxpy stream + +```python + SensorReplay(name="raw_odometry_rotate_walk").stream().pipe(...) +``` + +to set the rate + +```python + SensorReplay(name="raw_odometry_rotate_walk").stream(rate_hz=10).pipe(...) +``` + +```sh +ls tests/data/raw_odometry_rotate_walk/ + 000.pickle  019.pickle  005.pickle  032.pickle  023.pickle  052.pickle  056.pickle  045.pickle  065.pickle  071.pickle  091.pickle  088.pickle  110.pickle  105.pickle  108.pickle  118.pickle  119.pickle  140.pickle  151.pickle  142.pickle  165.pickle  159.pickle  172.pickle 012.pickle  010.pickle  016.pickle  027.pickle  030.pickle  043.pickle  044.pickle  039.pickle  062.pickle  076.pickle  079.pickle  092.pickle  112.pickle  106.pickle  111.pickle  126.pickle  123.pickle  135.pickle  143.pickle # etc etc +``` + +TODO - some smarter reply that actually takes into account message timestamps. +can easily be done as a stream processor. + +Slightly fancier test that calculates total change in radians of some odometry stream: + +```python +def test_total_rotation_travel_rxpy() -> None: + total_rad = ( + SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg) + .stream() + .pipe( + ops.map(lambda odom: odom.rot.z), + ops.pairwise(), # [1,2,3,4] -> [[1,2], [2,3], [3,4]] + ops.starmap(sub), # [sub(1,2), sub(2,3), sub(3,4)] + ops.reduce(add), + ) + .run() + ) + + assert total_rad == pytest.approx(4.05, abs=0.01) +``` + +Lidar data reply example (200mb dir) + +```python +def test_robot_mapping(): + lidar_stream = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) + map = Map(voxel_size=0.5) + # this will block until map has consumed the whole stream + map.consume(lidar_stream.stream()).run() + + # we investigate built map + costmap = map.costmap + + assert costmap.grid.shape == (404, 276) + + # etc etc +``` + + +lower level acces to data is + +```python + +absolute_path: Path = testData("some_name") + +``` +it will return a Path, that either points to a file or a dir, and you can do whatever you want with it (could be a video file, a directory with a model or some code even, etc) + +# Implementation + +Anything new you add to `tests/data/*` will be autodetected and you are prompted to push into our LFS store. +It can then be pulled on-demand programatically when/if needed like described above (so that we don't make dimos repo checkout large by default) + +IRL Usage examples: + - `dimos/robot/unitree_webrtc/type/test_odometry.py` + - `dimos/robot/unitree_webrtc/type/test_map.py` + +# Message storage + +if you mark your test with `tool` it will not be ran by default but is a convinient place for codebase specific tooling? not super set on this but convinient for now. +Below uses SensorStorage (as opposed to SensorReplay) to store new messages in `tests/data/*.pickle` + +```python +@pytest.mark.tool +def test_store_odometry_stream() -> None: + load_dotenv() + + robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") + robot.standup() + + storage = SensorStorage("raw_odometry_rotate_walk") + storage.save_stream(robot.raw_odom_stream()) + + shutdown = threading.Event() + + try: + while not shutdown.wait(0.1): + pass + except KeyboardInterrupt: + shutdown.set() + finally: + robot.liedown() +``` + +Anything new that you add to tests/data (can be a directory or file) will be considered "a data blob" and is automatically compressed into `tests/data/.lfs/*.tar.gz` and pushed to LFS. raw data in `tests/data/` is not commited anywhere and is .gitignored. This data is then pulled on demand when needed by others. Upload is done by a simple tool `./bin/lfs_push`. will autodetect new stuff in tests/data/ and automatically compress to `tests/data/.lfs` and push to lfs. You can then commit this new file in `.lfs/` into your PR. + +Below is not neccessary but recommended, if you install pre-commit + +```sh +apt install pre-commit +cd $REPO +pre-commit install +``` + +This will register hooks that will auto-detect changes in `tests/data` (among other things like formatting etc) so you don't forget to add stuff on your commits + +```sh +echo blabla > tests/data/bla.txt +pre-commit run + +CRLF end-lines checker...............................(no files to check)Skipped +CRLF end-lines remover...............................(no files to check)Skipped +Insert license in comments...........................(no files to check)Skipped +ruff format..........................................(no files to check)Skipped +check for case conflicts.............................(no files to check)Skipped +check json...........................................(no files to check)Skipped +check toml...........................................(no files to check)Skipped +check yaml...........................................(no files to check)Skipped +format json..........................................(no files to check)Skipped +LFS data.................................................................Failed +- hook id: lfs_check +- exit code: 1 + +✗ New test data detected at /tests/data: + bla.txt + +Either delete or run ./bin/lfs_push +(lfs_push will compress the files into /tests/data/.lfs/, upload to LFS, and add them to your commit) +``` + From ea7b25b8024aa202685a5a67e188b742d14a080e Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 18:12:06 +0300 Subject: [PATCH 69/72] llm-ed the docs to be way better --- docs/testing_stream_reply.md | 208 ++++++++++++++++++----------------- 1 file changed, 108 insertions(+), 100 deletions(-) diff --git a/docs/testing_stream_reply.md b/docs/testing_stream_reply.md index 8841ca7890..7df8f96747 100644 --- a/docs/testing_stream_reply.md +++ b/docs/testing_stream_reply.md @@ -1,156 +1,164 @@ -# Stream replying for testing +# Sensor Replay & Storage Toolkit +A lightweight framework for **recording, storing, and replaying binary data streams for automated tests**. It keeps your repository small (data lives in Git LFS) while giving you Python‑first ergonomics for working with RxPY streams, point‑clouds, videos, command logs—anything you can pickle. -## Goals -- super easy binary data delivery for testing, no data stored in git but lfs -- super easy data stream storage +--- -## Usage +## 1 At a Glance -you get this SensorReply class that you can use to stream previously stored rxpy streams. -(example below will auto download and extract `raw_odometry_rotate_walk` directory and load & reply a stream stored within it: +| Need | One liner | +| ------------------------------ | ------------------------------------------------------------- | +| **Iterate over every message** | `SensorReplay("raw_odometry_rotate_walk").iterate(print)` | +| **RxPY stream for piping** | `SensorReplay("raw_odometry_rotate_walk").stream().pipe(...)` | +| **Throttle replay rate** | `SensorReplay("raw_odometry_rotate_walk").stream(rate_hz=10)` | +| **Raw path to a blob/dir** | `path = testData("raw_odometry_rotate_walk")` | +| **Store a new stream** | see [`SensorStorage`](#5-storing-new-streams) | -to simply print all messages: +> If the requested blob is missing locally, it is transparently downloaded from Git LFS, extracted to `tests/data//`, and cached for subsequent runs. -```python - SensorReplay(name="raw_odometry_rotate_walk").iterate(print) -``` +--- -or to get an rxpy stream +## 2 Goals -```python - SensorReplay(name="raw_odometry_rotate_walk").stream().pipe(...) -``` +* **Zero setup for CI & collaborators** – data is fetched on demand. +* **No repo bloat** – binaries live in Git LFS; the working tree stays trim. +* **Symmetric API** – `SensorReplay` ↔︎ `SensorStorage`; same name, different direction. +* **Format agnostic** – replay *anything* you can pickle (protobuf, numpy, JPEG, …). +* **Data type agnostic** – with testData("raw_odometry_rotate_walk") you get a Path object back, can be a raw video file, whole codebase, ML model etc -to set the rate + +--- + +## 3 Replaying Data + +### 3.1 Iterating Messages ```python - SensorReplay(name="raw_odometry_rotate_walk").stream(rate_hz=10).pipe(...) -``` +from sensor_tools import SensorReplay -```sh -ls tests/data/raw_odometry_rotate_walk/ - 000.pickle  019.pickle  005.pickle  032.pickle  023.pickle  052.pickle  056.pickle  045.pickle  065.pickle  071.pickle  091.pickle  088.pickle  110.pickle  105.pickle  108.pickle  118.pickle  119.pickle  140.pickle  151.pickle  142.pickle  165.pickle  159.pickle  172.pickle 012.pickle  010.pickle  016.pickle  027.pickle  030.pickle  043.pickle  044.pickle  039.pickle  062.pickle  076.pickle  079.pickle  092.pickle  112.pickle  106.pickle  111.pickle  126.pickle  123.pickle  135.pickle  143.pickle # etc etc +# Print every stored Odometry message +SensorReplay(name="raw_odometry_rotate_walk").iterate(print) ``` -TODO - some smarter reply that actually takes into account message timestamps. -can easily be done as a stream processor. - -Slightly fancier test that calculates total change in radians of some odometry stream: +### 3.2 RxPY Streaming ```python -def test_total_rotation_travel_rxpy() -> None: - total_rad = ( - SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg) - .stream() - .pipe( - ops.map(lambda odom: odom.rot.z), - ops.pairwise(), # [1,2,3,4] -> [[1,2], [2,3], [3,4]] - ops.starmap(sub), # [sub(1,2), sub(2,3), sub(3,4)] - ops.reduce(add), - ) - .run() +from rx import operators as ops +from operator import sub, add +from sensor_tools import SensorReplay, Odometry + +# Compute total yaw rotation (radians) + +total_rad = ( + SensorReplay("raw_odometry_rotate_walk", autocast=Odometry.from_msg) + .stream() + .pipe( + ops.map(lambda odom: odom.rot.z), + ops.pairwise(), # → [(v₀,v₁), (v₁,v₂), …] + ops.starmap(sub), # → [v₁−v₀, v₂−v₁, …] + ops.reduce(add), ) + .run() +) - assert total_rad == pytest.approx(4.05, abs=0.01) +assert total_rad == pytest.approx(4.05, abs=0.01) ``` -Lidar data reply example (200mb dir) +### 3.3 Lidar Mapping Example (200MB blob) ```python -def test_robot_mapping(): - lidar_stream = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) - map = Map(voxel_size=0.5) - # this will block until map has consumed the whole stream - map.consume(lidar_stream.stream()).run() - - # we investigate built map - costmap = map.costmap - - assert costmap.grid.shape == (404, 276) - - # etc etc -``` +from sensor_tools import SensorReplay, LidarMessage +from mapping import Map +lidar_stream = SensorReplay("office_lidar", autocast=LidarMessage.from_msg) +map_ = Map(voxel_size=0.5) -lower level acces to data is +# Blocks until the stream is consumed +map_.consume(lidar_stream.stream()).run() -```python +assert map_.costmap.grid.shape == (404, 276) +``` -absolute_path: Path = testData("some_name") +--- -``` -it will return a Path, that either points to a file or a dir, and you can do whatever you want with it (could be a video file, a directory with a model or some code even, etc) +## 4 Low Level Access -# Implementation +If you want complete control, call **`testData(name)`** to get a `Path` to the extracted file or directory — no pickling assumptions: + +```python +absolute_path: Path = testData("some_name") +``` -Anything new you add to `tests/data/*` will be autodetected and you are prompted to push into our LFS store. -It can then be pulled on-demand programatically when/if needed like described above (so that we don't make dimos repo checkout large by default) +Do whatever you like: open a video file, load a model checkpoint, etc. -IRL Usage examples: - - `dimos/robot/unitree_webrtc/type/test_odometry.py` - - `dimos/robot/unitree_webrtc/type/test_map.py` +--- -# Message storage +## 5 Storing New Streams -if you mark your test with `tool` it will not be ran by default but is a convinient place for codebase specific tooling? not super set on this but convinient for now. -Below uses SensorStorage (as opposed to SensorReplay) to store new messages in `tests/data/*.pickle` +1. **Write a test marked `@pytest.mark.tool`** so CI skips it by default. +2. Use `SensorStorage` to persist the stream into `tests/data//*.pickle`. ```python @pytest.mark.tool -def test_store_odometry_stream() -> None: +def test_store_odometry_stream(): load_dotenv() robot = UnitreeGo2(ip=os.getenv("ROBOT_IP"), mode="ai") robot.standup() - storage = SensorStorage("raw_odometry_rotate_walk") - storage.save_stream(robot.raw_odom_stream()) - - shutdown = threading.Event() + storage = SensorStorage("raw_odometry_rotate_walk2") + storage.save_stream(robot.raw_odom_stream()) # ← records until interrupted try: - while not shutdown.wait(0.1): - pass + while True: + time.sleep(0.1) except KeyboardInterrupt: - shutdown.set() - finally: robot.liedown() ``` -Anything new that you add to tests/data (can be a directory or file) will be considered "a data blob" and is automatically compressed into `tests/data/.lfs/*.tar.gz` and pushed to LFS. raw data in `tests/data/` is not commited anywhere and is .gitignored. This data is then pulled on demand when needed by others. Upload is done by a simple tool `./bin/lfs_push`. will autodetect new stuff in tests/data/ and automatically compress to `tests/data/.lfs` and push to lfs. You can then commit this new file in `.lfs/` into your PR. +### 5.1 Behind the Scenes -Below is not neccessary but recommended, if you install pre-commit +* Any new file/dir under `tests/data/` is treated as a **data blob**. +* `./bin/lfs_push` compresses it into `tests/data/.lfs/.tar.gz` *and* uploads it to Git LFS. +* Only the `.lfs/` archive is committed; raw binaries remain `.gitignored`. -```sh -apt install pre-commit -cd $REPO -pre-commit install -``` +--- -This will register hooks that will auto-detect changes in `tests/data` (among other things like formatting etc) so you don't forget to add stuff on your commits +## 6 Developer Workflow Checklist + +1. **Drop new data** into `tests/data/`. +2. Run `./bin/lfs_push` (or let the pre commit hook nag you). +3. Commit the resulting `tests/data/.lfs/.tar.gz`. +4. Open PR — reviewers pull only what they need, when they need it. + +### 6.1 Pre commit Setup (optional but recommended) ```sh -echo blabla > tests/data/bla.txt -pre-commit run - -CRLF end-lines checker...............................(no files to check)Skipped -CRLF end-lines remover...............................(no files to check)Skipped -Insert license in comments...........................(no files to check)Skipped -ruff format..........................................(no files to check)Skipped -check for case conflicts.............................(no files to check)Skipped -check json...........................................(no files to check)Skipped -check toml...........................................(no files to check)Skipped -check yaml...........................................(no files to check)Skipped -format json..........................................(no files to check)Skipped -LFS data.................................................................Failed -- hook id: lfs_check -- exit code: 1 +sudo apt install pre-commit +pre-commit install # inside repo root +``` -✗ New test data detected at /tests/data: - bla.txt +Now each commit checks formatting, linting, *and* whether you forgot to push new blobs: +``` +$ echo test > tests/data/foo.txt +$ git add tests/data/foo.txt && git commit -m "demo" +LFS data ......................................................... Failed +✗ New test data detected at /tests/data: + foo.txt Either delete or run ./bin/lfs_push -(lfs_push will compress the files into /tests/data/.lfs/, upload to LFS, and add them to your commit) ``` +--- + +## 7 Future Work + +A replay rate that mirrors the **original message timestamps** can be implemented downstream (e.g., an RxPY operator). Contributions welcome! + +--- + +## 8 In the Wild Examples + +* `dimos/robot/unitree_webrtc/type/test_odometry.py` +* `dimos/robot/unitree_webrtc/type/test_map.py` + From b18f81da16bb191f2cb42201568e90ee93533c80 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 18:17:19 +0300 Subject: [PATCH 70/72] further readme cleanup --- docs/testing_stream_reply.md | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/docs/testing_stream_reply.md b/docs/testing_stream_reply.md index 7df8f96747..76e30bbac3 100644 --- a/docs/testing_stream_reply.md +++ b/docs/testing_stream_reply.md @@ -119,19 +119,26 @@ def test_store_odometry_stream(): ### 5.1 Behind the Scenes * Any new file/dir under `tests/data/` is treated as a **data blob**. -* `./bin/lfs_push` compresses it into `tests/data/.lfs/.tar.gz` *and* uploads it to Git LFS. -* Only the `.lfs/` archive is committed; raw binaries remain `.gitignored`. +* `./bin/lfs_push` compresses it into `tests/data/.lfs/.tar.gz` *and* uploads it to Git LFS. +* Only the `.lfs/` archive is committed; raw binaries remain `.gitignored`. --- -## 6 Developer Workflow Checklist +## 6 Storing Arbitrary Binary Data + +Just copy to `tests/data/whatever` +* `./bin/lfs_push` compresses it into `tests/data/.lfs/.tar.gz` *and* uploads it to Git LFS. + +--- + +## 7 Developer Workflow Checklist 1. **Drop new data** into `tests/data/`. 2. Run `./bin/lfs_push` (or let the pre commit hook nag you). 3. Commit the resulting `tests/data/.lfs/.tar.gz`. 4. Open PR — reviewers pull only what they need, when they need it. -### 6.1 Pre commit Setup (optional but recommended) +### 7.1 Pre commit Setup (optional but recommended) ```sh sudo apt install pre-commit @@ -151,13 +158,14 @@ Either delete or run ./bin/lfs_push --- -## 7 Future Work +## 8 Future Work -A replay rate that mirrors the **original message timestamps** can be implemented downstream (e.g., an RxPY operator). Contributions welcome! +- A replay rate that mirrors the **original message timestamps** can be implemented downstream (e.g., an RxPY operator). Contributions welcome! +- Likely this same system should be used for production binary data delivery as well (Models etc) --- -## 8 In the Wild Examples +## 9 Existing Examples * `dimos/robot/unitree_webrtc/type/test_odometry.py` * `dimos/robot/unitree_webrtc/type/test_map.py` From d2c5bf55cf9e2aa0a49617a7bfff2823fbd0af81 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 18:25:39 +0300 Subject: [PATCH 71/72] cleanu --- docs/testing_stream_reply.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/testing_stream_reply.md b/docs/testing_stream_reply.md index 76e30bbac3..490a4dd0c9 100644 --- a/docs/testing_stream_reply.md +++ b/docs/testing_stream_reply.md @@ -160,7 +160,7 @@ Either delete or run ./bin/lfs_push ## 8 Future Work -- A replay rate that mirrors the **original message timestamps** can be implemented downstream (e.g., an RxPY operator). Contributions welcome! +- A replay rate that mirrors the **original message timestamps** can be implemented downstream (e.g., an RxPY operator) - Likely this same system should be used for production binary data delivery as well (Models etc) --- From 93f2d04ac605c526c9a5f8c1218674f6769c9247 Mon Sep 17 00:00:00 2001 From: lesh Date: Tue, 3 Jun 2025 18:38:10 +0300 Subject: [PATCH 72/72] code comments revert --- docs/testing_stream_reply.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/testing_stream_reply.md b/docs/testing_stream_reply.md index 490a4dd0c9..90f9f38b1d 100644 --- a/docs/testing_stream_reply.md +++ b/docs/testing_stream_reply.md @@ -54,8 +54,8 @@ total_rad = ( .stream() .pipe( ops.map(lambda odom: odom.rot.z), - ops.pairwise(), # → [(v₀,v₁), (v₁,v₂), …] - ops.starmap(sub), # → [v₁−v₀, v₂−v₁, …] + ops.pairwise(), # [1,2,3,4] -> [[1,2], [2,3], [3,4]] + ops.starmap(sub), # [sub(1,2), sub(2,3), sub(3,4)] ops.reduce(add), ) .run()