├── .dockerignore ├── .github └── workflows │ └── tests.yml ├── .gitignore ├── .pre-commit-config.yaml ├── .readthedocs.yaml ├── CONTRIBUTING.md ├── LICENSE.md ├── README.md ├── docker-compose.yaml ├── docker ├── Dockerfile └── entrypoint.sh ├── docs ├── Makefile ├── generate_docs.bash ├── python_docs_requirements.txt └── source │ ├── _templates │ └── autosummary │ │ ├── class.rst │ │ └── module.rst │ ├── api.rst │ ├── concepts.rst │ ├── conf.py │ ├── index.rst │ ├── media │ ├── example_navigate.png │ ├── gazebo_classic_demo_world.png │ ├── gazebo_demo_world.png │ ├── pddlstream_demo_ros.png │ ├── pddlstream_demo_standalone.png │ ├── pyrobosim_demo.gif │ ├── pyrobosim_demo.png │ ├── pyrobosim_demo_multirobot.png │ ├── pyrobosim_demo_multirobot_plan.png │ ├── pyrobosim_demo_ros.png │ ├── pyrobosim_partial_observability.png │ └── world_entities.png │ ├── setup.rst │ ├── usage │ ├── basic_usage.rst │ ├── geometry_conventions.rst │ ├── index.rst │ ├── multirobot.rst │ ├── path_planners.rst │ ├── robot_actions.rst │ ├── robot_dynamics.rst │ ├── sensors.rst │ └── tamp.rst │ └── yaml │ ├── index.rst │ ├── location_schema.rst │ ├── object_schema.rst │ └── world_schema.rst ├── mypy.ini ├── pyrobosim ├── .gitignore ├── README.md ├── examples │ ├── demo.py │ ├── demo_astar.py │ ├── demo_dynamics.py │ ├── demo_grasping.py │ ├── demo_pddl.py │ ├── demo_prm.py │ ├── demo_rrt.py │ └── demo_world_save.py ├── pyrobosim │ ├── __init__.py │ ├── core │ │ ├── __init__.py │ │ ├── dynamics.py │ │ ├── gazebo.py │ │ ├── hallway.py │ │ ├── locations.py │ │ ├── objects.py │ │ ├── robot.py │ │ ├── room.py │ │ ├── types.py │ │ ├── world.py │ │ └── yaml_utils.py │ ├── data │ │ ├── .gitignore │ │ ├── example_location_data.yaml │ │ ├── example_location_data_accessories.yaml │ │ ├── example_location_data_furniture.yaml │ │ ├── example_object_data.yaml │ │ ├── example_object_data_drink.yaml │ │ ├── example_object_data_food.yaml │ │ ├── pddlstream │ │ │ └── domains │ │ │ │ ├── 01_simple │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ │ │ ├── 02_derived │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ │ │ ├── 03_nav_stream │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ │ │ ├── 04_nav_manip_stream │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ │ │ ├── 05_nav_grasp_stream │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ │ │ └── 06_open_close_detect │ │ │ │ ├── domain.pddl │ │ │ │ └── streams.pddl │ │ ├── pddlstream_simple_world.yaml │ │ ├── roscon_2024_location_data.yaml │ │ ├── roscon_2024_object_data.yaml │ │ ├── roscon_2024_workshop_world.yaml │ │ ├── sample_models │ │ │ ├── coke_can │ │ │ │ ├── materials │ │ │ │ │ └── textures │ │ │ │ │ │ └── coke_can.png │ │ │ │ ├── meshes │ │ │ │ │ └── coke_can.dae │ │ │ │ ├── model-1_2.sdf │ │ │ │ ├── model-1_3.sdf │ │ │ │ ├── model-1_4.sdf │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ └── first_2015_trash_can │ │ │ │ ├── meshes │ │ │ │ └── trash_can.dae │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ ├── templates │ │ │ ├── link_template_polyline.sdf │ │ │ ├── model_template.config │ │ │ ├── model_template.sdf │ │ │ ├── world_template_gazebo.sdf │ │ │ └── world_template_gazebo_classic.sdf │ │ ├── test_world.yaml │ │ └── test_world_multirobot.yaml │ ├── gui │ │ ├── __init__.py │ │ ├── action_runners.py │ │ ├── main.py │ │ └── world_canvas.py │ ├── manipulation │ │ ├── __init__.py │ │ └── grasping.py │ ├── navigation │ │ ├── __init__.py │ │ ├── a_star.py │ │ ├── execution.py │ │ ├── occupancy_grid.py │ │ ├── prm.py │ │ ├── rrt.py │ │ ├── types.py │ │ ├── visualization.py │ │ └── world_graph.py │ ├── planning │ │ ├── __init__.py │ │ ├── actions.py │ │ └── pddlstream │ │ │ ├── __init__.py │ │ │ ├── default_mappings.py │ │ │ ├── planner.py │ │ │ ├── primitives.py │ │ │ └── utils.py │ ├── sensors │ │ ├── __init__.py │ │ ├── lidar.py │ │ └── types.py │ └── utils │ │ ├── __init__.py │ │ ├── general.py │ │ ├── graph_types.py │ │ ├── knowledge.py │ │ ├── logging.py │ │ ├── path.py │ │ ├── polygon.py │ │ ├── pose.py │ │ ├── search_graph.py │ │ ├── trajectory.py │ │ └── world_motion_planning.py ├── setup.py └── test │ ├── core │ ├── test_dynamics.py │ ├── test_gazebo.py │ ├── test_get_entities.py │ ├── test_hallway.py │ ├── test_locations.py │ ├── test_objects.py │ ├── test_robot.py │ ├── test_room.py │ ├── test_world.py │ └── test_yaml_utils.py │ ├── manipulation │ └── test_grasping.py │ ├── navigation │ ├── test_astar.py │ ├── test_occupancy_grid.py │ ├── test_prm.py │ ├── test_rrt.py │ └── test_world_graph_planner.py │ ├── planning │ ├── test_pddlstream_manip.py │ ├── test_pddlstream_nav.py │ └── test_task_objects.py │ ├── sensors │ └── test_lidar.py │ ├── system │ ├── test_system.py │ └── test_system_world.yaml │ ├── test_pyrobosim.py │ └── utils │ ├── test_general_utils.py │ ├── test_knowledge_utils.py │ ├── test_motion_utils.py │ ├── test_polygon_utils.py │ ├── test_pose_utils.py │ ├── test_search_graph.py │ └── test_trajectory.py ├── pyrobosim_msgs ├── CMakeLists.txt ├── action │ ├── DetectObjects.action │ ├── ExecuteTaskAction.action │ ├── ExecuteTaskPlan.action │ ├── FollowPath.action │ └── PlanPath.action ├── msg │ ├── ExecutionResult.msg │ ├── GoalPredicate.msg │ ├── GoalSpecification.msg │ ├── HallwayState.msg │ ├── LocationState.msg │ ├── ObjectState.msg │ ├── Path.msg │ ├── RobotState.msg │ ├── TaskAction.msg │ ├── TaskPlan.msg │ └── WorldState.msg ├── package.xml └── srv │ ├── RequestWorldState.srv │ ├── ResetWorld.srv │ └── SetLocationState.srv ├── pyrobosim_ros ├── CMakeLists.txt ├── examples │ ├── demo.py │ ├── demo_commands.py │ ├── demo_pddl_goal_publisher.py │ ├── demo_pddl_planner.py │ ├── demo_pddl_world.py │ └── demo_velocity_publisher.py ├── launch │ ├── demo.launch.py │ ├── demo_commands.launch.py │ ├── demo_commands_multirobot.launch.py │ └── demo_pddl.launch.py ├── package.xml ├── pyrobosim_ros │ ├── __init__.py │ ├── ros_conversions.py │ └── ros_interface.py ├── setup.py └── test │ ├── CMakeLists.txt │ ├── test_ros_conversions.py │ └── test_ros_interface.py ├── pytest.ini ├── setup ├── configure_pddlstream.bash ├── setup_pyrobosim.bash └── source_pyrobosim.bash └── test ├── python_test_requirements.txt └── run_tests.bash /.dockerignore: -------------------------------------------------------------------------------- 1 | # Files and folders that don't need to be in the Docker build context. 2 | docker-compose.yaml 3 | dependencies/ 4 | docker/Dockerfile 5 | test/results/ 6 | 7 | # Metadata that doesn't need to be in the Docker build context. 8 | */__pycache__/ 9 | *.egg-info 10 | .dockerignore 11 | .git/ 12 | .github/ 13 | .vscode/ 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Environment file written by setup scripts 2 | pyrobosim.env 3 | 4 | # Dependencies folder 5 | dependencies/ 6 | 7 | # VS Code metadata 8 | .vscode/ 9 | 10 | # Python cache and metadata folders 11 | __pycache__/ 12 | .mypy_cache/ 13 | .pytest_cache/ 14 | *.egg-info/ 15 | 16 | # Sphinx documentation 17 | docs/build/ 18 | docs/source/generated 19 | 20 | # Build artifacts 21 | **/build/ 22 | 23 | # Test artifacts 24 | **/.coverage 25 | test/results/ 26 | 27 | # Temporary folders generated by PDDLStream 28 | **/statistics/ 29 | **/temp/ 30 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | 3 | # Runs pre-commit hooks and other file format checks. 4 | - repo: https://github.com/pre-commit/pre-commit-hooks 5 | rev: v5.0.0 6 | hooks: 7 | - id: check-added-large-files 8 | - id: check-ast 9 | - id: check-builtin-literals 10 | - id: check-case-conflict 11 | - id: check-docstring-first 12 | - id: check-executables-have-shebangs 13 | - id: check-json 14 | - id: check-merge-conflict 15 | - id: check-symlinks 16 | - id: check-toml 17 | - id: check-vcs-permalinks 18 | - id: check-yaml 19 | - id: debug-statements 20 | - id: destroyed-symlinks 21 | - id: detect-private-key 22 | - id: end-of-file-fixer 23 | - id: fix-byte-order-marker 24 | - id: forbid-new-submodules 25 | - id: mixed-line-ending 26 | - id: name-tests-test 27 | - id: requirements-txt-fixer 28 | - id: sort-simple-yaml 29 | - id: trailing-whitespace 30 | 31 | # Autoformats Python code. 32 | - repo: https://github.com/psf/black.git 33 | rev: 25.1.0 34 | hooks: 35 | - id: black 36 | 37 | # Removes unused imports. 38 | - repo: https://github.com/PyCQA/autoflake 39 | rev: v2.3.1 40 | hooks: 41 | - id: autoflake 42 | 43 | # Finds spelling issues in code. 44 | - repo: https://github.com/codespell-project/codespell 45 | rev: v2.4.1 46 | hooks: 47 | - id: codespell 48 | 49 | # Finds issues in YAML files. 50 | - repo: https://github.com/adrienverge/yamllint 51 | rev: v1.37.1 52 | hooks: 53 | - id: yamllint 54 | args: 55 | [ 56 | "--no-warnings", 57 | "--config-data", 58 | "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", 59 | ] 60 | types: [text] 61 | files: \.(yml|yaml)$ 62 | 63 | # Checks links in markdown files. 64 | - repo: https://github.com/tcort/markdown-link-check 65 | rev: v3.13.7 66 | hooks: 67 | - id: markdown-link-check 68 | 69 | # Type checking -- see mypy.ini for configuration 70 | - repo: https://github.com/pre-commit/mirrors-mypy 71 | rev: v1.16.0 72 | hooks: 73 | - id: mypy 74 | 75 | ci: 76 | autofix_commit_msg: | 77 | [pre-commit.ci] auto fixes from pre-commit.com hooks 78 | 79 | for more information, see https://pre-commit.ci 80 | autofix_prs: true 81 | autoupdate_branch: '' 82 | autoupdate_commit_msg: '[pre-commit.ci] pre-commit autoupdate' 83 | autoupdate_schedule: weekly 84 | skip: ['markdown-link-check'] 85 | submodules: false 86 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yaml 2 | version: 2 3 | 4 | # Set the version of Python and other tools 5 | build: 6 | os: ubuntu-24.04 7 | tools: 8 | python: "3.12" 9 | 10 | # Install the Python modules to generate the documentation 11 | python: 12 | install: 13 | - requirements: docs/python_docs_requirements.txt 14 | - method: pip 15 | path: pyrobosim 16 | - method: pip 17 | path: pyrobosim_ros 18 | 19 | # Build documentation in the Sphinx source directory 20 | sphinx: 21 | configuration: docs/source/conf.py 22 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing to PyRoboSim 2 | 3 | Thank you for considering a contribution to PyRoboSim! 4 | 5 | While jumping into someone else's code base can be challenging, here are some tips to help you navigate contributing to this repository. 6 | 7 | ## Getting Started 8 | 9 | Before you can contribute, you should first make sure you can run PyRoboSim. 10 | Refer to the [setup documentation](https://pyrobosim.readthedocs.io/en/latest/setup.html), and choose either a local or Docker based installation. 11 | 12 | ## Submitting an Issue 13 | 14 | If you have any issues with PyRoboSim, or any ideas to make the tool better for yourself and others, please [submit a Git issue](https://github.com/sea-bass/pyrobosim/issues/new)! 15 | 16 | Make sure to take a look at the [list of open issues](https://github.com/sea-bass/pyrobosim/issues) first. 17 | 18 | ## Making Contributions 19 | 20 | Before submitting a pull request (PR) to this repository, there are a few tools you can use to increase the chance that your changes will be accepted. 21 | 22 | ### Formatting 23 | 24 | We use [pre-commit](https://pre-commit.com/) for code formatting. 25 | These checks must pass for a PR to be merged. 26 | 27 | To set up pre-commit locally on your system: 28 | 29 | * Install pre-commit: `pip3 install pre-commit`. 30 | * Go to the root folder of this repository. 31 | * Run `pre-commit run -a` before committing changes. 32 | * Commit any of the changes that were automatically made, or fix them otherwise. 33 | 34 | NOTE: You can also run `pre-commit install` to automatically run pre-commit before your commits. 35 | 36 | ### Tests 37 | 38 | You can run all unit tests locally before you push to your branch and wait for CI to complete. 39 | To do this: 40 | 41 | * Go to the root folder of this repository. 42 | * Run `test/run_tests.bash` (or `test/run_tests.bash $ROS_DISTRO` if using ROS) 43 | 44 | The above script runs [`pytest`](https://docs.pytest.org/) with the settings configured for this test. 45 | You can also look at, and modify, the `pytest.ini` file at the root of this repository. 46 | 47 | If you are using a Docker based workflow, you can also run the tests inside a container: 48 | 49 | ``` 50 | docker compose run test 51 | ``` 52 | 53 | In both cases, the latest test results will be saved to the `test/results` folder. 54 | You can open the `test_results.html` file using your favorite browser if you want to dig into more details about your test results. 55 | 56 | ### Documentation 57 | 58 | You can also build the PyRoboSim documentation locally. 59 | This lets you verify how changes to the documentation (new pages, changes to existing pages, docstrings in the Python code, etc.) look before you push to your branch. 60 | 61 | The docs are built using [Sphinx](https://www.sphinx-doc.org/en/master/) and [ReadTheDocs (RTD)](https://about.readthedocs.com/). 62 | 63 | To build docs locally: 64 | 65 | * Install the packages needed to build docs: `pip3 install -r docs/python_docs_requirements.txt` 66 | * Go to the root folder of this repository. 67 | * Run `docs/generate_docs.bash` 68 | 69 | This will generate docs pages to the `docs/build` folder. 70 | You can view the homepage by opening `docs/build/html/index.html` using your favorite browser. 71 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022-2025 Sebastian Castro 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PyRoboSim 2 | 3 | [![PyRoboSim Tests](https://github.com/sea-bass/pyrobosim/actions/workflows/tests.yml/badge.svg?branch=main)](https://github.com/sea-bass/pyrobosim/actions/workflows/tests.yml) 4 | [![Documentation Status](https://readthedocs.org/projects/pyrobosim/badge/?version=latest)](https://pyrobosim.readthedocs.io/en/latest/?badge=latest) 5 | ![Coverage Status](https://img.shields.io/endpoint?url=https://gist.githubusercontent.com/sea-bass/3761a8aa05af7b0e8c84210b9d103df8/raw/pyrobosim-test-coverage.json) 6 | 7 | ROS 2 enabled 2D mobile robot simulator for behavior prototyping. 8 | 9 | By Sebastian Castro, 2022-2025 10 | 11 | Refer to the [full documentation](https://pyrobosim.readthedocs.io/) for setup, usage, and other concepts. 12 | 13 | We look forward to your open-source contributions to PyRoboSim. 14 | For more information, refer to the [contributor guide](CONTRIBUTING.md). 15 | 16 | ![Example animation of the simulator](docs/source/media/pyrobosim_demo.gif) 17 | -------------------------------------------------------------------------------- /docker-compose.yaml: -------------------------------------------------------------------------------- 1 | # Docker Compose file for PyRoboSim 2 | # 3 | # Usage: 4 | # 5 | # To build the images: 6 | # docker compose build 7 | # 8 | # To run a specific service by name: 9 | # docker compose run 10 | # 11 | # To open an interactive shell to a running container: 12 | # docker exec -it bash 13 | 14 | services: 15 | base: 16 | image: pyrobosim_ros:${ROS_DISTRO:-humble} 17 | build: 18 | context: . 19 | dockerfile: docker/Dockerfile 20 | target: pyrobosim_ros 21 | args: 22 | ROS_DISTRO: ${ROS_DISTRO:-humble} 23 | # Ensures signals are actually passed and reaped in the container for shutdowns. 24 | # https://docs.docker.com/compose/compose-file/compose-file-v3/#init 25 | init: true 26 | # Interactive shell 27 | stdin_open: true 28 | tty: true 29 | # Networking and IPC for ROS 2 30 | network_mode: host 31 | ipc: host 32 | # Allows graphical programs in the container. 33 | environment: 34 | - DISPLAY=${DISPLAY} 35 | - QT_X11_NO_MITSHM=1 36 | - NVIDIA_DRIVER_CAPABILITIES=all 37 | volumes: 38 | # Mount the source code. 39 | - ./pyrobosim/:/pyrobosim_ws/src/pyrobosim/:rw 40 | - ./pyrobosim_msgs/:/pyrobosim_ws/src/pyrobosim_msgs/:rw 41 | - ./pyrobosim_ros/:/pyrobosim_ws/src/pyrobosim_ros/:rw 42 | # Mount docs and testing utilities. 43 | - ./docs/:/pyrobosim_ws/src/docs/:rw 44 | - ./test/:/pyrobosim_ws/src/test/:rw 45 | - ./pytest.ini:/pyrobosim_ws/pytest.ini:rw 46 | # Allows graphical programs in the container. 47 | - /tmp/.X11-unix:/tmp/.X11-unix:rw 48 | - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority 49 | command: sleep infinity 50 | 51 | docs: 52 | extends: base 53 | command: src/docs/generate_docs.bash 54 | test: 55 | extends: base 56 | command: src/test/run_tests.bash ${ROS_DISTRO:-humble} 57 | 58 | ############### 59 | # Basic demos # 60 | ############### 61 | demo: 62 | extends: base 63 | command: python3 src/pyrobosim/examples/demo.py 64 | demo_multirobot: 65 | extends: base 66 | command: python3 src/pyrobosim/examples/demo.py --multirobot 67 | demo_ros: 68 | extends: base 69 | command: ros2 launch pyrobosim_ros demo.launch.py 70 | demo_multirobot_ros: 71 | extends: base 72 | command: ros2 launch pyrobosim_ros demo_commands_multirobot.launch.py 73 | 74 | ################################## 75 | # Task and Motion Planning demos # 76 | ################################## 77 | demo_pddl: 78 | extends: base 79 | command: python3 src/pyrobosim/examples/demo_pddl.py --example 01_simple --verbose 80 | demo_pddl_ros: 81 | extends: base 82 | command: ros2 launch pyrobosim_ros demo_pddl.launch.py example:=01_simple subscribe:=true verbose:=true 83 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # Dockerfile for PyRoboSim builds with and without ROS 2 | ARG ROS_DISTRO=humble 3 | 4 | ################################## 5 | # 6 | # base PyRoboSim build for Ubuntu 7 | # 8 | ################################## 9 | FROM ubuntu:latest AS pyrobosim 10 | 11 | # Install dependencies 12 | ARG DEBIAN_FRONTEND=noninteractive 13 | RUN apt-get update &&\ 14 | apt-get install -y \ 15 | apt-utils \ 16 | libegl1 \ 17 | libgl1-mesa-dev \ 18 | python3 \ 19 | python3-pip \ 20 | python3-tk \ 21 | git \ 22 | cmake \ 23 | ffmpeg \ 24 | libsm6 \ 25 | libxext6 26 | 27 | RUN mkdir -p /opt/pyrobosim/test 28 | RUN mkdir -p /opt/pyrobosim/setup 29 | WORKDIR /opt/pyrobosim/ 30 | 31 | # Install PDDLStream 32 | COPY setup/configure_pddlstream.bash setup/ 33 | RUN setup/configure_pddlstream.bash 34 | 35 | # Install pip dependencies for docs and testing 36 | ENV PIP_BREAK_SYSTEM_PACKAGES=1 37 | COPY docs/python_docs_requirements.txt docs/ 38 | COPY test/python_test_requirements.txt test/ 39 | RUN pip3 install -r docs/python_docs_requirements.txt && \ 40 | pip3 install -r test/python_test_requirements.txt 41 | 42 | # Copy the rest of the source directory and install PyRoboSim 43 | COPY . /opt/pyrobosim/ 44 | RUN pip3 install -e pyrobosim 45 | 46 | # Set the default startup command 47 | CMD /bin/bash 48 | 49 | ################################## 50 | # 51 | # PyRoboSim build for Ubuntu / ROS 52 | # 53 | ################################## 54 | FROM ros:${ROS_DISTRO}-ros-base AS pyrobosim_ros 55 | ENV ROS_DISTRO=${ROS_DISTRO} 56 | SHELL ["/bin/bash", "-o", "pipefail", "-c"] 57 | 58 | # Install dependencies 59 | ARG DEBIAN_FRONTEND=noninteractive 60 | RUN apt-get update && apt-get upgrade -y 61 | RUN apt-get install -y \ 62 | apt-utils python3-pip python3-tk \ 63 | libegl1 libgl1-mesa-dev libglu1-mesa-dev '^libxcb.*-dev' libx11-xcb-dev \ 64 | libxi-dev libxkbcommon-dev libxkbcommon-x11-dev libxrender-dev 65 | 66 | # Create a ROS 2 workspace 67 | RUN mkdir -p /pyrobosim_ws/src/pyrobosim 68 | WORKDIR /pyrobosim_ws/src 69 | 70 | # Install dependencies 71 | COPY setup/configure_pddlstream.bash /pyrobosim_ws/src/setup/ 72 | RUN setup/configure_pddlstream.bash 73 | 74 | # Install PyRoboSim, docs, and testing dependencies 75 | ENV PIP_BREAK_SYSTEM_PACKAGES=1 76 | COPY pyrobosim/setup.py pyrobosim/ 77 | COPY docs/python_docs_requirements.txt docs/ 78 | COPY test/python_test_requirements.txt test/ 79 | RUN pip3 install -e ./pyrobosim && \ 80 | pip3 install -r docs/python_docs_requirements.txt && \ 81 | pip3 install -r test/python_test_requirements.txt 82 | 83 | # Build the ROS workspace, which includes PyRoboSim 84 | COPY . /pyrobosim_ws/src/ 85 | WORKDIR /pyrobosim_ws 86 | RUN . /opt/ros/${ROS_DISTRO}/setup.bash && \ 87 | colcon build 88 | 89 | # Remove display warnings 90 | RUN mkdir /tmp/runtime-root 91 | ENV XDG_RUNTIME_DIR "/tmp/runtime-root" 92 | RUN chmod -R 0700 /tmp/runtime-root 93 | ENV NO_AT_BRIDGE 1 94 | 95 | # Setup an entrypoint and working folder 96 | COPY setup /pyrobosim_ws/src/setup 97 | CMD /bin/bash 98 | COPY docker/entrypoint.sh /entrypoint.sh 99 | ENTRYPOINT ["/entrypoint.sh"] 100 | RUN echo "source /entrypoint.sh" >> ~/.bashrc 101 | -------------------------------------------------------------------------------- /docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Source ROS and the PyRoboSim workspace 4 | source /opt/ros/${ROS_DISTRO}/setup.bash 5 | if [ ! -f /pyrobosim_ws/install/setup.bash ] 6 | then 7 | colcon build 8 | fi 9 | source /pyrobosim_ws/install/setup.bash 10 | 11 | # Add dependencies to path 12 | PDDLSTREAM_PATH=/pyrobosim_ws/src/dependencies/pddlstream 13 | if [ -d "$PDDLSTREAM_PATH" ] 14 | then 15 | export PYTHONPATH=$PDDLSTREAM_PATH:$PYTHONPATH 16 | fi 17 | 18 | # Execute the command passed into this entrypoint 19 | exec "$@" 20 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/generate_docs.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Generate Sphinx documentation 4 | # 5 | # Note that you may need some additional Python packages: 6 | # pip3 install -r docs/python_docs_requirements.txt 7 | 8 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 9 | source "${SCRIPT_DIR}/../setup/source_pyrobosim.bash ${ROS_DISTRO}" 10 | pushd "${SCRIPT_DIR}" > /dev/null || exit 11 | rm -rf build/ 12 | rm -rf source/generated 13 | make html 14 | popd > /dev/null || exit 15 | -------------------------------------------------------------------------------- /docs/python_docs_requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx 2 | sphinx-copybutton 3 | sphinx_rtd_theme 4 | sphinxcontrib-youtube 5 | -------------------------------------------------------------------------------- /docs/source/_templates/autosummary/class.rst: -------------------------------------------------------------------------------- 1 | {{ fullname }} 2 | {{ underline }} 3 | 4 | .. currentmodule:: {{ module }} 5 | 6 | .. autoclass:: {{ objname }} 7 | 8 | {% block methods %} 9 | 10 | {% if methods %} 11 | .. rubric:: Methods 12 | 13 | .. autosummary:: 14 | :toctree: {{ objname }} 15 | {% for item in methods %} 16 | {%- if item not in inherited_members %} 17 | ~{{ name }}.{{ item }} 18 | {%- endif %} 19 | {%- endfor %} 20 | {% endif %} 21 | {% endblock %} 22 | 23 | {% block attributes %} 24 | {% if attributes %} 25 | .. rubric:: Attributes 26 | 27 | .. autosummary:: 28 | :toctree: {{ objname }} 29 | {% for item in attributes %} 30 | {%- if item not in inherited_members %} 31 | ~{{ name }}.{{ item }} 32 | {%- endif %} 33 | {%- endfor %} 34 | {% endif %} 35 | {% endblock %} 36 | -------------------------------------------------------------------------------- /docs/source/_templates/autosummary/module.rst: -------------------------------------------------------------------------------- 1 | {{ fullname | escape | underline}} 2 | 3 | .. automodule:: {{ fullname }} 4 | 5 | {% block attributes %} 6 | {% if attributes %} 7 | .. rubric:: Module Attributes 8 | 9 | .. autosummary:: 10 | :toctree: 11 | {% for item in attributes %} 12 | {{ item }} 13 | {%- endfor %} 14 | {% endif %} 15 | {% endblock %} 16 | 17 | {% block classes %} 18 | {% if classes %} 19 | .. rubric:: {{ _('Classes') }} 20 | 21 | .. autosummary:: 22 | :toctree: 23 | {% for item in classes %} 24 | {{ item }} 25 | {%- endfor %} 26 | {% endif %} 27 | {% endblock %} 28 | 29 | {% block functions %} 30 | {% if functions %} 31 | .. rubric:: {{ _('Functions') }} 32 | 33 | .. autosummary:: 34 | :toctree: 35 | {% for item in functions %} 36 | {{ item }} 37 | {%- endfor %} 38 | {% endif %} 39 | {% endblock %} 40 | 41 | {% block exceptions %} 42 | {% if exceptions %} 43 | .. rubric:: {{ _('Exceptions') }} 44 | 45 | .. autosummary:: 46 | :toctree: 47 | {% for item in exceptions %} 48 | {{ item }} 49 | {%- endfor %} 50 | {% endif %} 51 | {% endblock %} 52 | 53 | {% block modules %} 54 | {% if modules %} 55 | .. rubric:: Modules 56 | 57 | .. autosummary:: 58 | :toctree: 59 | :recursive: 60 | {% for item in modules %} 61 | {{ item }} 62 | {%- endfor %} 63 | {% endif %} 64 | {% endblock %} 65 | -------------------------------------------------------------------------------- /docs/source/api.rst: -------------------------------------------------------------------------------- 1 | API 2 | === 3 | 4 | .. autosummary:: 5 | :recursive: 6 | :toctree: generated 7 | 8 | pyrobosim 9 | pyrobosim_ros 10 | -------------------------------------------------------------------------------- /docs/source/concepts.rst: -------------------------------------------------------------------------------- 1 | Concepts 2 | ======== 3 | 4 | PyRoboSim is primarily a world modeling framework for prototyping high-level robotics behavior applications. 5 | 6 | 7 | Worlds 8 | ------ 9 | 10 | Worlds in PyRoboSim consist of a hierarchy of polygonal *entities*, including: 11 | 12 | * **Robots**: A movable entity capable of actions that can change its own state and the state of the world. 13 | * **Rooms**: Regions that a robot can navigate. 14 | * **Hallways**: Regions connecting two rooms, which a robot can navigate, open, and close. 15 | * **Locations**: Regions inside rooms that may contain objects (e.g., furniture or storage locations). 16 | * **Object Spawns**: Subregions of locations where objects may exist (e.g., a left vs. right countertop). 17 | * **Objects**: Discrete entities that can be manipulated around the world. 18 | 19 | This is all represented in a 2.5D environment (SE(2) pose with vertical (Z) height). 20 | However, full 3D poses are representable as well. 21 | For more information, refer to the :ref:`geometry_conventions` section. 22 | 23 | .. image:: media/world_entities.png 24 | :align: center 25 | :width: 600px 26 | :alt: Entities in a world. 27 | 28 | | 29 | 30 | Actions 31 | ------- 32 | 33 | Within the world, we can spawn a robot that can perform a set of *actions*, such as navigating, picking, and placing. 34 | To learn more, refer to :ref:`robot_actions`. 35 | 36 | These actions can be specified individually, or as a sequence of actions (a *plan*). 37 | Actions or plans can be commanded directly, e.g., "go to the table and pick up an apple". 38 | They can also come from a :ref:`task_and_motion_planning` framework that accepts a task specification (e.g., "all apples should be on the kitchen table") and outputs a plan that, when executed, satisfies the specification. 39 | 40 | For example, here is a robot performing a **Navigate** action from the kitchen to the desk in our simple test world. 41 | 42 | .. image:: media/example_navigate.png 43 | :align: center 44 | :width: 600px 45 | :alt: Example navigation action. 46 | 47 | | 48 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | 17 | 18 | # -- Project information ----------------------------------------------------- 19 | 20 | project = "pyrobosim" 21 | copyright = "2022-2025, Sebastian Castro" 22 | author = "Sebastian Castro" 23 | 24 | # The full version, including alpha/beta/rc tags 25 | version = release = "4.0.0" 26 | 27 | 28 | # -- General configuration --------------------------------------------------- 29 | 30 | # Add any Sphinx extension module names here, as strings. They can be 31 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 32 | # ones. 33 | extensions = [ 34 | "sphinx.ext.autodoc", 35 | "sphinx.ext.autosummary", 36 | "sphinx_rtd_theme", 37 | "sphinx_copybutton", 38 | "sphinxcontrib.youtube", 39 | ] 40 | 41 | # Add any paths that contain templates here, relative to this directory. 42 | templates_path = ["_templates"] 43 | 44 | # List of patterns, relative to source directory, that match files and 45 | # directories to ignore when looking for source files. 46 | # This pattern also affects html_static_path and html_extra_path. 47 | exclude_patterns: list[str] = [] 48 | 49 | # Mock imports for external dependencies. 50 | autodoc_mock_imports = [ 51 | "action_msgs", 52 | "geometry_msgs", 53 | "pddlstream", 54 | "pyrobosim_msgs", 55 | "rclpy", 56 | "std_srvs", 57 | ] 58 | 59 | 60 | # -- Options for HTML output ------------------------------------------------- 61 | 62 | # The theme to use for HTML and HTML Help pages. See the documentation for 63 | # a list of builtin themes. 64 | # 65 | html_theme = "sphinx_rtd_theme" 66 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. PyRoboSim documentation master file. 2 | 3 | PyRoboSim 4 | ========= 5 | 6 | PyRoboSim is a ROS 2 enabled 2D mobile robot simulator for behavior prototyping. 7 | 8 | .. youtube:: K5359uSeyVA 9 | 10 | | 11 | 12 | We look forward to your open-source contributions to PyRoboSim. 13 | For more information, refer to the `contributor guide on GitHub `_. 14 | 15 | 16 | Vision Statement 17 | ---------------- 18 | 19 | The vision for PyRoboSim is that you will be able to **create worlds** to prototype your robot behavior in a simple environment before moving to a more realistic simulator, or even real robot hardware. 20 | 21 | To enable this, a typical user of PyRoboSim would: 22 | 23 | * **Build complex worlds** using the world modeling framework, both manually and programmatically. 24 | * **Define custom actions and action executors** (e.g., path planning/following or decision-making algorithms). 25 | * **Design task and motion planners** that go from task specification to an executable plan. 26 | * **Export worlds to Gazebo** to test in a 3D world with more complex sensing and actuation models. 27 | 28 | 29 | Usage Examples 30 | -------------- 31 | 32 | Below is a list of known projects that use PyRoboSim. 33 | 34 | * `Home Service Robotics at MIT CSAIL `_, by Sebastian Castro (2020) -- the precursor to this work! 35 | * `Task Planning in Robotics `_ and `Integrated Task and Motion Planning in Robotics `_ blog posts, by Sebastian Castro (2022) 36 | * `Hierarchically Decentralized Heterogeneous Multi-Robot Task Allocation System `_, by Sujeet Kashid and Ashwin D. Kumat (2024) 37 | * `Hands-On with ROS 2 Deliberation Technologies `_ workshop at `ROSCon 2024 `_, by Christian Henkel, Sebastian Castro, Davide Faconti, David Oberacker, David Conner, and Matthias Mayr (2024) 38 | * `AutoAPMS: A unified software framework for creating behavior-based robotic applications `_, by Robin Müller (2025) 39 | * `Collaborative Large Language Models for Task Allocation in Construction Robots `_, by Samuel A. Prieto and Borja García de Soto (2025) 40 | 41 | If you have something to add, please submit a pull request! 42 | 43 | 44 | .. toctree:: 45 | :maxdepth: 2 46 | :caption: Contents: 47 | 48 | concepts 49 | setup 50 | usage/index 51 | yaml/index 52 | api 53 | 54 | 55 | Indices and tables 56 | ================== 57 | 58 | * :ref:`genindex` 59 | * :ref:`modindex` 60 | * :ref:`search` 61 | -------------------------------------------------------------------------------- /docs/source/media/example_navigate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/example_navigate.png -------------------------------------------------------------------------------- /docs/source/media/gazebo_classic_demo_world.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/gazebo_classic_demo_world.png -------------------------------------------------------------------------------- /docs/source/media/gazebo_demo_world.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/gazebo_demo_world.png -------------------------------------------------------------------------------- /docs/source/media/pddlstream_demo_ros.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pddlstream_demo_ros.png -------------------------------------------------------------------------------- /docs/source/media/pddlstream_demo_standalone.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pddlstream_demo_standalone.png -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_demo.gif -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_demo.png -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_demo_multirobot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_demo_multirobot.png -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_demo_multirobot_plan.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_demo_multirobot_plan.png -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_demo_ros.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_demo_ros.png -------------------------------------------------------------------------------- /docs/source/media/pyrobosim_partial_observability.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/pyrobosim_partial_observability.png -------------------------------------------------------------------------------- /docs/source/media/world_entities.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/docs/source/media/world_entities.png -------------------------------------------------------------------------------- /docs/source/setup.rst: -------------------------------------------------------------------------------- 1 | Setup 2 | ===== 3 | 4 | This package is being tested with: 5 | 6 | * Python 3.10 / Ubuntu 22.04 (optionally with ROS 2 Humble) 7 | * Python 3.12 / Ubuntu 24.04 (optionally with ROS 2 Jazzy, Kilted, or Rolling) 8 | 9 | pip install (Limited) 10 | --------------------- 11 | 12 | You can quickly install PyRoboSim through PyPi. 13 | However, note that this will not include any of the ROS 2 or Task and Motion Planning functionality. 14 | 15 | :: 16 | 17 | pip install pyrobosim 18 | 19 | 20 | Local Setup 21 | ----------- 22 | 23 | If using ROS 2, clone this repo in a valid `ROS 2 workspace `_. 24 | Otherwise, if running standalone, clone it wherever you would like. 25 | 26 | To set up your Python virtual environment, configure and run 27 | 28 | :: 29 | 30 | ./setup/setup_pyrobosim.bash 31 | 32 | By default, this will create a Python virtual environment in ``~/python-virtualenvs/pyrobosim``. 33 | You can change this location by modifying the script. 34 | 35 | When you run this script, you will get prompts for optionally setting up ROS 2 and PDDLStream for task and motion planning. 36 | 37 | To then setup the environment, run 38 | 39 | :: 40 | 41 | source ./setup/source_pyrobosim.bash 42 | 43 | We recommend making a bash function in your ``~/.bashrc`` file (or equivalent) so you can easily just call ``pyrobosim`` from your shell to get started. 44 | 45 | :: 46 | 47 | pyrobosim() { 48 | source /path/to/pyrobosim/setup/source_pyrobosim.bash 49 | } 50 | 51 | .. note:: 52 | The ``setup_pyrobosim_bash`` script will create a configuration file named ``pyrobosim.env`` in the repo root folder. 53 | This file is then read by the ``source_pyrobosim.bash`` script to start up the environment. 54 | 55 | You can edit this file manually if you're comfortable with how these settings work. 56 | Otherwise, we recommend rerunning ``setup_pyrobosim.bash`` and following the prompts if you want to make changes to your setup. 57 | 58 | 59 | Docker Setup 60 | ------------ 61 | 62 | We also provide Docker images compatible with supported ROS 2 distributions. 63 | 64 | If you already have sourced ROS 2 in your system (e.g., ``source /opt/ros/humble/setup.bash``), 65 | then you should have a ``ROS_DISTRO`` environment variable set. 66 | Otherwise, 67 | 68 | :: 69 | 70 | export ROS_DISTRO=humble 71 | docker compose build 72 | 73 | Then, you can start any of the existing demos. 74 | To see all the available services, refer to the ``docker-compose.yaml`` file or use Tab completion. 75 | 76 | :: 77 | 78 | docker compose run demo 79 | docker compose run demo_ros 80 | 81 | Alternatively, you can start a Docker container without running any examples. 82 | 83 | :: 84 | 85 | docker compose run base 86 | # python3 src/pyrobosim/examples/demo.py 87 | 88 | From a new Terminal, you can then start a new interactive shell in the same container as follows. 89 | 90 | :: 91 | 92 | docker exec -it bash 93 | # ros2 launch pyrobosim_ros demo.py 94 | 95 | The source code on your host machine is mounted as a volume. 96 | This means you can make modifications on your host and rebuild the ROS 2 workspace inside the container. 97 | -------------------------------------------------------------------------------- /docs/source/usage/basic_usage.rst: -------------------------------------------------------------------------------- 1 | Basic Usage 2 | =========== 3 | To get started with PyRoboSim, you can run the following examples. 4 | 5 | 6 | Standalone 7 | ---------- 8 | 9 | First, go to the ``pyrobosim`` subfolder under the repository root. 10 | 11 | :: 12 | 13 | cd /path/to/pyrobosim/pyrobosim 14 | 15 | Then, run the example. 16 | 17 | :: 18 | 19 | python3 examples/demo.py 20 | 21 | You can now interact with the GUI through the buttons and text boxes. 22 | For example, enter "bedroom desk" in the **Goal query** text box and then click the **Navigate** button. 23 | Once at the destination, click **Pick**. 24 | 25 | .. image:: ../media/pyrobosim_demo.png 26 | :align: center 27 | :width: 600px 28 | :alt: Basic standalone demo. 29 | 30 | | 31 | 32 | With ROS 2 33 | ---------- 34 | 35 | First, build and setup your ROS 2 workspace (or use one of our provided Docker containers). 36 | 37 | :: 38 | 39 | cd /path/to/ros_workspace 40 | colcon build 41 | . install/local_setup.bash 42 | 43 | 44 | You can run a ROS 2 enabled demo and interact with the GUI: 45 | 46 | :: 47 | 48 | ros2 run pyrobosim_ros demo.py 49 | 50 | 51 | In a separate Terminal, you can send an action goal with a task plan or a single action: 52 | 53 | :: 54 | 55 | ros2 run pyrobosim_ros demo_commands.py --ros-args -p mode:=action 56 | ros2 run pyrobosim_ros demo_commands.py --ros-args -p mode:=plan 57 | 58 | 59 | Or, you can run both of these nodes together using a provided launch file: 60 | 61 | :: 62 | 63 | ros2 launch pyrobosim_ros demo_commands.launch.py mode:=plan 64 | ros2 launch pyrobosim_ros demo_commands.launch.py mode:=action 65 | 66 | 67 | The first command will start a world as a ROS 2 node, and the second one will execute a plan (or set of actions) to the node. 68 | 69 | .. image:: ../media/pyrobosim_demo_ros.png 70 | :align: center 71 | :width: 600px 72 | :alt: Basic ROS 2 demo. 73 | 74 | | 75 | 76 | Creating Worlds 77 | --------------- 78 | 79 | Worlds can be created either with the Python API, or loaded from a YAML file using the :py:class:`pyrobosim.core.yaml_utils.WorldYamlLoader` utility. 80 | 81 | By default, ``demo.py`` creates a world using the API, but you can alternatively try a demo YAML file using the ``--world-file`` argument. 82 | For example: 83 | 84 | :: 85 | 86 | # Standalone 87 | python3 examples/demo.py --world-file test_world.yaml 88 | 89 | # ROS 2 90 | ros2 launch pyrobosim_ros demo.launch.py world_file:=test_world.yaml 91 | 92 | Refer to the :ref:`yaml_schemas` documentation for more information. 93 | 94 | 95 | Exporting Worlds to Gazebo 96 | -------------------------- 97 | 98 | To export worlds to Gazebo, there is a :py:class:`pyrobosim.core.gazebo.WorldGazeboExporter` utility. 99 | You can try this with the following commands. 100 | 101 | :: 102 | 103 | # Standalone 104 | python3 examples/demo_world_save.py 105 | 106 | # ROS 2 107 | ros2 run pyrobosim_ros demo_world_save.py 108 | 109 | Then, follow the steps displayed on the console to see the generated world. 110 | 111 | .. image:: ../media/gazebo_demo_world.png 112 | :align: center 113 | :width: 600px 114 | :alt: Example world exported to Gazebo. 115 | 116 | If you add the ``--classic`` flag to this demo, you can similarly export to Gazebo Classic. 117 | 118 | :: 119 | 120 | # Standalone 121 | python3 examples/demo_world_save.py --classic 122 | 123 | # ROS 2 124 | ros2 run pyrobosim_ros demo_world_save.py --classic 125 | 126 | .. image:: ../media/gazebo_classic_demo_world.png 127 | :align: center 128 | :width: 600px 129 | :alt: Example world exported to Gazebo Classic. 130 | 131 | | 132 | -------------------------------------------------------------------------------- /docs/source/usage/geometry_conventions.rst: -------------------------------------------------------------------------------- 1 | .. _geometry_conventions: 2 | 3 | Geometry Conventions 4 | ==================== 5 | 6 | 3D orientations can be represented using Euler angles or quaternions. 7 | For simplicity, if you are representing poses in 2D, you can use the yaw angle (Z Euler angle), but we recommend using quaternions for generic 3D pose calculations and for interfacing with ROS. 8 | 9 | The Euler angle convention is extrinsic XYZ (roll = X, pitch = Y, yaw = Z) in radians. 10 | Quaternions use the order convention ``[qw, qx, qy, qz]``. 11 | There is more documentation in the :py:class:`pyrobosim.utils.pose.Pose` source code. 12 | 13 | When ROS 2 is connected to PyRoboSim, the reference frame is ``map``. 14 | -------------------------------------------------------------------------------- /docs/source/usage/index.rst: -------------------------------------------------------------------------------- 1 | Usage 2 | ===== 3 | 4 | PyRoboSim offers a standalone Python API for modeling worlds and simulating robots acting within those worlds. 5 | Optionally, you can also launch this tool with: 6 | 7 | * A ROS 2 interface that offers topics, services, and actions to interact with the world. 8 | * A `PySide6 `_ based GUI for visualizing the world. 9 | 10 | Refer to the following pages for different types of usage guides. 11 | 12 | .. toctree:: 13 | :maxdepth: 1 14 | 15 | basic_usage 16 | robot_actions 17 | robot_dynamics 18 | multirobot 19 | path_planners 20 | sensors 21 | tamp 22 | geometry_conventions 23 | -------------------------------------------------------------------------------- /docs/source/usage/multirobot.rst: -------------------------------------------------------------------------------- 1 | Multirobot Environments 2 | ======================= 3 | 4 | PyRoboSim enables you to run multirobot environments. 5 | 6 | 7 | Standalone 8 | ---------- 9 | 10 | To run a multirobot world, you can try this example: 11 | 12 | :: 13 | 14 | cd /path/to/pyrobosim/pyrobosim 15 | python3 examples/demo.py --multirobot 16 | 17 | Or you can use a sample multirobot world file: 18 | 19 | :: 20 | 21 | cd /path/to/pyrobosim/pyrobosim 22 | python3 examples/demo.py --world-file test_world_multirobot.yaml 23 | 24 | .. image:: ../media/pyrobosim_demo_multirobot.png 25 | :align: center 26 | :width: 720px 27 | :alt: PyRoboSim multirobot demo. 28 | 29 | | 30 | 31 | With ROS 2 32 | ---------- 33 | 34 | First, build and setup your ROS 2 workspace (or use one of our provided Docker containers). 35 | 36 | :: 37 | 38 | cd /path/to/ros_workspace 39 | colcon build 40 | . install/local_setup.bash 41 | 42 | 43 | You can run a ROS 2 enabled multirobot demo and interact with the GUI: 44 | 45 | :: 46 | 47 | ros2 run pyrobosim_ros demo.py --ros-args -p world_file:=test_world_multirobot.yaml 48 | 49 | 50 | In a separate Terminal, you can send an action goal with a multirobot plan: 51 | 52 | :: 53 | 54 | ros2 run pyrobosim_ros demo_commands.py --ros-args -p mode:=multirobot-plan 55 | 56 | Alternatively, you can run a single launch file: 57 | 58 | :: 59 | 60 | ros2 launch pyrobosim_ros demo_commands_multirobot.launch.py 61 | 62 | The output should look as follows: 63 | 64 | .. image:: ../media/pyrobosim_demo_multirobot_plan.png 65 | :align: center 66 | :width: 720px 67 | :alt: PyRoboSim multirobot demo with ROS 2 executing multiple plans. 68 | 69 | | 70 | -------------------------------------------------------------------------------- /docs/source/usage/path_planners.rst: -------------------------------------------------------------------------------- 1 | .. _path_planners: 2 | 3 | Path Planners 4 | ============= 5 | 6 | This section explains how to use (and optionally extend) path planners in PyRoboSim. 7 | 8 | Path Planner Definitions 9 | ------------------------ 10 | 11 | The ``pyrobosim/navigation`` module contains all path planner implementations. 12 | 13 | What to Implement in a Planner 14 | ------------------------------ 15 | 16 | The path planners implemented in PyRoboSim provide general functionality to plan paths in a known world, as well as data for visualization in the UI. 17 | 18 | You should implement a ``reset()`` method. 19 | This is needed if, for example, the world changes and you want to generate new persistent data structures such as a new occupancy grid or roadmap. 20 | Note that the parent class' method can be called using ``super().reset()``, which automatically resets common attributes such as ``latest_path``, ``robot``, and ``world``. 21 | 22 | .. code-block:: python 23 | 24 | from pyrobosim.navigation.types import PathPlanner 25 | from pyrobosim.utils.path import Path 26 | 27 | class MyNewPlanner(PathPlanner): 28 | 29 | plugin_name = "my_planner" # Needed to register the plugin 30 | 31 | def __init__( 32 | self, 33 | *, 34 | grid_resolution: float, 35 | grid_inflation_radius: float, 36 | ) -> None: 37 | super().__init__() 38 | self.reset() 39 | 40 | def reset(self) -> None: 41 | super().reset() 42 | if self.world is not None: 43 | self.grid = OccupancyGrid.from_world( 44 | world, 45 | resolution=grid_resolution, 46 | inflation_radius=grid_inflation_radius, 47 | ) 48 | 49 | Then, you need to implement the actual path planning. 50 | This is done using a ``plan()`` method that accepts a start and goal pose and returns a ``Path`` object. 51 | 52 | .. code-block:: python 53 | 54 | import time 55 | from pyrobosim.utils.pose import Pose 56 | 57 | def plan(self, start: Pose, goal: Pose) -> Path: 58 | t_start = time.time() 59 | 60 | # Your planning logic goes here 61 | 62 | return Path( 63 | poses=[start, goal], 64 | planning_time=time.time() - t_start 65 | ) 66 | 67 | For visualization, you can provide ``get_graphs()`` and ``get_latest_paths()`` methods. 68 | 69 | .. code-block:: python 70 | 71 | from pyrobosim.utils.search_graph.SearchGraph 72 | 73 | def plan(self, start: Pose, goal: Pose) -> Path: 74 | t_start = time.time() 75 | self.search_graph = SearchGraph() 76 | 77 | # Your planning logic goes here 78 | 79 | self.latest_path = Path( 80 | poses=[start, goal], 81 | planning_time=time.time() - t_start 82 | ) 83 | return self.latest_path 84 | 85 | def get_graphs(self) -> list[SearchGraph]: 86 | return [SearchGraph()] 87 | 88 | def get_latest_path(self) -> Path: 89 | return self.latest_path 90 | 91 | To serialize to file, which is needed to reset the world, you should also implement the ``to_dict()`` method. 92 | Note the ``plugin_name`` attribute, which contains the name of the planner you defined earlier on. 93 | 94 | .. code-block:: python 95 | 96 | def to_dict(self) -> dict[str, Any]: 97 | return { 98 | "type": self.plugin_name, 99 | "grid_resolution": self.grid_resolution, 100 | "grid_inflation_radius": self.grid_inflation_radius, 101 | } 102 | 103 | At this point, you can import your own path planner in code and load it dynamically using the ``PathPlanner`` parent class. 104 | 105 | .. code-block:: python 106 | 107 | from pyrobosim.navigation import PathPlanner 108 | from my_module import MyNewPlanner # Still need to import this! 109 | 110 | planner_class = PathPlanner.registered_plugins["my_planner"] 111 | planner = planner_class(grid_resolution=0.01, grid_inflation_radius=0.1) 112 | 113 | ... or from YAML world files. 114 | 115 | .. code-block:: yaml 116 | 117 | robots: 118 | name: robot 119 | path_planner: 120 | type: my_planner 121 | grid_resolution: 0.01 122 | grid_inflation_radius: 0.1 123 | 124 | If you would like to implement your own path planner, it is highly recommended to look at the existing planner implementations as a reference. 125 | You can also always ask the maintainers through a Git issue! 126 | -------------------------------------------------------------------------------- /docs/source/usage/sensors.rst: -------------------------------------------------------------------------------- 1 | .. _sensors: 2 | 3 | Sensors 4 | ======= 5 | 6 | This section explains how to use (and optionally extend) sensor models in PyRoboSim. 7 | 8 | Sensor Definitions 9 | ------------------ 10 | 11 | The ``pyrobosim/sensors`` module contains all sensor model implementations. 12 | 13 | 14 | What to Implement in a Sensor 15 | ------------------------------ 16 | 17 | The sensors implemented in PyRoboSim provide general functionality to simulate a sensor, as well as data for visualization in the UI. 18 | 19 | First, you want subclass from the ``Sensor`` class and create a constructor as follows. 20 | 21 | .. code-block:: python 22 | 23 | from pyrobosim.sensors.types import Sensor 24 | 25 | class MyNewSensor(Sensor): 26 | 27 | plugin_name = "my_sensor" # Needed to register the plugin 28 | 29 | def __init__( 30 | self, 31 | *, 32 | update_rate_s: float, 33 | initial_value: float, 34 | ) -> None: 35 | super().__init__() 36 | self.update_rate_s = update_rate_s 37 | self.latest_value = initial_value 38 | 39 | 40 | Next, you should implement ``update()`` and ``get_measurement()`` functions. 41 | These update the internals of the sensor and return the latest measurement, respectively. 42 | 43 | .. code-block:: python 44 | 45 | def update(self) -> None: 46 | # Increments the sensor value by 1. 47 | self.latest_value += 1.0 48 | 49 | def get_measurement(self) -> float: 50 | return self.latest_value 51 | 52 | 53 | If you want to run the sensor automatically in the background, you can also implement a ``thread_function()`` function. 54 | This will only take effect if your robot is created with the ``start_sensor_threads`` argument set to ``True``. 55 | 56 | .. code-block:: python 57 | 58 | def thread_function(self) -> None: 59 | if self.robot is None: # This is created in the constructor! 60 | return 61 | 62 | # The `is_active` attribute should be used to cleanly 63 | # stop this thread when the sensor is shut down. 64 | while self.is_active: 65 | t_start = time.time() 66 | self.update() 67 | t_end = time.time() 68 | time.sleep(max(0.0, self.update_rate_s - (t_end - t_start))) 69 | 70 | 71 | For visualization, you can provide ``setup_artists()`` and ``update_artists()`` methods. 72 | 73 | .. code-block:: python 74 | 75 | from matplotlib.artist import Artist 76 | from matplotlib.patches import Circle 77 | from matplotlib.transforms import Affine2D 78 | 79 | def setup_artists(self) -> list[Artist]: 80 | """Executes when the sensor is first visualized.""" 81 | pose = self.robot.get_pose() 82 | self.circle = Circle( 83 | (pose.x, pose.y), 84 | radius=1.0, 85 | color="r", 86 | ) 87 | return [self.circle] 88 | 89 | def update_artists(self) -> None: 90 | """Updates the artist as needed.""" 91 | pose = self.robot.get_pose() 92 | new_tform = Affine2D().translate(pose.x, pose.y) 93 | self.circle.set_transform(new_tform) 94 | 95 | 96 | To serialize to file, which is needed to reset the world, you should also implement the ``to_dict()`` method. 97 | Note the ``plugin_name`` attribute, which contains the name of the sensor you defined earlier on. 98 | 99 | .. code-block:: python 100 | 101 | def to_dict(self) -> dict[str, Any]: 102 | return { 103 | "type": self.plugin_name, 104 | "update_rate_s": self.update_rate_s, 105 | "initial_value": self.initial_value, 106 | } 107 | 108 | At this point, you can import your own sensor in code and load it dynamically using the ``Sensor`` parent class. 109 | 110 | .. code-block:: python 111 | 112 | from pyrobosim.sensors import Sensor 113 | from my_module import MyNewSensor # Still need to import this! 114 | 115 | sensor_class = Sensor.registered_plugins["my_sensor"] 116 | sensor = sensor_class(update_rate_s=0.1, initial_value=42) 117 | 118 | ... or from YAML world files. 119 | 120 | .. code-block:: yaml 121 | 122 | robots: 123 | name: robot 124 | sensors: 125 | my_cool_sensor: 126 | type: my_sensor 127 | update_rate_s: 0.1 128 | initial_value: 42 129 | 130 | If you would like to implement your own sensor, it is highly recommended to look at the existing sensor implementations as a reference. 131 | You can also always ask the maintainers through a Git issue! 132 | -------------------------------------------------------------------------------- /docs/source/usage/tamp.rst: -------------------------------------------------------------------------------- 1 | .. _task_and_motion_planning: 2 | 3 | Task and Motion Planning 4 | ======================== 5 | 6 | We use `PDDLStream `_ to perform integrated task and motion planning (TAMP). 7 | This tool expands task planning with purely discrete parameters using `Planning Domain Definition Language (PDDL) `_ 8 | by adding the concept of *streams* for sampling continuous parameters in actions. 9 | 10 | If you did not already install PDDLStream, ensure you do so with this script, then re-source. 11 | 12 | :: 13 | 14 | ./setup/configure_pddlstream.bash 15 | source ./setup/source_pyrobosim.bash 16 | 17 | 18 | Examples 19 | -------- 20 | 21 | Regardless of running standalone or using ROS 2, we have included a set of examples 22 | that gradually build up from simple, purely discrete planning, to a more complex integrated TAMP demo with continuous action parameters. 23 | 24 | The current example list is: 25 | 26 | * ``01_simple`` - Simple domain with purely discrete actions. 27 | * ``02_derived`` - Purely discrete actions, but uses *derived predicates* for more complex goals. 28 | * ``03_nav_stream`` - Samples navigation poses and motion plan instances. 29 | * ``04_nav_manip_stream`` - Samples navigation poses, motion plans, and collision-free object placement instances. 30 | * ``05_nav_grasp_stream`` - Samples navigation poses, motion plans, grasp plans, and collision-free object placement instances. 31 | * ``06_open_close_detect`` - Extends the ``02_derived`` domain with additional actions to detect objects and open and close locations. Does not contain any streams. 32 | 33 | These PDDL domain and stream description files can be found in the ``pyrobosim/pyrobosim/data/pddlstream/domains`` folder. 34 | 35 | 36 | Standalone 37 | ---------- 38 | 39 | You can try running a sample script as follows 40 | 41 | :: 42 | 43 | cd /path/to/pyrobosim/pyrobosim 44 | python3 examples/demo_pddl.py --example 01_simple --verbose 45 | 46 | .. image:: ../media/pddlstream_demo_standalone.png 47 | :align: center 48 | :width: 720px 49 | :alt: PDDLStream standalone demo. 50 | 51 | | 52 | 53 | With ROS 2 54 | ---------- 55 | 56 | First, build and setup your ROS 2 workspace (or use one of our provided Docker containers). 57 | 58 | :: 59 | 60 | cd /path/to/ros_workspace 61 | colcon build 62 | . install/local_setup.bash 63 | 64 | 65 | With ROS 2, the idea is to separate out functionality into different *nodes*. 66 | 67 | To start a world and then a planner with a hard-coded goal specification: 68 | 69 | :: 70 | 71 | ros2 run pyrobosim_ros demo_pddl_world.py 72 | ros2 run pyrobosim_ros demo_pddl_planner.py --ros-args -p example:=01_simple -p subscribe:=false 73 | 74 | To start a world, a planner, and a separate node that publishes a goal specification: 75 | 76 | :: 77 | 78 | ros2 run pyrobosim_ros demo_pddl_world.py 79 | ros2 run pyrobosim_ros demo_pddl_planner.py --ros-args -p example:=01_simple -p subscribe:=true 80 | ros2 run pyrobosim_ros demo_pddl_goal_publisher.py --ros-args -p example:=01_simple 81 | 82 | Alternatively, you can use a single launch file to run the full example and configure it: 83 | 84 | :: 85 | 86 | ros2 launch pyrobosim_ros demo_pddl.launch.py example:=01_simple 87 | ros2 launch pyrobosim_ros demo_pddl.launch.py example:=04_nav_manip_stream subscribe:=true verbose:=true 88 | 89 | The output should look as follows: 90 | 91 | .. image:: ../media/pddlstream_demo_ros.png 92 | :align: center 93 | :width: 720px 94 | :alt: PDDLStream demo with ROS 2. 95 | 96 | | 97 | -------------------------------------------------------------------------------- /docs/source/yaml/index.rst: -------------------------------------------------------------------------------- 1 | .. _yaml_schemas: 2 | 3 | YAML Schemas 4 | ============ 5 | 6 | PyRoboSim worlds can be created using the Python API, but can also loaded from YAML files. 7 | 8 | Specifically, each world draws from a set of **location** and **object** metadata files. 9 | 10 | Worlds themselves can be created programmatically, or defined using world YAML files. 11 | 12 | For the programmatic approach, you can create a world as follows either using ``set_metadata`` or ``add_metadata``. 13 | 14 | .. code-block:: python 15 | 16 | from pyrobosim.core import Robot, World 17 | 18 | world = World() 19 | 20 | # locations and objects can accept either a single file path or a list of file paths. 21 | world.add_metadata( 22 | locations=[ 23 | "/path/to/location_data1.yaml", 24 | "/path/to/location_data2.yaml" 25 | ], 26 | objects=[ 27 | "/path/to/object_data1.yaml", 28 | "/path/to/object_data2.yaml" 29 | ] 30 | ) 31 | 32 | # Then, you can add the other entities 33 | world.add_robot(...) 34 | world.add_room(...) 35 | world.add_hallway(...) 36 | world.add_location(...) 37 | world.add_object(...) 38 | 39 | 40 | For the YAML approach, you can directly point to the location and object metadata in the world YAML file itself. 41 | 42 | .. code-block:: yaml 43 | 44 | metadata: 45 | locations: /path/to/location_data.yaml 46 | objects: /path/to/object_data.yaml 47 | 48 | Then, the world can be loaded from file as follows. 49 | 50 | .. code-block:: python 51 | 52 | from pyrobosim.core import WorldYamlLoader 53 | 54 | world = WorldYamlLoader().from_file("/path/to/world_file.yaml") 55 | 56 | Refer to the following sections for more details on the schemas. 57 | 58 | .. toctree:: 59 | :maxdepth: 1 60 | 61 | world_schema 62 | location_schema 63 | object_schema 64 | -------------------------------------------------------------------------------- /docs/source/yaml/object_schema.rst: -------------------------------------------------------------------------------- 1 | Object Schema 2 | ============= 3 | 4 | Objects in a world come from a library of categories defined in a YAML file. 5 | 6 | The generic object schema, where ```` are placeholders, is: 7 | 8 | .. code-block:: yaml 9 | 10 | : 11 | footprint: 12 | type: 13 | : 14 | color: [, , ] or <"color_name"> or <"hexadecimalcode"> 15 | 16 | Examples 17 | -------- 18 | 19 | A simple object with a circular footprint. 20 | 21 | .. code-block:: yaml 22 | 23 | apple: # The category name is "apple" 24 | footprint: 25 | type: circle # Circular footprint 26 | radius: 0.06 # 6 cm radius 27 | color: [1, 0, 0] # Red 28 | 29 | A simple object with a box footprint. 30 | 31 | .. code-block:: yaml 32 | 33 | banana: # Category name is "banana" 34 | footprint: 35 | type: box # Box footprint 36 | dims: [0.05, 0.2] # 5 cm by 20 cm 37 | offset: [0.0, 0.1] # 10 cm Y offset from origin 38 | color: "yellow" # Yellow 39 | 40 | An object with a generic polygon footprint. 41 | 42 | .. code-block:: yaml 43 | 44 | water: # Category name is "water" 45 | footprint: 46 | type: polygon # Generic polygon footprint 47 | coords: # List of X-Y coordinates 48 | - [0.035, -0.075] 49 | - [0.035, 0.075] 50 | - [0.0, 0.1] 51 | - [-0.035, 0.075] 52 | - [-0.035, -0.075] 53 | offset: [0.02, 0.0] # 2 cm X offset from origin 54 | height: 0.25 # 25 cm extruded height (Z dimension) 55 | color: [0.0, 0.1, 0.9] # Blue 56 | 57 | An object with a footprint read from a mesh file. 58 | Note that the literal ``$DATA`` resolves to the ``pyrobosim/data`` folder, but you can specify an absolute path as well or create your own tokens. 59 | 60 | .. code-block:: yaml 61 | 62 | coke: # Category name is "coke" 63 | footprint: 64 | type: mesh # Geometry from mesh 65 | model_path: $DATA/sample_models/coke_can 66 | mesh_path: meshes/coke_can.dae 67 | color: "#CC0000" # Red color (for viewing in pyrobosim) 68 | -------------------------------------------------------------------------------- /mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | strict = True 3 | disable_error_code = import-untyped 4 | disallow_untyped_calls = False 5 | explicit_package_bases = True 6 | implicit_reexport = True 7 | warn_unused_ignores = False 8 | -------------------------------------------------------------------------------- /pyrobosim/.gitignore: -------------------------------------------------------------------------------- 1 | # Build artifacts 2 | build/ 3 | dist/ 4 | 5 | # Autogenerated worlds 6 | data/worlds/ 7 | -------------------------------------------------------------------------------- /pyrobosim/README.md: -------------------------------------------------------------------------------- 1 | ROS 2 enabled 2D mobile robot simulator for behavior prototyping. 2 | 3 | Refer to the [GitHub repo](https://github.com/sea-bass/pyrobosim) or [full documentation](https://pyrobosim.readthedocs.io/) for more information. 4 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_astar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | 5 | from pyrobosim.core import WorldYamlLoader 6 | from pyrobosim.gui import start_gui 7 | from pyrobosim.navigation.a_star import AStarPlanner 8 | from pyrobosim.utils.general import get_data_folder 9 | from pyrobosim.utils.pose import Pose 10 | 11 | # Load a test world. 12 | world_file = os.path.join(get_data_folder(), "test_world.yaml") 13 | world = WorldYamlLoader().from_file(world_file) 14 | 15 | 16 | def demo_astar() -> None: 17 | """Creates an occupancy grid based A* planner and plans a path.""" 18 | robot = world.robots[0] 19 | planner_config = { 20 | "grid_resolution": 0.05, 21 | "grid_inflation_radius": 1.5 * robot.radius, 22 | "diagonal_motion": True, 23 | "heuristic": "euclidean", 24 | "compress_path": False, 25 | } 26 | 27 | planner = AStarPlanner(**planner_config) 28 | start = Pose(x=-0.5, y=-0.5) 29 | goal = Pose(x=3.0, y=3.0) 30 | robot.set_pose(start) 31 | robot.set_path_planner(planner) 32 | path = robot.plan_path(start, goal) 33 | if path: 34 | path.print_details() 35 | 36 | 37 | if __name__ == "__main__": 38 | demo_astar() 39 | start_gui(world) 40 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_dynamics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Test script showing how to command robot velocities and simulate dynamics. 5 | """ 6 | import os 7 | import numpy as np 8 | import time 9 | from threading import Thread 10 | 11 | from pyrobosim.core import Robot, World 12 | from pyrobosim.gui import start_gui 13 | from pyrobosim.utils.general import get_data_folder 14 | from pyrobosim.utils.pose import Pose 15 | 16 | 17 | data_folder = get_data_folder() 18 | 19 | 20 | def create_world() -> World: 21 | """Create a test world""" 22 | world = World() 23 | 24 | # Set the location and object metadata 25 | world.set_metadata( 26 | locations=os.path.join(data_folder, "example_location_data.yaml"), 27 | objects=os.path.join(data_folder, "example_object_data.yaml"), 28 | ) 29 | 30 | # Add rooms 31 | r1coords = [(-1, -1), (1.5, -1), (1.5, 1.5), (0.5, 1.5)] 32 | world.add_room( 33 | name="kitchen", 34 | footprint=r1coords, 35 | color=[1, 0, 0], 36 | nav_poses=[Pose(x=0.75, y=0.75, z=0.0, yaw=0.0)], 37 | ) 38 | r2coords = [(1.75, 2.5), (3.5, 2.5), (3.5, 4), (1.75, 4)] 39 | world.add_room(name="bedroom", footprint=r2coords, color=[0, 0.6, 0]) 40 | r3coords = [(-1, 1), (-1, 3.5), (-3.0, 3.5), (-2.5, 1)] 41 | world.add_room(name="bathroom", footprint=r3coords, color=[0, 0, 0.6]) 42 | 43 | # Add hallways between the rooms 44 | world.add_hallway(room_start="kitchen", room_end="bathroom", width=0.7) 45 | world.add_hallway( 46 | room_start="bathroom", 47 | room_end="bedroom", 48 | width=0.5, 49 | conn_method="angle", 50 | conn_angle=0, 51 | offset=0.8, 52 | ) 53 | world.add_hallway( 54 | room_start="kitchen", 55 | room_end="bedroom", 56 | width=0.6, 57 | conn_method="points", 58 | conn_points=[(1.0, 0.5), (2.5, 0.5), (2.5, 3.0)], 59 | ) 60 | 61 | # Add locations 62 | table = world.add_location( 63 | category="table", 64 | parent="kitchen", 65 | pose=Pose(x=0.85, y=-0.5, z=0.0, yaw=-np.pi / 2.0), 66 | ) 67 | desk = world.add_location( 68 | category="desk", parent="bedroom", pose=Pose(x=3.15, y=3.65, z=0.0, yaw=0.0) 69 | ) 70 | counter = world.add_location( 71 | category="counter", 72 | parent="bathroom", 73 | pose=Pose(x=-2.45, y=2.5, z=0.0, q=[0.634411, 0.0, 0.0, 0.7729959]), 74 | ) 75 | 76 | # Add robots 77 | robot0 = Robot( 78 | name="robot0", 79 | radius=0.1, 80 | max_linear_acceleration=1.0, 81 | max_angular_acceleration=1.0, 82 | ) 83 | world.add_robot(robot0, loc="kitchen") 84 | 85 | robot1 = Robot( 86 | name="robot1", radius=0.08, color=(0.8, 0.8, 0), max_angular_acceleration=0.05 87 | ) 88 | world.add_robot(robot1, loc="bathroom") 89 | 90 | robot2 = Robot( 91 | name="robot2", radius=0.06, color=(0, 0.8, 0.8), max_linear_acceleration=5.0 92 | ) 93 | world.add_robot(robot2, loc="bedroom") 94 | 95 | return world 96 | 97 | 98 | def command_robots(world: World) -> None: 99 | """Demonstrates robot dynamics by commanding robots.""" 100 | dt = 0.1 101 | vel_commands = [ 102 | np.array([0.1, 0.0, 0.5]), # robot0 103 | np.array([0.0, 0.0, -1.0]), # robot1 104 | np.array([0.2, 0.0, 0.0]), # robot2 105 | ] 106 | backup_vel = np.array([-1.0, 0.0, 0.0]) 107 | 108 | while True: 109 | t_start = time.time() 110 | for robot, cmd_vel in zip(world.robots, vel_commands): 111 | if robot.is_in_collision(): 112 | cmd_vel = backup_vel 113 | 114 | new_pose = robot.dynamics.step(cmd_vel, dt) 115 | robot.set_pose(new_pose) 116 | 117 | t_elapsed = time.time() - t_start 118 | time.sleep(max(0, dt - t_elapsed)) 119 | 120 | 121 | if __name__ == "__main__": 122 | world = create_world() 123 | 124 | # Command robots on a separate thread. 125 | robot_commands_thread = Thread(target=lambda: command_robots(world)) 126 | robot_commands_thread.start() 127 | 128 | # Start the program either as ROS node or standalone. 129 | start_gui(world) 130 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_grasping.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from pyrobosim.core import Object 4 | from pyrobosim.manipulation import GraspGenerator, ParallelGraspProperties 5 | from pyrobosim.utils.pose import Pose 6 | 7 | 8 | def create_test_object(object_pose: Pose) -> Object: 9 | """Helper function to create a test object.""" 10 | obj_footprint_coords = [ 11 | [0.05, 0.0], 12 | [0.025, 0.025], 13 | [-0.05, 0.025], 14 | [-0.025, 0.0], 15 | [-0.05, -0.025], 16 | [0.025, -0.025], 17 | [0.05, 0.0], 18 | ] 19 | obj_data = {} 20 | obj_data["object"] = { 21 | "footprint": { 22 | "type": "polygon", 23 | "coords": obj_footprint_coords, 24 | "height": 0.25, 25 | }, 26 | } 27 | Object.metadata = obj_data 28 | 29 | return Object(name="test_object", category="object", pose=object_pose) 30 | 31 | 32 | if __name__ == "__main__": 33 | # Define the inputs 34 | robot_pose = Pose(x=0.0, y=0.0, z=0.0) 35 | object_pose = Pose(x=0.5, y=0.1, z=0.2) 36 | obj = create_test_object(object_pose) 37 | cuboid_pose = obj.get_grasp_cuboid_pose() 38 | 39 | # Create a grasp generator 40 | properties = ParallelGraspProperties( 41 | max_width=0.15, 42 | depth=0.1, 43 | height=0.04, 44 | width_clearance=0.01, 45 | depth_clearance=0.01, 46 | ) 47 | gen = GraspGenerator(properties) 48 | 49 | # Generate and display grasps 50 | grasps = gen.generate( 51 | obj.cuboid_dims, 52 | cuboid_pose, 53 | robot_pose, 54 | front_grasps=True, 55 | top_grasps=True, 56 | side_grasps=False, 57 | ) 58 | print(grasps) 59 | gen.show_grasps( 60 | obj.cuboid_dims, grasps, cuboid_pose, robot_pose, obj.get_footprint() 61 | ) 62 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_pddl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Test script showing how to perform task and motion planning with PDDLStream. 5 | """ 6 | 7 | import os 8 | import argparse 9 | import threading 10 | import time 11 | 12 | from pyrobosim.core import World, WorldYamlLoader 13 | from pyrobosim.gui import start_gui 14 | from pyrobosim.planning.pddlstream import PDDLStreamPlanner, get_default_domains_folder 15 | from pyrobosim.utils.general import get_data_folder 16 | 17 | 18 | def parse_args() -> argparse.Namespace: 19 | """Parse command-line arguments""" 20 | parser = argparse.ArgumentParser(description="PDDLStream planning demo.") 21 | parser.add_argument( 22 | "--example", 23 | default="01_simple", 24 | help="Example name (01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream, 06_open_close_detect)", 25 | ) 26 | parser.add_argument("--verbose", action="store_true", help="Print planning output") 27 | parser.add_argument( 28 | "--search-sample-ratio", 29 | type=float, 30 | default=0.5, 31 | help="Search to sample ratio for planner", 32 | ) 33 | return parser.parse_args() 34 | 35 | 36 | def load_world() -> World: 37 | """Load a test world.""" 38 | world_file = os.path.join(get_data_folder(), "pddlstream_simple_world.yaml") 39 | return WorldYamlLoader().from_file(world_file) 40 | 41 | 42 | def start_planner(world: World, args: argparse.Namespace) -> None: 43 | """Test PDDLStream planner under various scenarios.""" 44 | domain_folder = os.path.join(get_default_domains_folder(), args.example) 45 | planner = PDDLStreamPlanner(world, domain_folder) 46 | 47 | # Wait for the GUI to load 48 | while world.gui is None: 49 | time.sleep(1.0) 50 | time.sleep(0.5) # Extra time for log messages to not interfere with prompt 51 | 52 | if args.example == "01_simple": 53 | # Task specification for simple example. 54 | goal_literals = [ 55 | ("At", "robot", "bedroom"), 56 | ("At", "apple0", "table0_tabletop"), 57 | ("At", "banana0", "counter0_left"), 58 | ("Holding", "robot", "water0"), 59 | ] 60 | elif args.example in [ 61 | "02_derived", 62 | "03_nav_stream", 63 | "04_nav_manip_stream", 64 | "05_nav_grasp_stream", 65 | "06_open_close_detect", 66 | ]: 67 | # Task specification for derived predicate example. 68 | goal_literals = [ 69 | ("Has", "desk0_desktop", "banana0"), 70 | ("Has", "counter", "apple1"), 71 | ("HasNone", "bathroom", "banana"), 72 | ("HasAll", "table", "water"), 73 | ] 74 | # If using the open/close/detect example, close the desk location. 75 | if args.example == "06_open_close_detect": 76 | world.close_location(world.get_location_by_name("desk0")) 77 | else: 78 | print(f"Invalid example: {args.example}") 79 | return 80 | 81 | input("Press Enter to start planning.") 82 | robot = world.robots[0] 83 | plan = planner.plan( 84 | robot, 85 | goal_literals, 86 | verbose=args.verbose, 87 | max_attempts=3, 88 | search_sample_ratio=args.search_sample_ratio, 89 | planner="ff-astar", 90 | max_planner_time=10.0, 91 | max_time=60.0, 92 | ) 93 | robot.execute_plan(plan) 94 | 95 | 96 | if __name__ == "__main__": 97 | args = parse_args() 98 | world = load_world() 99 | 100 | # Start task and motion planner in separate thread. 101 | planner_thread = threading.Thread(target=start_planner, args=(world, args)) 102 | planner_thread.start() 103 | 104 | # Start GUI in main thread. 105 | start_gui(world) 106 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_prm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | 4 | from pyrobosim.core import WorldYamlLoader 5 | from pyrobosim.gui import start_gui 6 | from pyrobosim.navigation.prm import PRMPlanner 7 | from pyrobosim.utils.general import get_data_folder 8 | from pyrobosim.utils.pose import Pose 9 | 10 | # Load a test world. 11 | world_file = os.path.join(get_data_folder(), "test_world.yaml") 12 | world = WorldYamlLoader().from_file(world_file) 13 | 14 | 15 | def test_prm() -> None: 16 | """Creates a PRM planner and plans""" 17 | planner_config = { 18 | "max_nodes": 100, 19 | "collision_check_step_dist": 0.025, 20 | "max_connection_dist": 1.5, 21 | "compress_path": False, 22 | } 23 | prm = PRMPlanner(**planner_config) 24 | start = Pose(x=-0.5, y=-0.5) 25 | goal = Pose(x=3.0, y=3.0) 26 | 27 | robot = world.robots[0] 28 | robot.set_pose(start) 29 | robot.set_path_planner(prm) 30 | path = robot.plan_path(start, goal) 31 | if path: 32 | path.print_details() 33 | 34 | 35 | if __name__ == "__main__": 36 | test_prm() 37 | start_gui(world) 38 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_rrt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | 4 | from pyrobosim.core import WorldYamlLoader 5 | from pyrobosim.gui import start_gui 6 | from pyrobosim.navigation.rrt import RRTPlanner 7 | from pyrobosim.utils.general import get_data_folder 8 | from pyrobosim.utils.pose import Pose 9 | 10 | # Load a test world. 11 | world_file = os.path.join(get_data_folder(), "test_world.yaml") 12 | world = WorldYamlLoader().from_file(world_file) 13 | 14 | 15 | def test_rrt() -> None: 16 | """Creates an RRT planner and plans""" 17 | planner_config = { 18 | "bidirectional": True, 19 | "rrt_connect": True, 20 | "rrt_star": True, 21 | "compress_path": False, 22 | } 23 | rrt = RRTPlanner(**planner_config) 24 | start = Pose(x=-0.5, y=-0.5) 25 | goal = Pose(x=3.0, y=3.0) 26 | 27 | robot = world.robots[0] 28 | robot.set_pose(start) 29 | robot.set_path_planner(rrt) 30 | path = robot.plan_path(start, goal) 31 | if path: 32 | path.print_details() 33 | 34 | 35 | if __name__ == "__main__": 36 | test_rrt() 37 | start_gui(world) 38 | -------------------------------------------------------------------------------- /pyrobosim/examples/demo_world_save.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Tests Gazebo world saving and occupancy grid export capabilities. 5 | """ 6 | 7 | import argparse 8 | import os 9 | 10 | from pyrobosim.core import WorldGazeboExporter, WorldYamlLoader 11 | from pyrobosim.navigation.occupancy_grid import OccupancyGrid 12 | from pyrobosim.utils.general import get_data_folder 13 | 14 | 15 | def parse_args() -> argparse.Namespace: 16 | parser = argparse.ArgumentParser( 17 | description="Test Gazebo world saving and occupancy grid export." 18 | ) 19 | parser.add_argument( 20 | "--world-file", 21 | default="test_world.yaml", 22 | help="YAML file name (should be in the pyrobosim/data folder). " 23 | + "Defaults to test_world.yaml", 24 | ) 25 | parser.add_argument( 26 | "--out-folder", default=None, help="Output folder for exporting the world" 27 | ) 28 | parser.add_argument( 29 | "--classic", action="store_true", help="Enable to export to Gazebo Classic" 30 | ) 31 | parser.add_argument("--save-grid", action="store_true", help="Save occupancy grid") 32 | parser.add_argument( 33 | "--grid-resolution", 34 | type=float, 35 | default=0.05, 36 | help="Occupancy grid resolution (meters)", 37 | ) 38 | parser.add_argument( 39 | "--grid-inflation-radius", 40 | type=float, 41 | default=0.05, 42 | help="Occupancy grid inflation radius (meters)", 43 | ) 44 | parser.add_argument( 45 | "--show-grid", action="store_true", help="Show the occupancy grid" 46 | ) 47 | return parser.parse_args() 48 | 49 | 50 | def main() -> None: 51 | args = parse_args() 52 | 53 | # Load a test world from YAML file. 54 | data_folder = get_data_folder() 55 | loader = WorldYamlLoader() 56 | world = loader.from_file(os.path.join(data_folder, args.world_file)) 57 | 58 | # Export a Gazebo world. 59 | exp = WorldGazeboExporter(world) 60 | world_folder = exp.export(classic=args.classic, out_folder=args.out_folder) 61 | 62 | # Save an occupancy grid to the world folder and show it, if enabled. 63 | if args.save_grid: 64 | occ_grid = OccupancyGrid.from_world( 65 | world, 66 | resolution=args.grid_resolution, 67 | inflation_radius=args.grid_inflation_radius, 68 | ) 69 | occ_grid.save_to_file(world_folder) 70 | if args.show_grid: 71 | occ_grid.show() 72 | 73 | 74 | if __name__ == "__main__": 75 | main() 76 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/__init__.py: -------------------------------------------------------------------------------- 1 | """ROS 2 enabled 2D mobile robot simulator for behavior prototyping.""" 2 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/core/__init__.py: -------------------------------------------------------------------------------- 1 | """Core PyRoboSim module. 2 | 3 | This module contains all the tools for world representation 4 | (e.g. robots, rooms, locations, objects). 5 | 6 | Additionally, tools for interfacing with ROS 2, importing from 7 | YAML files, and exporting Gazebo worlds and occupancy grids reside here. 8 | """ 9 | 10 | from .dynamics import * 11 | from .gazebo import * 12 | from .hallway import * 13 | from .locations import * 14 | from .objects import * 15 | from .robot import * 16 | from .room import * 17 | from .types import * 18 | from .world import World 19 | from .yaml_utils import * 20 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/core/dynamics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Robot dynamics utilities. 3 | """ 4 | 5 | import copy 6 | import numpy as np 7 | from typing import Sequence 8 | 9 | from ..utils.pose import Pose 10 | 11 | 12 | class RobotDynamics2D: 13 | """Simple 2D dynamics for robots.""" 14 | 15 | def __init__( 16 | self, 17 | init_pose: Pose = Pose(), 18 | init_vel: np.ndarray = np.array([0.0, 0.0, 0.0]), 19 | max_linear_velocity: float = np.inf, 20 | max_angular_velocity: float = np.inf, 21 | max_linear_acceleration: float = np.inf, 22 | max_angular_acceleration: float = np.inf, 23 | ) -> None: 24 | """ 25 | Creates an instance of a 2D robot dynamics object. 26 | 27 | :param init_pose: The initial pose of the robot. 28 | :param init_vel: The initial velocity of the robot, as a [vx, vy, vtheta] array. 29 | :param max_linear_velocity: The maximum linear velocity magnitude, in m/s. 30 | :param max_angular_velocity: The maximum angular velocity magnitude, in rad/s. 31 | :param max_linear_acceleration: The maximum linear acceleration magnitude, in m/s^2. 32 | :param max_angular_acceleration: The maximum angular acceleration magnitude, in rad/s^2. 33 | """ 34 | # Initial state 35 | self.pose = init_pose 36 | self.velocity = np.array(init_vel) 37 | 38 | # Velocity and acceleration limits 39 | self.vel_limits = np.array( 40 | [max_linear_velocity, max_linear_velocity, max_angular_velocity] 41 | ) 42 | self.accel_limits = np.array( 43 | [max_linear_acceleration, max_linear_acceleration, max_angular_acceleration] 44 | ) 45 | 46 | def step(self, cmd_vel: Sequence[float], dt: float) -> Pose: 47 | """ 48 | Perform a single dynamics step. 49 | 50 | :param cmd_vel: Velocity command array, in the form [vx, vy, vtheta]. 51 | :param dt: Time step, in seconds. 52 | :return: The new pose after applying the dynamics. 53 | """ 54 | # Trivial case of zero or None command velocities. 55 | if (np.count_nonzero(cmd_vel) == 0) or (cmd_vel is None): 56 | return self.pose 57 | 58 | self.velocity = self.enforce_dynamics_limits(cmd_vel, dt) 59 | 60 | # Dynamics 61 | roll, pitch, yaw = self.pose.eul 62 | sin_yaw = np.sin(yaw) 63 | cos_yaw = np.cos(yaw) 64 | 65 | vx = self.velocity[0] * cos_yaw - self.velocity[1] * sin_yaw 66 | vy = self.velocity[0] * sin_yaw + self.velocity[1] * cos_yaw 67 | 68 | target_pose = copy.copy(self.pose) 69 | target_pose.x += vx * dt 70 | target_pose.y += vy * dt 71 | target_pose.set_euler_angles(roll, pitch, yaw + self.velocity[2] * dt) 72 | return target_pose 73 | 74 | def enforce_dynamics_limits( 75 | self, cmd_vel: Sequence[float], dt: float 76 | ) -> np.ndarray: 77 | """ 78 | Enforces velocity and acceleration limits by saturating a velocity command. 79 | 80 | :param cmd_vel: Velocity command array, in the form [vx, vy, vtheta]. 81 | :param dt: Time step, in seconds. 82 | :return: The saturated velocity command array. 83 | """ 84 | # First saturate to velocity limits 85 | cmd_vel = np.clip(cmd_vel, -self.vel_limits, self.vel_limits) 86 | 87 | # Then saturate to acceleration limits 88 | cmd_vel = np.clip( 89 | cmd_vel, 90 | self.velocity - self.accel_limits * dt, 91 | self.velocity + self.accel_limits * dt, 92 | ) 93 | 94 | return cmd_vel 95 | 96 | def reset( 97 | self, pose: Pose | None = None, velocity: np.ndarray = np.array([0.0, 0.0, 0.0]) 98 | ) -> None: 99 | """ 100 | Reset all the dynamics of the robot to provided values. 101 | 102 | :param pose: The pose to reset to. If None, will keep the current pose. 103 | :param velocity: The velocity to reset to. Defaults to zero velocities. 104 | """ 105 | if pose is not None: 106 | self.pose = pose 107 | self.velocity = velocity 108 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/.gitignore: -------------------------------------------------------------------------------- 1 | worlds/ 2 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_location_data.yaml: -------------------------------------------------------------------------------- 1 | ############################# 2 | # Example location metadata # 3 | ############################# 4 | 5 | table: 6 | footprint: 7 | type: box 8 | dims: [0.9, 1.2] 9 | height: 0.5 10 | nav_poses: 11 | - position: # left 12 | x: -0.75 13 | y: 0.0 14 | - position: # right 15 | x: 0.75 16 | y: 0.0 17 | rotation_eul: 18 | yaw: 3.14 19 | locations: 20 | - name: "tabletop" 21 | footprint: 22 | type: parent 23 | padding: 0.1 24 | color: [0.2, 0, 0] 25 | 26 | desk: 27 | footprint: 28 | type: polygon 29 | coords: 30 | - [-0.3, -0.3] 31 | - [0.3, -0.3] 32 | - [0.3, 0.3] 33 | - [-0.3, 0.3] 34 | height: 0.3 35 | locations: 36 | - name: "desktop" 37 | footprint: 38 | type: parent 39 | nav_poses: 40 | - position: # below 41 | x: 0.0 42 | y: -0.5 43 | rotation_eul: 44 | yaw: 1.57 45 | - position: # left 46 | x: -0.5 47 | y: 0.0 48 | - position: # above 49 | x: 0.0 50 | y: 0.5 51 | rotation_eul: 52 | yaw: -1.57 53 | - position: # right 54 | x: 0.5 55 | y: 0.0 56 | rotation_eul: 57 | yaw: 3.14 58 | 59 | counter: 60 | footprint: 61 | type: box 62 | dims: [1.2, 0.6] 63 | height: 0.75 64 | locations: 65 | - name: "left" 66 | footprint: 67 | type: polygon 68 | coords: 69 | - [-0.25, -0.25] 70 | - [0.25, -0.25] 71 | - [0.25, 0.25] 72 | - [-0.25, 0.25] 73 | offset: [0.3, 0] 74 | nav_poses: 75 | - position: # below 76 | x: 0.0 77 | y: -0.5 78 | rotation_eul: 79 | yaw: 1.57 80 | - position: # above 81 | x: 0.0 82 | y: 0.5 83 | rotation_eul: 84 | yaw: -1.57 85 | - name: "right" 86 | footprint: 87 | type: polygon 88 | coords: 89 | - [-0.25, -0.25] 90 | - [0.25, -0.25] 91 | - [0.25, 0.25] 92 | - [-0.25, 0.25] 93 | offset: [-0.3, 0] 94 | nav_poses: 95 | - position: # below 96 | x: 0.0 97 | y: -0.5 98 | rotation_eul: 99 | yaw: 1.57 100 | - position: # above 101 | x: 0.0 102 | y: 0.5 103 | rotation_eul: 104 | yaw: -1.57 105 | color: [0, 0.2, 0] 106 | 107 | trash_can: 108 | footprint: 109 | type: mesh 110 | model_path: $DATA/sample_models/first_2015_trash_can 111 | mesh_path: meshes/trash_can.dae 112 | locations: 113 | - name: "top" 114 | footprint: 115 | type: parent 116 | padding: 0.05 117 | nav_poses: 118 | - position: # left 119 | x: -0.5 120 | y: 0.0 121 | - position: # right 122 | x: 0.5 123 | y: 0.0 124 | rotation_eul: 125 | yaw: 3.14 126 | color: [0, 0.35, 0.2] 127 | 128 | charger: 129 | footprint: 130 | type: polygon 131 | coords: 132 | - [-0.3, -0.15] 133 | - [0.3, -0.15] 134 | - [0.3, 0.15] 135 | - [-0.3, 0.15] 136 | height: 0.1 137 | locations: 138 | - name: "dock" 139 | footprint: 140 | type: parent 141 | nav_poses: 142 | - position: # below 143 | x: 0.0 144 | y: -0.5 145 | rotation_eul: 146 | yaw: 1.57 147 | - position: # left 148 | x: -0.35 149 | y: 0.0 150 | - position: # above 151 | x: 0.0 152 | y: 0.5 153 | rotation_eul: 154 | yaw: -1.57 155 | - position: # right 156 | x: 0.35 157 | y: 0.0 158 | rotation_eul: 159 | yaw: 3.14 160 | color: [0.4, 0.4, 0] 161 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_location_data_accessories.yaml: -------------------------------------------------------------------------------- 1 | trash_can: 2 | footprint: 3 | type: mesh 4 | model_path: $DATA/sample_models/first_2015_trash_can 5 | mesh_path: meshes/trash_can.dae 6 | locations: 7 | - name: "top" 8 | footprint: 9 | type: parent 10 | padding: 0.05 11 | nav_poses: 12 | - position: # left 13 | x: -0.5 14 | y: 0.0 15 | - position: # right 16 | x: 0.5 17 | y: 0.0 18 | rotation_eul: 19 | yaw: 3.14 20 | color: [0, 0.35, 0.2] 21 | 22 | charger: 23 | footprint: 24 | type: polygon 25 | coords: 26 | - [-0.3, -0.15] 27 | - [0.3, -0.15] 28 | - [0.3, 0.15] 29 | - [-0.3, 0.15] 30 | height: 0.1 31 | locations: 32 | - name: "dock" 33 | footprint: 34 | type: parent 35 | nav_poses: 36 | - position: # below 37 | x: 0.0 38 | y: -0.35 39 | rotation_eul: 40 | yaw: 1.57 41 | - position: # left 42 | x: -0.5 43 | y: 0.0 44 | - position: # above 45 | x: 0.0 46 | y: 0.35 47 | rotation_eul: 48 | yaw: -1.57 49 | - position: # right 50 | x: 0.5 51 | y: 0.0 52 | rotation_eul: 53 | yaw: 3.14 54 | color: [0.4, 0.4, 0] 55 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_location_data_furniture.yaml: -------------------------------------------------------------------------------- 1 | table: 2 | footprint: 3 | type: box 4 | dims: [0.9, 1.2] 5 | height: 0.5 6 | nav_poses: 7 | - position: # left 8 | x: -0.75 9 | y: 0.0 10 | - position: # right 11 | x: 0.75 12 | y: 0.0 13 | rotation_eul: 14 | yaw: 3.14 15 | locations: 16 | - name: "tabletop" 17 | footprint: 18 | type: parent 19 | padding: 0.1 20 | color: [0.2, 0, 0] 21 | 22 | desk: 23 | footprint: 24 | type: polygon 25 | coords: 26 | - [-0.3, -0.3] 27 | - [0.3, -0.3] 28 | - [0.3, 0.3] 29 | - [-0.3, 0.3] 30 | height: 0.3 31 | locations: 32 | - name: "desktop" 33 | footprint: 34 | type: parent 35 | nav_poses: 36 | - position: # below 37 | x: 0.0 38 | y: -0.5 39 | rotation_eul: 40 | yaw: 1.57 41 | - position: # left 42 | x: -0.5 43 | y: 0.0 44 | - position: # above 45 | x: 0.0 46 | y: 0.5 47 | rotation_eul: 48 | yaw: -1.57 49 | - position: # right 50 | x: 0.5 51 | y: 0.0 52 | rotation_eul: 53 | yaw: 3.14 54 | 55 | counter: 56 | footprint: 57 | type: box 58 | dims: [1.2, 0.6] 59 | height: 0.75 60 | locations: 61 | - name: "left" 62 | footprint: 63 | type: polygon 64 | coords: 65 | - [-0.25, -0.25] 66 | - [0.25, -0.25] 67 | - [0.25, 0.25] 68 | - [-0.25, 0.25] 69 | offset: [0.3, 0] 70 | nav_poses: 71 | - position: # below 72 | x: 0.0 73 | y: -0.5 74 | rotation_eul: 75 | yaw: 1.57 76 | - position: # above 77 | x: 0.0 78 | y: 0.5 79 | rotation_eul: 80 | yaw: -1.57 81 | - name: "right" 82 | footprint: 83 | type: polygon 84 | coords: 85 | - [-0.25, -0.25] 86 | - [0.25, -0.25] 87 | - [0.25, 0.25] 88 | - [-0.25, 0.25] 89 | offset: [-0.3, 0] 90 | nav_poses: 91 | - position: # below 92 | x: 0.0 93 | y: -0.5 94 | rotation_eul: 95 | yaw: 1.57 96 | - position: # above 97 | x: 0.0 98 | y: 0.5 99 | rotation_eul: 100 | yaw: -1.57 101 | color: [0, 0.2, 0] 102 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_object_data.yaml: -------------------------------------------------------------------------------- 1 | ########################### 2 | # Example object metadata # 3 | ########################### 4 | 5 | apple: 6 | footprint: 7 | type: circle 8 | radius: 0.06 9 | color: [1, 0, 0] 10 | 11 | banana: 12 | footprint: 13 | type: box 14 | dims: [0.05, 0.2] 15 | color: [0.7, 0.7, 0] 16 | 17 | water: 18 | footprint: 19 | type: polygon 20 | coords: 21 | - [0.035, -0.075] 22 | - [0.035, 0.075] 23 | - [0.0, 0.1] 24 | - [-0.035, 0.075] 25 | - [-0.035, -0.075] 26 | color: [0.0, 0.1, 0.7] 27 | 28 | coke: 29 | footprint: 30 | type: mesh 31 | model_path: $DATA/sample_models/coke_can 32 | mesh_path: meshes/coke_can.dae 33 | color: [0.8, 0, 0] 34 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_object_data_drink.yaml: -------------------------------------------------------------------------------- 1 | water: 2 | footprint: 3 | type: polygon 4 | coords: 5 | - [0.035, -0.075] 6 | - [0.035, 0.075] 7 | - [0.0, 0.1] 8 | - [-0.035, 0.075] 9 | - [-0.035, -0.075] 10 | color: [0.0, 0.1, 0.7] 11 | 12 | coke: 13 | footprint: 14 | type: mesh 15 | model_path: $DATA/sample_models/coke_can 16 | mesh_path: meshes/coke_can.dae 17 | color: [0.8, 0, 0] 18 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/example_object_data_food.yaml: -------------------------------------------------------------------------------- 1 | apple: 2 | footprint: 3 | type: circle 4 | radius: 0.06 5 | color: [1, 0, 0] 6 | 7 | banana: 8 | footprint: 9 | type: box 10 | dims: [0.05, 0.2] 11 | color: [0.7, 0.7, 0] 12 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/domain.pddl: -------------------------------------------------------------------------------- 1 | ; PDDL PLANNING DOMAIN (SIMPLE) 2 | ; 3 | ; This planning domain contains `navigate`, `pick`, and `place` actions. 4 | ; 5 | ; All actions are symbolic, meaning there are no different types of grasps 6 | ; or feasibility checks, under the assumption that a downstream planner exists. 7 | ; 8 | ; Accompanying streams are defined in the `streams.pddl` file. 9 | 10 | 11 | (define (domain domain_simple) 12 | (:requirements :strips :equality) 13 | (:predicates 14 | ; Static predicates 15 | (Robot ?r) ; Represents the robot 16 | (Obj ?o) ; Object representation 17 | (Room ?r) ; Room representation 18 | (Hallway ?h) ; Hallway representation 19 | (Location ?l) ; Location representation 20 | 21 | ; Fluent predicates 22 | (HandEmpty ?r) ; Whether the robot's gripper is empty 23 | (CanMove ?r) ; Whether the robot can move (prevents duplicate moves) 24 | (Holding ?r ?o) ; Object the robot is holding 25 | (At ?o ?l) ; Robot/Object's location, or location's Room 26 | ) 27 | 28 | ; FUNCTIONS : See their descriptions in the stream PDDL file 29 | (:functions 30 | (Dist ?l1 ?l2) 31 | (PickPlaceCost ?l ?o) 32 | ) 33 | 34 | ; ACTIONS 35 | ; NAVIGATE: Moves the robot from one location to the other 36 | (:action navigate 37 | :parameters (?r ?l1 ?l2) 38 | :precondition (and (Robot ?r) 39 | (CanMove ?r) 40 | (Location ?l1) 41 | (Location ?l2) 42 | (At ?r ?l1)) 43 | :effect (and (not (CanMove ?r)) 44 | (At ?r ?l2) (not (At ?r ?l1)) 45 | (increase (total-cost) (Dist ?l1 ?l2))) 46 | ) 47 | 48 | ; PICK: Picks up an object from a specified location 49 | (:action pick 50 | :parameters (?r ?o ?l) 51 | :precondition (and (Robot ?r) 52 | (Obj ?o) 53 | (Location ?l) 54 | (not (Room ?l)) 55 | (not (Hallway ?l)) 56 | (HandEmpty ?r) 57 | (At ?r ?l) 58 | (At ?o ?l)) 59 | :effect (and (Holding ?r ?o) (CanMove ?r) 60 | (not (HandEmpty ?r)) 61 | (not (At ?o ?l)) 62 | (increase (total-cost) (PickPlaceCost ?l ?o))) 63 | ) 64 | 65 | ; PLACE: Places an object in a specified location 66 | (:action place 67 | :parameters (?r ?o ?l) 68 | :precondition (and (Robot ?r) 69 | (Obj ?o) 70 | (Location ?l) 71 | (not (Room ?l)) 72 | (not (Hallway ?l)) 73 | (At ?r ?l) 74 | (not (HandEmpty ?r)) 75 | (Holding ?r ?o)) 76 | :effect (and (HandEmpty ?r) (CanMove ?r) 77 | (At ?o ?l) 78 | (not (Holding ?r ?o)) 79 | (increase (total-cost) (PickPlaceCost ?l ?o))) 80 | ) 81 | 82 | ) 83 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/01_simple/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (SIMPLE) 2 | ; 3 | ; Contains simple cost functions for actions. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_simple) 8 | 9 | ; DIST: Distance between two locations. 10 | (:function (Dist ?l1 ?l2) 11 | (and (Location ?l1) (Location ?l2)) 12 | ) 13 | 14 | ; PICKPLACECOST: Cost to perform pick and place at a location. 15 | (:function (PickPlaceCost ?l ?o) 16 | (and (Location ?l) (Obj ?o)) 17 | ) 18 | 19 | ) 20 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/02_derived/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (DERIVED PREDICATES) 2 | ; 3 | ; Contains simple cost functions for actions. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_derived) 8 | 9 | ; DIST: Distance between two locations. 10 | (:function (Dist ?l1 ?l2) 11 | (and (Location ?l1) (Location ?l2)) 12 | ) 13 | 14 | ; PICKPLACECOST: Cost to perform pick and place at a location. 15 | (:function (PickPlaceCost ?l ?o) 16 | (and (Location ?l) (Obj ?o)) 17 | ) 18 | 19 | ) 20 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/03_nav_stream/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (NAVIGATION STREAMS) 2 | ; 3 | ; Contains cost functions for actions and streams for navigation. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_nav_stream) 8 | 9 | ; PATHLENGTH: Length of a path. 10 | (:function (PathLength ?pth) 11 | (and (Path ?pth)) 12 | ) 13 | 14 | ; PICKPLACECOST: Cost to perform pick and place at a location. 15 | (:function (PickPlaceCost ?l ?o) 16 | (and (Location ?l) (Obj ?o)) 17 | ) 18 | 19 | ; S-NAVPOSE: Samples a pose from a finite set of navigation poses for that location. 20 | (:stream s-navpose 21 | :inputs (?l) 22 | :domain (Location ?l) 23 | :outputs (?p) 24 | :certified (and (Pose ?p) (NavPose ?l ?p)) 25 | ) 26 | 27 | ; S-MOTION: Samples a valid path from one pose to another 28 | (:stream s-motion 29 | :inputs (?p1 ?p2) 30 | :domain (and (Pose ?p1) (Pose ?p2)) 31 | :outputs (?pth) 32 | :certified (and (Path ?pth) (Motion ?p1 ?p2 ?pth))) 33 | 34 | ) 35 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/04_nav_manip_stream/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (NAVIGATION + MANIPULATION STREAMS) 2 | ; 3 | ; Contains cost functions for actions and streams for navigation and manipulation. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_nav_stream) 8 | 9 | ; PATHLENGTH: Length of a path. 10 | (:function (PathLength ?pth) 11 | (and (Path ?pth)) 12 | ) 13 | 14 | ; PICKPLACEATPOSECOST: Cost to perform pick and place at a location. 15 | ; The object is at Pose p and robot at Pose pr. 16 | (:function (PickPlaceAtPoseCost ?l ?o ?p ?pr) 17 | (and (Location ?l) (Obj ?o) (Pose ?p) (Pose ?pr)) 18 | ) 19 | 20 | ; S-NAVPOSE: Samples a pose from a finite set of navigation poses for that location. 21 | (:stream s-navpose 22 | :inputs (?l) 23 | :domain (Location ?l) 24 | :outputs (?p) 25 | :certified (and (Pose ?p) (NavPose ?l ?p)) 26 | ) 27 | 28 | ; S-MOTION: Samples a valid path from one pose to another 29 | (:stream s-motion 30 | :inputs (?p1 ?p2) 31 | :domain (and (Pose ?p1) (Pose ?p2)) 32 | :outputs (?pth) 33 | :certified (and (Path ?pth) (Motion ?p1 ?p2 ?pth))) 34 | 35 | ; S-PLACE: Samples an object placement pose 36 | (:stream s-place 37 | :inputs (?l ?o) 38 | :domain (and (Location ?l) (Obj ?o)) 39 | :outputs (?p) 40 | :certified (and (Pose ?p) (Placeable ?l ?o ?p)) 41 | ) 42 | 43 | ; T-COLLISION-FREE: Check if a placement pose is collision free 44 | (:stream t-collision-free 45 | :inputs (?o1 ?p1 ?o2 ?p2) 46 | :domain (and (Obj ?o1) (Pose ?p1) 47 | (Obj ?o2) (Pose ?p2)) 48 | :certified (CollisionFree ?o1 ?p1 ?o2 ?p2) 49 | ) 50 | 51 | ) 52 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/05_nav_grasp_stream/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (NAVIGATION + MANIPULATION + GRASP STREAMS) 2 | ; 3 | ; Contains cost functions for actions and streams for navigation and manipulation. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_nav_grasp) 8 | 9 | ; PATHLENGTH: Length of a path. 10 | (:function (PathLength ?pth) 11 | (and (Path ?pth)) 12 | ) 13 | 14 | ; GRASPATPOSECOST: Cost to perform a grasp on an object. 15 | ; The robot is at Pose pr, and the grasp is defined by Grasp g. 16 | (:function (GraspAtPoseCost ?g ?pr) 17 | (and (Grasp ?g) (Pose ?pr)) 18 | ) 19 | 20 | ; PICKPLACEATPOSECOST: Cost to perform pick and place at a location. 21 | ; The object is at Pose p and robot at Pose pr. 22 | (:function (PickPlaceAtPoseCost ?l ?o ?p ?pr) 23 | (and (Location ?l) (Obj ?o) (Pose ?p) (Pose ?pr)) 24 | ) 25 | 26 | ; S-NAVPOSE: Samples a pose from a finite set of navigation poses for that location. 27 | (:stream s-navpose 28 | :inputs (?l) 29 | :domain (Location ?l) 30 | :outputs (?p) 31 | :certified (and (Pose ?p) (NavPose ?l ?p)) 32 | ) 33 | 34 | ; S-MOTION: Samples a valid path from one pose to another 35 | (:stream s-motion 36 | :inputs (?p1 ?p2) 37 | :domain (and (Pose ?p1) (Pose ?p2)) 38 | :outputs (?pth) 39 | :certified (and (Path ?pth) (Motion ?p1 ?p2 ?pth))) 40 | 41 | ; S-GRASP: Samples an object grasp pose 42 | (:stream s-grasp 43 | :inputs (?o ?po ?pr) 44 | :domain (and (Obj ?o) (Pose ?po) (Pose ?pr)) 45 | :outputs (?g) 46 | :certified (and (Grasp ?g) (Graspable ?o ?po ?pr ?g)) 47 | ) 48 | 49 | ; S-PLACE: Samples an object placement pose 50 | (:stream s-place 51 | :inputs (?l ?o) 52 | :domain (and (Location ?l) (Obj ?o)) 53 | :outputs (?p) 54 | :certified (and (Pose ?p) (Placeable ?l ?o ?p)) 55 | ) 56 | 57 | ; T-COLLISION-FREE: Check if a placement pose is collision free 58 | (:stream t-collision-free 59 | :inputs (?o1 ?p1 ?o2 ?p2) 60 | :domain (and (Obj ?o1) (Pose ?p1) 61 | (Obj ?o2) (Pose ?p2)) 62 | :certified (CollisionFree ?o1 ?p1 ?o2 ?p2) 63 | ) 64 | 65 | ) 66 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream/domains/06_open_close_detect/streams.pddl: -------------------------------------------------------------------------------- 1 | ; STREAMS FOR PDDL PLANNING DOMAIN (OPEN, CLOSE, AND DETECT) 2 | ; 3 | ; Contains simple cost functions for actions. 4 | ; 5 | ; Accompanying planning domain defined in the `domain.pddl` file. 6 | 7 | (define (stream stream_open_close_detect) 8 | 9 | ; DIST: Distance between two locations. 10 | (:function (Dist ?l1 ?l2) 11 | (and (Location ?l1) (Location ?l2)) 12 | ) 13 | 14 | ; PICKPLACECOST: Cost to perform pick and place at a location. 15 | (:function (PickPlaceCost ?l ?o) 16 | (and (Location ?l) (Obj ?o)) 17 | ) 18 | 19 | ; DETECTCOST: Cost to detect objects at a location. 20 | (:function (DetectCost ?l) 21 | (and (Location ?l)) 22 | ) 23 | 24 | ; OPENCLOSECOST: Cost to open or close a location. 25 | (:function (OpenCloseCost ?l) 26 | (and (Location ?l)) 27 | ) 28 | 29 | ) 30 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/pddlstream_simple_world.yaml: -------------------------------------------------------------------------------- 1 | #################################### 2 | # PDDLStream Example: Simple world # 3 | #################################### 4 | 5 | # WORLD PARAMETERS 6 | params: 7 | name: pddlstream_simple_world 8 | object_radius: 0.0375 # Radius around objects 9 | wall_height: 2.0 # Wall height for exporting to Gazebo 10 | 11 | 12 | # METADATA: Describes information about locations and objects 13 | metadata: 14 | locations: 15 | - $DATA/example_location_data_furniture.yaml 16 | - $DATA/example_location_data_accessories.yaml 17 | objects: 18 | - $DATA/example_object_data_food.yaml 19 | - $DATA/example_object_data_drink.yaml 20 | 21 | 22 | # ROBOTS 23 | robots: 24 | - name: robot 25 | radius: 0.1 26 | location: kitchen 27 | pose: 28 | position: 29 | x: 0.0 30 | y: 0.0 31 | path_planner: 32 | type: rrt 33 | collision_check_step_dist: 0.025 34 | max_connection_dist: 1.0 35 | bidirectional: false 36 | rrt_star: true 37 | rewire_radius: 1.5 38 | compress_path: false 39 | path_executor: 40 | type: constant_velocity 41 | linear_velocity: 1.0 42 | max_angular_velocity: 4.0 43 | dt: 0.1 44 | validate_during_execution: true 45 | validation_dt: 0.5 46 | validation_step_dist: 0.025 47 | grasping: 48 | generator: parallel_grasp 49 | max_width: 0.15 50 | depth: 0.1 51 | height: 0.04 52 | width_clearance: 0.01 53 | depth_clearance: 0.01 54 | 55 | 56 | # ROOMS: Polygonal regions that can contain object locations 57 | rooms: 58 | - name: kitchen 59 | footprint: 60 | type: polygon 61 | coords: 62 | - [-1, -1] 63 | - [1.5, -1] 64 | - [1.5, 1.5] 65 | - [0.5, 1.5] 66 | nav_poses: 67 | - position: 68 | x: 0.75 69 | y: 0.5 70 | wall_width: 0.2 71 | color: [1, 0, 0] 72 | 73 | - name: bedroom 74 | footprint: 75 | type: box 76 | dims: [1.75, 1.5] 77 | pose: 78 | position: 79 | x: 2.625 80 | y: 3.25 81 | wall_width: 0.2 82 | color: [0, 0.6, 0] 83 | 84 | - name: bathroom 85 | footprint: 86 | type: polygon 87 | coords: 88 | - [-1, 1] 89 | - [-1, 3.5] 90 | - [-3, 3.5] 91 | - [-2.5, 1] 92 | wall_width: 0.2 93 | color: [0, 0, 0.6] 94 | 95 | 96 | # HALLWAYS: Connect rooms 97 | hallways: 98 | - room_start: kitchen 99 | room_end: bathroom 100 | width: 0.7 101 | conn_method: auto 102 | is_open: True 103 | is_locked: False 104 | 105 | - room_start: bathroom 106 | room_end: bedroom 107 | width: 0.5 108 | conn_method: angle 109 | conn_angle: 0.0 110 | offset: 0.8 111 | is_open: True 112 | is_locked: False 113 | 114 | - room_start: kitchen 115 | room_end: bedroom 116 | width: 0.6 117 | conn_method: points 118 | conn_points: 119 | - [1.0, 0.5] 120 | - [2.5, 0.5] 121 | - [2.5, 3.0] 122 | is_open: True 123 | is_locked: False 124 | 125 | 126 | # LOCATIONS: Can contain objects 127 | locations: 128 | - category: table 129 | parent: kitchen 130 | pose: 131 | position: 132 | x: 0.85 133 | y: -0.5 134 | rotation_eul: 135 | yaw: -90.0 136 | angle_units: "degrees" 137 | is_open: True 138 | is_locked: True 139 | 140 | - category: desk 141 | parent: bedroom 142 | pose: 143 | position: 144 | x: 0.525 145 | y: 0.4 146 | relative_to: bedroom 147 | is_open: True 148 | is_locked: False 149 | 150 | - category: counter 151 | parent: bathroom 152 | pose: 153 | position: 154 | x: -2.45 155 | y: 2.5 156 | rotation_eul: 157 | yaw: 101.2 158 | angle_units: "degrees" 159 | is_open: True 160 | is_locked: True 161 | 162 | 163 | # OBJECTS: Can be picked, placed, and moved by robot 164 | objects: 165 | - category: banana 166 | parent: table0 167 | pose: 168 | position: 169 | x: 0.15 170 | y: 0.0 171 | rotation_eul: 172 | yaw: -49.5 173 | angle_units: "degrees" 174 | relative_to: table0 175 | 176 | - category: apple 177 | parent: desk0 178 | pose: 179 | position: 180 | x: 0.05 181 | y: -0.15 182 | relative_to: desk0 183 | 184 | - category: apple 185 | parent: table0 186 | 187 | - category: apple 188 | parent: table0 189 | 190 | - category: water 191 | parent: counter0 192 | 193 | - category: banana 194 | parent: counter0 195 | 196 | - category: water 197 | parent: desk0 198 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/roscon_2024_location_data.yaml: -------------------------------------------------------------------------------- 1 | # Location metadata for ROSCon 2024 deliberation workshop 2 | # https://github.com/ros-wg-delib/roscon24-workshop 3 | 4 | table: 5 | footprint: 6 | type: box 7 | dims: [1.2, 0.8] 8 | height: 0.5 9 | nav_poses: 10 | - position: # above 11 | x: 0.0 12 | y: 0.65 13 | rotation_eul: 14 | yaw: -1.57 15 | - position: # below 16 | x: 0.0 17 | y: -0.65 18 | rotation_eul: 19 | yaw: 1.57 20 | - position: # left 21 | x: -0.85 22 | y: 0.0 23 | - position: # right 24 | x: 0.85 25 | y: 0.0 26 | rotation_eul: 27 | yaw: 3.14 28 | locations: 29 | - name: "tabletop" 30 | footprint: 31 | type: parent 32 | padding: 0.05 33 | color: [0.2, 0.2, 0.2] 34 | 35 | desk: 36 | footprint: 37 | type: box 38 | dims: [0.6, 1.2] 39 | height: 0.5 40 | nav_poses: 41 | - position: # left 42 | x: -0.5 43 | y: 0.0 44 | - position: # right 45 | x: 0.5 46 | y: 0.0 47 | rotation_eul: 48 | yaw: 3.14 49 | locations: 50 | - name: "desktop" 51 | footprint: 52 | type: parent 53 | padding: 0.05 54 | color: [0.5, 0.2, 0.2] 55 | 56 | storage: 57 | footprint: 58 | type: box 59 | dims: [1.0, 0.6] 60 | height: 0.5 61 | nav_poses: 62 | - position: # above 63 | x: 0.0 64 | y: 0.55 65 | rotation_eul: 66 | yaw: -1.57 67 | - position: # below 68 | x: 0.0 69 | y: -0.55 70 | rotation_eul: 71 | yaw: 1.57 72 | locations: 73 | - name: "storage" 74 | footprint: 75 | type: parent 76 | padding: 0.05 77 | color: [0.2, 0.5, 0.2] 78 | 79 | trashcan_small: 80 | footprint: 81 | type: circle 82 | radius: 0.25 83 | height: 0.5 84 | nav_poses: 85 | - position: # above 86 | x: 0.0 87 | y: 0.5 88 | rotation_eul: 89 | yaw: -1.57 90 | - position: # below 91 | x: 0.0 92 | y: -0.5 93 | rotation_eul: 94 | yaw: 1.57 95 | - position: # left 96 | x: -0.5 97 | y: 0.0 98 | - position: # right 99 | x: 0.5 100 | y: 0.0 101 | rotation_eul: 102 | yaw: 3.14 103 | locations: 104 | - name: "disposal" 105 | footprint: 106 | type: parent 107 | padding: 0.05 108 | color: [0.1, 0.1, 0.1] 109 | 110 | trashcan_large: 111 | footprint: 112 | type: circle 113 | radius: 0.525 114 | height: 0.5 115 | nav_poses: 116 | - position: # above 117 | x: 0.0 118 | y: 0.75 119 | rotation_eul: 120 | yaw: -1.57 121 | - position: # below 122 | x: 0.0 123 | y: -0.75 124 | rotation_eul: 125 | yaw: 1.57 126 | - position: # left 127 | x: -0.75 128 | y: 0.0 129 | - position: # right 130 | x: 0.75 131 | y: 0.0 132 | rotation_eul: 133 | yaw: 3.14 134 | locations: 135 | - name: "disposal" 136 | footprint: 137 | type: parent 138 | padding: 0.05 139 | color: [0.1, 0.1, 0.1] 140 | 141 | charger: 142 | footprint: 143 | type: polygon 144 | coords: 145 | - [-0.5, -0.2] 146 | - [0.5, -0.2] 147 | - [0.5, 0.2] 148 | - [-0.5, 0.2] 149 | height: 0.1 150 | locations: 151 | - name: "dock" 152 | footprint: 153 | type: parent 154 | nav_poses: 155 | - position: # above 156 | x: 0.0 157 | y: 0.4 158 | rotation_eul: 159 | yaw: -1.57 160 | - position: # below 161 | x: 0.0 162 | y: -0.4 163 | rotation_eul: 164 | yaw: 1.57 165 | - position: # left 166 | x: -0.7 167 | y: 0.0 168 | - position: # right 169 | x: 0.7 170 | y: 0.0 171 | rotation_eul: 172 | yaw: 3.14 173 | color: [0.4, 0.4, 0] 174 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/roscon_2024_object_data.yaml: -------------------------------------------------------------------------------- 1 | # Object metadata for ROSCon 2024 deliberation workshop 2 | # https://github.com/ros-wg-delib/roscon24-workshop 3 | 4 | bread: 5 | footprint: 6 | type: box 7 | dims: [0.15, 0.2] 8 | color: [0.7, 0.5, 0] 9 | 10 | snacks: 11 | footprint: 12 | type: box 13 | dims: [0.12, 0.15] 14 | color: [0.0, 0.0, 0.6] 15 | 16 | soda: 17 | footprint: 18 | type: box 19 | dims: [0.12, 0.15] 20 | color: [0.8, 0.0, 0.0] 21 | 22 | butter: 23 | footprint: 24 | type: box 25 | dims: [0.1, 0.15] 26 | color: [0.6, 0.6, 0.0] 27 | 28 | waste: 29 | footprint: 30 | type: circle 31 | radius: 0.075 32 | color: [0.3, 0.5, 0.1] 33 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/materials/textures/coke_can.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sea-bass/pyrobosim/73872c44802c406fbf1a9865d12fe1a675bcb27e/pyrobosim/pyrobosim/data/sample_models/coke_can/materials/textures/coke_can.png -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.390 7 | 8 | 0.00055575 9 | 0 10 | 0 11 | 0.00055575 12 | 0 13 | 0.0001755 14 | 15 | 16 | 17 | 0.003937 0.0047244 -0.18 0 0 0 18 | 19 | 20 | model://coke_can/meshes/coke_can.dae 21 | 22 | 23 | 24 | 25 | 0.003937 0.0047244 -0.18 0 0 0 26 | 27 | 28 | model://coke_can/meshes/coke_can.dae 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.390 7 | 8 | 0.00055575 9 | 0 10 | 0 11 | 0.00055575 12 | 0 13 | 0.0001755 14 | 15 | 16 | 17 | 0.003937 0.0047244 -0.18 0 0 0 18 | 19 | 20 | model://coke_can/meshes/coke_can.dae 21 | 22 | 23 | 24 | 25 | 0.003937 0.0047244 -0.18 0 0 0 26 | 27 | 28 | model://coke_can/meshes/coke_can.dae 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.06 0 0 0 7 | 0.390 8 | 9 | 0.00055575 10 | 0 11 | 0 12 | 0.00055575 13 | 0 14 | 0.0001755 15 | 16 | 17 | 18 | 0.003937 0.0047244 -0.18 0 0 0 19 | 20 | 21 | model://coke_can/meshes/coke_can.dae 22 | 23 | 24 | 25 | 26 | 27 | 1.0 28 | 1.0 29 | 30 | 31 | 32 | 33 | 10000000.0 34 | 1.0 35 | 0.001 36 | 0.1 37 | 38 | 39 | 40 | 41 | 42 | 0.003937 0.0047244 -0.18 0 0 0 43 | 44 | 45 | model://coke_can/meshes/coke_can.dae 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Coke Can 5 | 1.0 6 | model-1_2.sdf 7 | model-1_3.sdf 8 | model-1_4.sdf 9 | model.sdf 10 | 11 | 12 | John Hsu 13 | hsu@osrfoundation.org 14 | 15 | 16 | 17 | A can of Coke. 18 | 19 | 20 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/coke_can/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.06 0 0 0 7 | 0.390 8 | 9 | 0.00055575 10 | 0 11 | 0 12 | 0.00055575 13 | 0 14 | 0.0001755 15 | 16 | 17 | 18 | 0.003937 0.0047244 -0.18 0 0 0 19 | 20 | 21 | model://coke_can/meshes/coke_can.dae 22 | 23 | 24 | 25 | 26 | 27 | 1.0 28 | 1.0 29 | 30 | 31 | 32 | 33 | 10000000.0 34 | 1.0 35 | 0.001 36 | 0.1 37 | 38 | 39 | 40 | 41 | 42 | 0.003937 0.0047244 -0.18 0 0 0 43 | 44 | 45 | model://coke_can/meshes/coke_can.dae 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/first_2015_trash_can/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | FIRST 2015 trash can 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A trash can. 15 | 16 | 17 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/sample_models/first_2015_trash_can/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.3683 0 0 0 7 | 4.83076 8 | 9 | 0.281534052 10 | 0 11 | 0 12 | 0.281534052 13 | 0 14 | 0.126222831 15 | 16 | 17 | 18 | 19 | 20 | 21 | model://first_2015_trash_can/meshes/trash_can.dae 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://first_2015_trash_can/meshes/trash_can.dae 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/templates/link_template_polyline.sdf: -------------------------------------------------------------------------------- 1 | 2 | 0 0 0 0 0 0 3 | 4 | 5 | 6 | 1 7 | 0 8 | 0 9 | 1 10 | 0 11 | 1 12 | 13 | 1 14 | 15 | 16 | 17 | 18 | $POINTS 19 | $HEIGHT 20 | 21 | 22 | 23 | 24 | 25 | 26 | $POINTS 27 | $HEIGHT 28 | 29 | 30 | 31 | $COLOR 1 32 | $COLOR 1 33 | $COLOR 1 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/templates/model_template.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | $NAME 4 | 1.0 5 | model.sdf 6 | 7 | "Autogenerated" 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/templates/model_template.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | $POSE 6 | 7 | 8 | 9 | $LINKS 10 | 11 | $STATIC 12 | 13 | 14 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/templates/world_template_gazebo.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 1 1 1 7 | 0.8 0.8 0.8 1 8 | true 9 | 10 | 0 0 -9.8 11 | 6e-06 2.3e-05 -4.2e-05 12 | 13 | 14 | 0.001 15 | 1 16 | 1000 17 | 18 | 19 | 0 0 10 0 -0 0 20 | true 21 | 1 22 | -0.5 0.1 -0.9 23 | 0.8 0.8 0.8 1 24 | 0.2 0.2 0.2 1 25 | 26 | 1000 27 | 0.01 28 | 0.90000000000000002 29 | 0.001 30 | 31 | 32 | 0 33 | 0 34 | 0 35 | 36 | 37 | 38 | 39 | 40 | true 41 | 42 | 43 | 44 | 45 | 0 0 1 46 | 100 100 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 0 0 1 61 | 100 100 62 | 63 | 64 | 65 | 0.8 0.8 0.8 1 66 | 0.8 0.8 0.8 1 67 | 0.8 0.8 0.8 1 68 | 69 | 70 | 0 0 0 0 -0 0 71 | 72 | 0 0 0 0 -0 0 73 | 1 74 | 75 | 1 76 | 0 77 | 0 78 | 1 79 | 0 80 | 1 81 | 82 | 83 | false 84 | 85 | 0 0 0 0 -0 0 86 | false 87 | 88 | 89 | 90 | $MODEL_INCLUDES 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/data/templates/world_template_gazebo_classic.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 1 1 1 7 | 0.8 0.8 0.8 1 8 | 1 9 | 10 | 11 | 1 12 | 0 0 10 0 -0 0 13 | 0.8 0.8 0.8 1 14 | 0.8 0.8 0.8 1 15 | 16 | 1000 17 | 0.9 18 | 0.01 19 | 0.001 20 | 21 | -0.5 0.1 -0.9 22 | 23 | 0 24 | 0 25 | 0 26 | 27 | 28 | 0 0 -9.80665 29 | 6e-06 2.3e-05 -4.2e-05 30 | 31 | 32 | 0.001 33 | 1 34 | 1000 35 | 36 | 37 | 38 | 39 | 1 40 | 41 | 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 0 0 1 60 | 100 100 61 | 62 | 63 | 64 | 0.8 0.8 0.8 1 65 | 0.8 0.8 0.8 1 66 | 0.8 0.8 0.8 1 67 | 68 | 69 | 70 | 0 0 0 0 -0 0 71 | 72 | 73 | 74 | $MODEL_INCLUDES 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/gui/__init__.py: -------------------------------------------------------------------------------- 1 | """GUI utilities. 2 | 3 | This module contains the main PyRoboSim UI, which is based on PySide6, 4 | as well as the tools to embed the world model as a matplotlib canvas. 5 | 6 | The GUI allows users to view the state of the robot(s) in the world 7 | and interact with the world model using actions such as navigating, 8 | picking, and placing objects. 9 | """ 10 | 11 | from .main import * 12 | from .world_canvas import * 13 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/manipulation/__init__.py: -------------------------------------------------------------------------------- 1 | """Manipulation utilities. 2 | 3 | This module contains tools associated with manipulation, including 4 | motion planning and grasping. 5 | """ 6 | 7 | from .grasping import Grasp, GraspGenerator, ParallelGraspProperties 8 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/navigation/__init__.py: -------------------------------------------------------------------------------- 1 | """Navigation utilities. 2 | 3 | This module contains tools associated with navigation, including 4 | localization, mapping, path planning, and path following. 5 | """ 6 | 7 | # Import all path planner plugins included with PyRoboSim. 8 | from .a_star import * 9 | from .prm import * 10 | from .rrt import * 11 | from .world_graph import * 12 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/navigation/types.py: -------------------------------------------------------------------------------- 1 | """ 2 | Basic types for path planning and navigation. 3 | """ 4 | 5 | from typing import Any 6 | 7 | from ..planning.actions import ExecutionResult 8 | from ..utils.path import Path 9 | from ..utils.pose import Pose 10 | from ..utils.search_graph import SearchGraph 11 | 12 | 13 | class PathPlanner: 14 | """ 15 | Generic path planner class that helps with type hinting. 16 | 17 | When implementing a new planner, you should subclass from this class. 18 | """ 19 | 20 | plugin_name: str 21 | """The name of the plugin. Must be implemented by child class.""" 22 | 23 | registered_plugins: dict[str, Any] = {} 24 | """List of registered path planner plugins.""" 25 | 26 | def __init__(self) -> None: 27 | from ..core.robot import Robot 28 | from ..core.world import World 29 | 30 | self.robot: Robot | None = None 31 | self.world: World | None = None 32 | 33 | def __init_subclass__(cls, **kwargs: Any): 34 | """Registers a path planner subclass.""" 35 | cls.registered_plugins[cls.plugin_name] = cls 36 | 37 | def plan(self, start: Pose, goal: Pose) -> Path: 38 | """ 39 | Plans a path from start to goal. 40 | 41 | :param start: Start pose. 42 | :param goal: Goal pose. 43 | :return: Path from start to goal. 44 | """ 45 | raise NotImplementedError("Must implement in subclass.") 46 | 47 | def reset(self) -> None: 48 | """Resets the path planner.""" 49 | self.latest_path = Path() 50 | if self.robot is not None: 51 | self.world = self.robot.world 52 | 53 | def get_graphs(self) -> list[SearchGraph]: 54 | """ 55 | Returns the graphs generated by the planner, if any. 56 | 57 | :return: List of graphs. 58 | """ 59 | raise NotImplementedError("Must implement in subclass.") 60 | 61 | def get_latest_path(self) -> Path | None: 62 | """ 63 | Returns the latest path generated by the planner, if any. 64 | 65 | :return: The latest path if one exists, else None. 66 | """ 67 | raise NotImplementedError("Must implement in subclass.") 68 | 69 | def to_dict(self) -> dict[str, Any]: 70 | """ 71 | Serializes the planner to a dictionary. 72 | 73 | :return: A dictionary containing the planner information. 74 | """ 75 | raise NotImplementedError("Must implement in subclass.") 76 | 77 | 78 | class PathExecutor: 79 | """ 80 | Generic path executor class that helps with type hinting. 81 | 82 | When implementing a new executor, you should subclass from this class. 83 | """ 84 | 85 | following_path = False 86 | """Flag to track path following.""" 87 | 88 | cancel_execution = False 89 | """Flag to cancel from user.""" 90 | 91 | def __init__(self) -> None: 92 | from ..core.robot import Robot 93 | 94 | self.robot: Robot | None = None 95 | 96 | def execute( 97 | self, path: Path, realtime_factor: float = 1.0, battery_usage: float = 0.0 98 | ) -> ExecutionResult: 99 | """ 100 | Generates and executes a trajectory on the robot. 101 | 102 | :param path: Path to execute on the robot. 103 | :param realtime_factor: A multiplier on the execution time relative to 104 | real time, defaults to 1.0. 105 | :param battery_usage: Robot battery usage per unit distance. 106 | :return: An object describing the execution result. 107 | """ 108 | raise NotImplementedError("Must implement in subclass.") 109 | 110 | def to_dict(self) -> dict[str, Any]: 111 | """ 112 | Serializes the executor to a dictionary. 113 | 114 | :return: A dictionary containing the executor information. 115 | """ 116 | raise NotImplementedError("Must implement in subclass.") 117 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/navigation/visualization.py: -------------------------------------------------------------------------------- 1 | """Visualization utilities for path planners.""" 2 | 3 | from typing import Sequence 4 | 5 | import matplotlib.pyplot as plt 6 | from matplotlib.artist import Artist 7 | from matplotlib.axes import Axes 8 | from matplotlib.collections import LineCollection 9 | 10 | from ..utils.path import Path 11 | from ..utils.search_graph import SearchGraph 12 | 13 | 14 | def plot_path_planner( 15 | axes: Axes, 16 | graphs: list[SearchGraph] = [], 17 | path: Path | None = None, 18 | path_color: Sequence[float] | str = "m", 19 | ) -> dict[str, list[Artist]]: 20 | """ 21 | Plots the planned path on a specified set of axes. 22 | 23 | :param axes: The axes on which to draw. 24 | :param graphs: A list of path planner graphs to display. 25 | :param path: Path to display. 26 | :param path_color: Color of the path, as an RGB tuple or string. 27 | :return: List of Matplotlib artists containing what was drawn, 28 | used for bookkeeping. 29 | """ 30 | graph_artists: list[Artist] = [] 31 | path_artists: list[Artist] = [] 32 | artists: dict[str, list[Artist]] = {} 33 | 34 | for graph in graphs: 35 | # Plot the markers 36 | (markers,) = axes.plot( 37 | [n.pose.x for n in graph.nodes], 38 | [n.pose.y for n in graph.nodes], 39 | color=graph.color, 40 | alpha=graph.color_alpha, 41 | linestyle="", 42 | marker="o", 43 | markerfacecolor=graph.color, 44 | markeredgecolor=graph.color, 45 | markersize=3, 46 | zorder=1, 47 | ) 48 | graph_artists.append(markers) 49 | 50 | # Plot the edges as a LineCollection 51 | edge_coords = [ 52 | [[e.nodeA.pose.x, e.nodeA.pose.y], [e.nodeB.pose.x, e.nodeB.pose.y]] 53 | for e in graph.edges 54 | ] 55 | line_segments = LineCollection( 56 | edge_coords, 57 | color=graph.color, 58 | alpha=graph.color_alpha, 59 | linewidth=0.5, 60 | linestyle="--", 61 | zorder=1, 62 | ) 63 | axes.add_collection(line_segments) 64 | graph_artists.append(line_segments) 65 | 66 | if path and path.num_poses > 0: 67 | x = [p.x for p in path.poses] 68 | y = [p.y for p in path.poses] 69 | (path,) = axes.plot( 70 | x, y, linestyle="-", color=path_color, linewidth=3, alpha=0.5, zorder=1 71 | ) 72 | (start,) = axes.plot(x[0], y[0], "go", zorder=2) 73 | (goal,) = axes.plot(x[-1], y[-1], "rx", zorder=2) 74 | path_artists.extend((path, start, goal)) 75 | 76 | if graph_artists: 77 | artists["graph"] = graph_artists 78 | if path_artists: 79 | artists["path"] = path_artists 80 | return artists 81 | 82 | 83 | def show_path_planner( 84 | graphs: list[SearchGraph] = [], 85 | path: Path | None = None, 86 | title: str = "Path Planner Output", 87 | ) -> None: 88 | """ 89 | Shows the path planner output in a new figure. 90 | 91 | :param graphs: A list of path planner graphs to display. 92 | :param path: Path to display. 93 | :param title: Title to display. 94 | """ 95 | 96 | f = plt.figure() 97 | ax = f.add_subplot(111) 98 | plot_path_planner(ax, graphs=graphs, path=path) 99 | plt.title(title) 100 | plt.axis("equal") 101 | plt.show() 102 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/planning/__init__.py: -------------------------------------------------------------------------------- 1 | """Task and motion planning utilities. 2 | 3 | This module contains tools associated with task and motion planning 4 | (TAMP). This includes representations for parametric actions that comprise 5 | a plan, as well as infrastructure to come up with such a plan given the 6 | current and desired state of the world. 7 | """ 8 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/planning/pddlstream/__init__.py: -------------------------------------------------------------------------------- 1 | """Tools for PDDLStream based task and motion planning. 2 | 3 | This module contains tools associated with task and motion planning (TAMP) 4 | using the PDDLStream package. This includes a wrapper around PDDLStream 5 | algorithms, as well as mechanisms to integrate PDDL domains and PDDLStream 6 | stream definition files, along with their mappings to Python functions with 7 | the actual implementations. 8 | """ 9 | 10 | from importlib.util import find_spec 11 | 12 | if find_spec("pddlstream") is None: 13 | raise ModuleNotFoundError("PDDLStream is not available. Cannot import planner.") 14 | 15 | from .default_mappings import * 16 | from .planner import * 17 | from .primitives import * 18 | from .utils import * 19 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/planning/pddlstream/default_mappings.py: -------------------------------------------------------------------------------- 1 | """ 2 | Default mappings for PDDLStream functions, streams, and certificate tests that represent 3 | Task and Motion Planning for pick-and-place applications with a mobile manipulator. 4 | """ 5 | 6 | from typing import Any 7 | 8 | from pddlstream.language.stream import StreamInfo 9 | from pddlstream.language.generator import from_gen_fn, from_list_fn, from_test 10 | 11 | from . import primitives 12 | from ...core.robot import Robot 13 | from ...core.world import World 14 | 15 | 16 | def get_stream_map(world: World, robot: Robot) -> dict[str, Any]: 17 | """ 18 | Returns a dictionary mapping stream names to function implementations. 19 | 20 | :param world: World object for planning. 21 | :param robot: Robot object for planning. 22 | :return: The stream map dictionary. 23 | """ 24 | robot.path_planner 25 | robot.grasp_generator 26 | 27 | stream_map = { 28 | # Functions 29 | "Dist": primitives.get_straight_line_distance, 30 | "PickPlaceCost": primitives.get_pick_place_cost, 31 | "PickPlaceAtPoseCost": primitives.get_pick_place_at_pose_cost, 32 | "GraspAtPoseCost": primitives.get_grasp_at_pose_cost, 33 | "DetectCost": primitives.get_detect_cost, 34 | "OpenCloseCost": primitives.get_open_close_cost, 35 | "PathLength": primitives.get_path_length, 36 | # Streams (that sample) 37 | "s-navpose": from_list_fn(primitives.get_nav_poses), 38 | "s-place": from_gen_fn( 39 | lambda loc, obj: primitives.sample_place_pose( 40 | loc, 41 | obj, 42 | max_tries=world.max_object_sample_tries, 43 | ) 44 | ), 45 | # Streams (no sampling, just testing) 46 | "t-collision-free": from_test(primitives.test_collision_free), 47 | } 48 | 49 | if robot.path_planner is not None: 50 | stream_map["s-motion"] = from_gen_fn( 51 | lambda p1, p2: primitives.sample_motion(robot.path_planner, p1, p2) 52 | ) 53 | if robot.grasp_generator is not None: 54 | stream_map["s-grasp"] = from_gen_fn( 55 | lambda obj, p_obj, p_robot: primitives.sample_grasp_pose( 56 | robot.grasp_generator, 57 | obj, 58 | p_obj, 59 | p_robot, 60 | front_grasps=True, 61 | top_grasps=True, 62 | side_grasps=False, 63 | ) 64 | ) 65 | 66 | return stream_map 67 | 68 | 69 | def get_stream_info() -> dict[str, StreamInfo]: 70 | """ 71 | Returns a dictionary from stream name to StreamInfo altering how 72 | individual streams are handled. 73 | 74 | :return: The stream information dictionary. 75 | """ 76 | return { 77 | # Streams (that sample) 78 | # Nav poses can be eagerly sampled since locations don't move. 79 | "s-navpose": StreamInfo(eager=True), 80 | # Other streams cannot be eagerly sampled as they depend on the 81 | # instantaneous pose of entities in the world during planning. 82 | "s-motion": StreamInfo(eager=False), 83 | "s-grasp": StreamInfo(eager=False), 84 | "s-place": StreamInfo(eager=False), 85 | # Streams (no sampling, just testing) 86 | "t-collision-free": StreamInfo(eager=False, negate=True), 87 | } 88 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/sensors/__init__.py: -------------------------------------------------------------------------------- 1 | """Sensor utilities. 2 | 3 | This module contains tools associated with sensor simulation, 4 | such as lidar and field-of-view (FOV) sensors. 5 | """ 6 | 7 | # Import all sensor plugins included with PyRoboSim. 8 | from .lidar import * 9 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/sensors/types.py: -------------------------------------------------------------------------------- 1 | """ 2 | Basic types for sensor simulation. 3 | """ 4 | 5 | from threading import Thread 6 | from typing import Any 7 | 8 | from matplotlib.artist import Artist 9 | 10 | 11 | class Sensor: 12 | """ 13 | Generic sensor class that helps with type hinting. 14 | 15 | When implementing a new sensor, you should subclass from this class. 16 | """ 17 | 18 | plugin_name: str 19 | """The name of the plugin. Must be implemented by child class.""" 20 | 21 | registered_plugins: dict[str, Any] = {} 22 | """List of registered sensor plugins.""" 23 | 24 | def __init__(self) -> None: 25 | from ..core.robot import Robot 26 | 27 | self.robot: Robot | None = None 28 | self.thread = Thread(target=self.thread_function) 29 | self.is_active = False 30 | 31 | def __init_subclass__(cls, **kwargs: Any): 32 | """Registers a path planner subclass.""" 33 | cls.registered_plugins[cls.plugin_name] = cls 34 | 35 | def __del__(self) -> None: 36 | self.is_active = False 37 | 38 | def update(self) -> None: 39 | """ 40 | Performs the sensor calculation. 41 | 42 | Must be implemented in your sensor implementation. 43 | """ 44 | raise NotImplementedError("Must implement update()!") 45 | 46 | def get_measurement(self) -> Any: 47 | """ 48 | Returns the latest measurement from the sensor. 49 | 50 | Must be implemented in your sensor implementation. 51 | 52 | :return: The latest measurement. 53 | """ 54 | raise NotImplementedError("Must implement get_measurement()!") 55 | 56 | def thread_function(self) -> None: 57 | """ 58 | Defines the sensor update function to run in a background thread. 59 | 60 | If the sensor has nothing to do in the background, you can leave this unimplemented in your sensor. 61 | """ 62 | return 63 | 64 | def start_thread(self) -> None: 65 | """ 66 | Starts the thread defined by your sensor's implementation of `thread_function()`. 67 | """ 68 | if self.thread is not None: 69 | self.is_active = True 70 | self.thread.start() 71 | 72 | def stop_thread(self) -> None: 73 | """ 74 | Stops the actively running sensor thread. 75 | 76 | You should not need to override this function, so long as your sensor's 77 | implementation of `thread_function()` has a way to stop when the 78 | `is_active` attribute becomes `False` on deletion. 79 | """ 80 | if (self.thread is not None) and self.thread.is_alive(): 81 | self.is_active = False 82 | self.thread.join() 83 | 84 | def setup_artists(self) -> list[Artist]: 85 | """ 86 | Sets up and returns the artists for visualizing the sensor. 87 | 88 | :return: The list of MatPlotLib artists for the sensor. 89 | """ 90 | return [] 91 | 92 | def update_artists(self) -> None: 93 | """ 94 | Updates the artists. 95 | 96 | These should have been originally returned by `setup_artists()`. 97 | """ 98 | 99 | def to_dict(self) -> dict[str, Any]: 100 | """ 101 | Serializes the sensor to a dictionary. 102 | 103 | :return: A dictionary containing the sensor information. 104 | """ 105 | raise NotImplementedError("Must implement to_dict()!") 106 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/__init__.py: -------------------------------------------------------------------------------- 1 | """General utilities. 2 | 3 | This module contains general utilities used throughout PyRoboSim, 4 | such as pose/polygon representations, or tracking knowledge about objects 5 | and locations in a specific world model. 6 | """ 7 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/general.py: -------------------------------------------------------------------------------- 1 | """General package utilities.""" 2 | 3 | import os 4 | import re 5 | from typing import Sequence 6 | 7 | from matplotlib.colors import CSS4_COLORS, to_rgb 8 | 9 | 10 | def get_data_folder() -> str: 11 | """ 12 | Get a path to the folder containing data. 13 | 14 | If running using ``ros2 run``, this looks at the installed data in the 15 | ``pyrobosim`` colcon package's share directory. 16 | 17 | If running standalone, this looks at the data folder in the actual source. 18 | 19 | :return: Path to data folder. 20 | """ 21 | try: 22 | # If running as a ROS 2 node, get the data folder from the package share directory. 23 | from ament_index_python.packages import get_package_share_directory 24 | 25 | data_folder = os.path.join(get_package_share_directory("pyrobosim"), "data") 26 | except: 27 | # Else, assume it's relative to the file's current directory. 28 | data_folder = os.path.join( 29 | os.path.dirname(os.path.abspath(__file__)), "..", "data" 30 | ) 31 | 32 | return data_folder 33 | 34 | 35 | def replace_special_yaml_tokens( 36 | in_text: str | list[str], root_dir: str | None = None 37 | ) -> str | list[str]: 38 | """ 39 | Replaces special tokens permitted in our YAML specification. 40 | If you want to add any other special tokens, you should do so in the process_text helper function. 41 | 42 | :param in_text: Input YAML text or a list of YAML texts. 43 | :param root_dir: Root directory for basing some tokens, uses the current directory if not specified. 44 | :return: YAML text(s) with all special tokens substituted. 45 | """ 46 | if root_dir is None: 47 | root_dir = os.getcwd() 48 | 49 | def process_text(text: str) -> str: 50 | """Helper function to replace tokens in a single metadata string.""" 51 | text = text.replace("$HOME", os.environ["HOME"]) 52 | text = text.replace("$DATA", get_data_folder()) 53 | text = text.replace("$PWD", root_dir) 54 | return text 55 | 56 | if isinstance(in_text, list): 57 | return [process_text(text) for text in in_text] 58 | elif isinstance(in_text, str): 59 | return process_text(in_text) 60 | else: 61 | raise TypeError(f"Could not replace text for input type: {type(in_text)}.") 62 | 63 | 64 | def parse_color(color: Sequence[float] | str) -> Sequence[float]: 65 | """ 66 | Parses a color input and returns an RGB tuple. 67 | 68 | :param color: Input color as a list, tuple, string, or hexadecimal. 69 | :return: RGB tuple in range (0.0, 1.0). 70 | """ 71 | if isinstance(color, (list, tuple)): 72 | if len(color) == 3: 73 | return tuple(color) 74 | raise ValueError( 75 | "Incorrect number of elements. RGB color must have exactly 3 elements." 76 | ) 77 | 78 | if isinstance(color, str): 79 | if color in CSS4_COLORS: 80 | color = to_rgb(CSS4_COLORS[color]) 81 | assert isinstance(color, tuple) and len(color) == 3 82 | return color 83 | 84 | hex_pattern = r"^#(?:[0-9a-fA-F]{3}){1,2}$" 85 | if re.match(hex_pattern, color): 86 | color = to_rgb(color) 87 | assert isinstance(color, tuple) and len(color) == 3 88 | return color 89 | 90 | raise ValueError(f"Invalid color name or hexadecimal value: {color}.") 91 | 92 | raise ValueError( 93 | "Unsupported input type. Expected a list, tuple, or string representing a color." 94 | ) 95 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/graph_types.py: -------------------------------------------------------------------------------- 1 | """Basic types for graphs.""" 2 | 3 | from typing import Any 4 | from typing_extensions import Self # For compatibility with Python <= 3.10 5 | 6 | from .pose import Pose 7 | 8 | 9 | class Node: 10 | """Graph node representation.""" 11 | 12 | def __init__(self, pose: Pose, parent: Any = None, cost: float = 0.0) -> None: 13 | """ 14 | Creates a graph node. 15 | 16 | :param pose: Pose of the node. 17 | :param parent: Parent node, if any. 18 | :param cost: Cost of the node, defaults to zero. 19 | """ 20 | self.pose = pose 21 | self.parent = parent 22 | self.cost = cost 23 | self.neighbors: set[Self] = set() # used in graph based planners 24 | 25 | 26 | class Edge: 27 | """Graph edge representation.""" 28 | 29 | def __init__(self, nodeA: Node, nodeB: Node) -> None: 30 | """ 31 | Creates a graph edge. 32 | 33 | :param nodeA: First node 34 | :param nodeB: Second node 35 | """ 36 | self.nodeA = nodeA 37 | self.nodeB = nodeB 38 | self.cost = nodeA.pose.get_linear_distance(nodeB.pose, ignore_z=True) 39 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/logging.py: -------------------------------------------------------------------------------- 1 | """ 2 | Logging utilities for PyRoboSim. 3 | """ 4 | 5 | import logging 6 | from logging import Logger 7 | 8 | 9 | def create_logger(name: str, level: int = logging.INFO) -> Logger: 10 | """ 11 | Defines a general logger for use with PyRoboSim. 12 | 13 | :param name: The name of the logger. 14 | :param level: The level of the logger. 15 | :return: A logger instance. 16 | """ 17 | logger = logging.getLogger(name) 18 | logger.setLevel(level) 19 | 20 | # TODO: Consider configuring console vs. file logging at some point. 21 | if not logger.hasHandlers(): # Prevents adding duplicate handlers 22 | log_formatter = logging.Formatter("[%(name)s] %(levelname)s: %(message)s") 23 | console_handler = logging.StreamHandler() 24 | console_handler.setFormatter(log_formatter) 25 | logger.addHandler(console_handler) 26 | 27 | # Needed to propagate to unit tests via the caplog fixture. 28 | logger.propagate = True 29 | 30 | return logger 31 | 32 | 33 | PYROBOSIM_GLOBAL_LOGGER = create_logger("pyrobosim") 34 | 35 | 36 | def get_global_logger() -> Logger: 37 | """ 38 | Returns the global logger for PyRoboSim. 39 | 40 | :return: The PyRoboSim global logger instance. 41 | """ 42 | return PYROBOSIM_GLOBAL_LOGGER 43 | 44 | 45 | def set_global_logger(logger: Logger) -> None: 46 | """ 47 | Sets the global PyRoboSim logger to another specified logger. 48 | 49 | :param logger: The logger to use as the new global logger. 50 | """ 51 | global PYROBOSIM_GLOBAL_LOGGER 52 | PYROBOSIM_GLOBAL_LOGGER = logger 53 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/path.py: -------------------------------------------------------------------------------- 1 | """ 2 | Path representation for motion planning. 3 | """ 4 | 5 | from typing_extensions import Self # For compatibility with Python <= 3.10 6 | 7 | from .pose import Pose 8 | 9 | 10 | class Path: 11 | """Representation of a path for motion planning.""" 12 | 13 | def __init__( 14 | self, poses: list[Pose] = [], planning_time: float | None = None 15 | ) -> None: 16 | """ 17 | Creates a Path object instance. 18 | 19 | :param poses: List of poses representing a path. 20 | :param planning_time: The time taken to generate this path. 21 | """ 22 | self.set_poses(poses) 23 | self.planning_time = planning_time 24 | 25 | def set_poses(self, poses: list[Pose]) -> None: 26 | """ 27 | Sets the list of poses and computes derived quantities. 28 | Use this method to change the poses of an existing path, 29 | rather than directly assigning the `poses` attribute. 30 | 31 | :param poses: List of poses representing a path. 32 | """ 33 | self.poses = poses 34 | self.num_poses = len(self.poses) 35 | self.length = 0.0 36 | for i in range(self.num_poses - 1): 37 | self.length += self.poses[i].get_linear_distance(self.poses[i + 1]) 38 | 39 | def fill_yaws(self) -> None: 40 | """ 41 | Fills in any yaw angles along a path to point at the next waypoint. 42 | """ 43 | if self.num_poses < 1: 44 | return 45 | 46 | for idx in range(1, self.num_poses - 1): 47 | cur_pose = self.poses[idx] 48 | prev_pose = self.poses[idx - 1] 49 | yaw = prev_pose.get_angular_distance(cur_pose) 50 | cur_pose.set_euler_angles(yaw=yaw) 51 | 52 | def __eq__(self, other: object) -> bool: 53 | """ 54 | Check if two paths are exactly equal. 55 | 56 | :param other: Path with which to check equality. 57 | :return: True if the paths are equal, else False 58 | """ 59 | if not (isinstance(other, Path)): 60 | raise TypeError("Expected a Path object.") 61 | 62 | return (self.poses == other.poses) and (self.length == other.length) 63 | 64 | def __repr__(self) -> str: 65 | """Return brief description of the path.""" 66 | print_str = f"Path with {self.num_poses} points, Length {self.length:.3f}" 67 | return print_str 68 | 69 | def print_details(self) -> None: 70 | """Print detailed description of the path.""" 71 | print_str = f"Path with {self.num_poses} points." 72 | for i, p in enumerate(self.poses): 73 | print_str += f"\n {i + 1}. {p}" 74 | print_str += f"\nTotal Length: {self.length:.3f}" 75 | if self.planning_time: 76 | if self.planning_time > 0.01: 77 | print_str += f"\nPlanning time: {self.planning_time:3f} seconds" 78 | else: 79 | print_str += ( 80 | f"\nPlanning time: {self.planning_time * 1000.0:3f} milliseconds" 81 | ) 82 | print(print_str) 83 | -------------------------------------------------------------------------------- /pyrobosim/pyrobosim/utils/world_motion_planning.py: -------------------------------------------------------------------------------- 1 | """ 2 | Motion planning utilities that require a world instance. 3 | 4 | This is mostly to avoid circular imports. 5 | """ 6 | 7 | from ..core.world import World 8 | from ..utils.pose import Pose 9 | 10 | 11 | def reduce_waypoints_polygon( 12 | world: World, poses: list[Pose], step_dist: float = 0.01 13 | ) -> list[Pose]: 14 | """ 15 | Reduces the number of waypoints in a path generated from a polygon based planner. 16 | 17 | :param world: The world object in which the path is generated. 18 | :param poses: The list of poses that make up the path. 19 | :param step_dist: The step size for discretizing a straight line to check collisions. 20 | """ 21 | waypoints = [] 22 | start = poses[0] 23 | waypoints.append(start) 24 | poses = poses[1:] 25 | i = len(poses) - 1 26 | while poses and i >= 0: 27 | current = poses[i] 28 | if world.is_connectable(start, current, step_dist): 29 | waypoints.append(current) 30 | start = current 31 | poses = poses[i + 1 :] 32 | i = len(poses) - 1 33 | else: 34 | i -= 1 35 | return waypoints 36 | -------------------------------------------------------------------------------- /pyrobosim/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from pathlib import Path 3 | from setuptools import setup, find_packages 4 | 5 | 6 | def get_files_in_folder(directory: str) -> list[str]: 7 | """Helper function to get all files in a specific directory.""" 8 | file_list: list[str] = [] 9 | for path, _, fnames in os.walk(directory): 10 | for filename in fnames: 11 | file_list.append(os.path.join("..", path, filename)) 12 | return file_list 13 | 14 | 15 | project_name = "pyrobosim" 16 | 17 | data_dir = os.path.join(project_name, "data") 18 | install_requires = [ 19 | "adjustText", 20 | "astar", 21 | "matplotlib", 22 | "numpy", 23 | "pycollada", 24 | "PySide6>=6.4.0", 25 | "PyYAML", 26 | "shapely>=2.0.1", 27 | "scipy", 28 | "transforms3d", 29 | "trimesh", 30 | "typing_extensions", # For compatibility with Python <= 3.10 31 | ] 32 | 33 | # This will gracefully fall back to an empty string if the README.md cannot be read. 34 | # This can happen if building this package with `colcon build --symlink-install`. 35 | readme_path = Path(__file__).parent / "README.md" 36 | readme_text = readme_path.read_text() if readme_path.exists() else "" 37 | 38 | setup( 39 | name=project_name, 40 | version="4.0.0", 41 | url="https://github.com/sea-bass/pyrobosim", 42 | author="Sebastian Castro", 43 | author_email="sebas.a.castro@gmail.com", 44 | description="ROS 2 enabled 2D mobile robot simulator for behavior prototyping.", 45 | long_description=readme_text, 46 | long_description_content_type="text/markdown", 47 | license="MIT", 48 | install_requires=install_requires, 49 | packages=find_packages(), 50 | package_data={project_name: get_files_in_folder(data_dir)}, 51 | tests_require=["pytest"], 52 | zip_safe=True, 53 | ) 54 | -------------------------------------------------------------------------------- /pyrobosim/test/core/test_gazebo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Unit tests for world Gazebo exporting utilities.""" 4 | 5 | import os 6 | import pytest 7 | import tempfile 8 | 9 | from pyrobosim.core import World, WorldGazeboExporter, WorldYamlLoader 10 | from pyrobosim.utils.general import get_data_folder 11 | 12 | 13 | def load_world() -> World: 14 | """Load a test world.""" 15 | world_file = os.path.join(get_data_folder(), "test_world.yaml") 16 | return WorldYamlLoader().from_file(world_file) 17 | 18 | 19 | def test_export_gazebo_default_folder() -> None: 20 | """Exports a test world to Gazebo using the default folder.""" 21 | world = load_world() 22 | 23 | exporter = WorldGazeboExporter(world) 24 | world_folder = exporter.export() 25 | assert world_folder == os.path.join(get_data_folder(), "worlds", world.name) 26 | 27 | 28 | @pytest.mark.parametrize("classic", [True, False]) # type: ignore[misc] 29 | def test_export_gazebo(classic: bool) -> None: 30 | """Exports a test world to Gazebo using a provided output folder.""" 31 | world = load_world() 32 | output_folder = tempfile.mkdtemp() 33 | 34 | exporter = WorldGazeboExporter(world) 35 | world_folder = exporter.export(classic=classic, out_folder=output_folder) 36 | assert world_folder == os.path.join(output_folder, world.name) 37 | -------------------------------------------------------------------------------- /pyrobosim/test/core/test_objects.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Tests for object creation in PyRoboSim. 5 | """ 6 | 7 | import os 8 | from pytest import LogCaptureFixture 9 | 10 | from pyrobosim.core import Object, World 11 | from pyrobosim.utils.general import get_data_folder 12 | from pyrobosim.utils.pose import Pose 13 | 14 | 15 | data_folder = get_data_folder() 16 | 17 | 18 | class TestObjects: 19 | def test_add_object_to_world_from_object(self, caplog: LogCaptureFixture) -> None: 20 | """Test adding an object from an Object instance.""" 21 | world = World() 22 | world.set_metadata( 23 | locations=os.path.join(data_folder, "example_location_data.yaml"), 24 | objects=os.path.join(data_folder, "example_object_data.yaml"), 25 | ) 26 | 27 | coords = [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)] 28 | room = world.add_room(name="test_room", footprint=coords) 29 | assert room is not None 30 | 31 | table = world.add_location( 32 | name="test_table", 33 | category="table", 34 | parent="test_room", 35 | pose=Pose(x=0.0, y=0.0), 36 | ) 37 | assert table is not None 38 | 39 | obj = Object( 40 | name="test_banana", 41 | category="banana", 42 | parent=table.children[0], 43 | pose=Pose(x=0.0, y=0.0, yaw=1.0), 44 | ) 45 | result = world.add_object(object=obj) 46 | assert isinstance(result, Object) 47 | assert world.num_objects == 1 48 | assert world.objects[0].name == "test_banana" 49 | assert world.objects[0].parent.parent == table 50 | 51 | # Adding the same object again should fail due to duplicate names. 52 | result = world.add_object(object=obj) 53 | assert result is None 54 | assert world.num_objects == 1 55 | assert ( 56 | "Object test_banana already exists in the world. Cannot add." in caplog.text 57 | ) 58 | 59 | def test_add_object_to_world_from_args(self) -> None: 60 | """Test adding an object from a list of named keyword arguments.""" 61 | world = World() 62 | world.set_metadata( 63 | locations=os.path.join(data_folder, "example_location_data.yaml"), 64 | objects=os.path.join(data_folder, "example_object_data.yaml"), 65 | ) 66 | 67 | coords = [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)] 68 | room = world.add_room(name="test_room", footprint=coords) 69 | assert room is not None 70 | 71 | table = world.add_location( 72 | name="test_table", 73 | category="table", 74 | parent="test_room", 75 | pose=Pose(x=0.0, y=0.0), 76 | ) 77 | assert table is not None 78 | 79 | result = world.add_object(category="banana", parent=table) 80 | assert isinstance(result, Object) 81 | assert world.num_objects == 1 82 | assert world.objects[0].name == "banana0" 83 | assert world.objects[0].parent.parent == table 84 | -------------------------------------------------------------------------------- /pyrobosim/test/core/test_room.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Tests for room creation in PyRoboSim. 5 | """ 6 | 7 | import pytest 8 | from pytest import LogCaptureFixture 9 | 10 | from pyrobosim.core import Room, World 11 | from pyrobosim.utils.pose import Pose 12 | 13 | 14 | class TestRoom: 15 | def test_add_room_to_world_from_object(self, caplog: LogCaptureFixture) -> None: 16 | """Test adding a room from a Room object.""" 17 | world = World() 18 | 19 | coords = [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)] 20 | room = Room(name="test_room", footprint=coords) 21 | result = world.add_room(room=room) 22 | 23 | assert isinstance(result, Room) 24 | assert world.num_rooms == 1 25 | assert world.rooms[0].name == "test_room" 26 | assert world.rooms[0].original_pose is None 27 | assert world.rooms[0].pose.is_approx(Pose(x=0.0, y=0.0)) # Centroid 28 | 29 | # Adding the same room again should fail due to duplicate names. 30 | result = world.add_room(room=room) 31 | assert result is None 32 | assert world.num_rooms == 1 33 | assert "Room test_room already exists in the world. Cannot add." in caplog.text 34 | 35 | def test_add_room_to_world_from_args(self) -> None: 36 | """Test adding a room from a list of named keyword arguments.""" 37 | world = World() 38 | 39 | coords = [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)] 40 | color = (1.0, 0.0, 0.1) 41 | pose = Pose(x=1.0, y=2.0) 42 | result = world.add_room(footprint=coords, color=color, pose=pose) 43 | 44 | assert isinstance(result, Room) 45 | assert world.num_rooms == 1 46 | assert world.rooms[0].name == "room0" 47 | assert world.rooms[0].viz_color == color 48 | assert world.rooms[0].original_pose == world.rooms[0].pose 49 | assert world.rooms[0].pose.is_approx(Pose(x=1.0, y=2.0)) # Specified pose 50 | 51 | def test_add_room_to_world_empty_geometry(self) -> None: 52 | """Test adding a room with an empty footprint. Should raise an exception.""" 53 | world = World() 54 | 55 | with pytest.raises(RuntimeError) as exc_info: 56 | world.add_room(name="test_room") 57 | assert str(exc_info.value) == "Room footprint cannot be empty." 58 | 59 | def test_add_room_to_world_in_collision(self, caplog: LogCaptureFixture) -> None: 60 | """Test adding a room in collision with another room.""" 61 | world = World() 62 | 63 | # This room should be added correctly. 64 | orig_coords = [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)] 65 | result = world.add_room(footprint=orig_coords) 66 | assert isinstance(result, Room) 67 | assert world.num_rooms == 1 68 | 69 | # This new room should fail to add since it's in the collision with the first room. 70 | new_coords = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)] 71 | result = world.add_room(footprint=new_coords) 72 | assert result is None 73 | assert world.num_rooms == 1 74 | assert "Room room1 in collision. Cannot add to world." in caplog.text 75 | -------------------------------------------------------------------------------- /pyrobosim/test/navigation/test_astar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Unit tests for A* planner""" 4 | 5 | import os 6 | 7 | from pyrobosim.core import WorldYamlLoader 8 | from pyrobosim.navigation.a_star import AStarPlanner 9 | from pyrobosim.utils.general import get_data_folder 10 | from pyrobosim.utils.pose import Pose 11 | 12 | 13 | def test_astar() -> None: 14 | """Test A* planner with and without path compression""" 15 | 16 | world = WorldYamlLoader().from_file( 17 | os.path.join(get_data_folder(), "test_world.yaml") 18 | ) 19 | 20 | start = Pose(x=-1.6, y=2.8) 21 | goal = Pose(x=2.5, y=3.0) 22 | 23 | robot = world.robots[0] 24 | planner_config = { 25 | "grid_resolution": 0.05, 26 | "grid_inflation_radius": 1.5 * robot.radius, 27 | "diagonal_motion": True, 28 | "heuristic": "euclidean", 29 | "compress_path": False, 30 | } 31 | astar_planner = AStarPlanner(**planner_config) 32 | robot.set_path_planner(astar_planner) 33 | full_path = astar_planner.plan(start, goal).poses 34 | 35 | assert len(full_path) >= 2 36 | assert full_path[0] == start 37 | assert full_path[-1] == goal 38 | 39 | # Plan for same start and goal with path compression enabled 40 | planner_config = { 41 | "grid_resolution": 0.05, 42 | "grid_inflation_radius": 1.5 * robot.radius, 43 | "diagonal_motion": True, 44 | "heuristic": "euclidean", 45 | "compress_path": True, 46 | } 47 | astar_planner = AStarPlanner(**planner_config) 48 | 49 | robot.set_path_planner(astar_planner) 50 | astar_planner.reset() 51 | compressed_path = astar_planner.plan(start, goal).poses 52 | 53 | assert len(compressed_path) >= 2 54 | assert len(compressed_path) <= len(full_path) 55 | assert compressed_path[0] == start 56 | assert compressed_path[-1] == goal 57 | -------------------------------------------------------------------------------- /pyrobosim/test/navigation/test_prm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Unit tests for the PRM planner""" 4 | 5 | import os 6 | import numpy as np 7 | from pytest import LogCaptureFixture 8 | 9 | from pyrobosim.core import WorldYamlLoader 10 | from pyrobosim.navigation.prm import PRMPlanner 11 | from pyrobosim.utils.general import get_data_folder 12 | from pyrobosim.utils.pose import Pose 13 | 14 | 15 | def test_prm_default() -> None: 16 | """Tests planning with default world graph planner settings.""" 17 | world = WorldYamlLoader().from_file( 18 | os.path.join(get_data_folder(), "test_world.yaml") 19 | ) 20 | 21 | np.random.seed(1234) # Fix seed for reproducibility 22 | prm = PRMPlanner() 23 | world.robots[0].set_path_planner(prm) 24 | 25 | start = Pose(x=-1.6, y=2.8) 26 | goal = Pose(x=2.5, y=3.0) 27 | path = prm.plan(start, goal) 28 | 29 | assert len(path.poses) >= 2 30 | assert path.poses[0] == start 31 | assert path.poses[-1] == goal 32 | 33 | 34 | def test_prm_no_path(caplog: LogCaptureFixture) -> None: 35 | """Test that PRM gracefully returns when there is no feasible path.""" 36 | world = WorldYamlLoader().from_file( 37 | os.path.join(get_data_folder(), "test_world.yaml") 38 | ) 39 | 40 | prm = PRMPlanner() 41 | world.robots[0].set_path_planner(prm) 42 | 43 | start = Pose(x=-1.6, y=2.8) 44 | goal = Pose(x=12.5, y=3.0) 45 | path = prm.plan(start, goal) 46 | 47 | assert len(path.poses) == 0 48 | assert "Could not find a path from start to goal." in caplog.text 49 | 50 | 51 | def test_prm_compress_path() -> None: 52 | """Tests planning with path compression option.""" 53 | world = WorldYamlLoader().from_file( 54 | os.path.join(get_data_folder(), "test_world.yaml") 55 | ) 56 | planner_config = {"compress_path": False} 57 | prm = PRMPlanner(**planner_config) 58 | world.robots[0].set_path_planner(prm) 59 | 60 | np.random.seed(1234) # Use same seed for reproducibility 61 | start = Pose(x=-0.3, y=0.6) 62 | goal = Pose(x=2.5, y=3.0) 63 | orig_path = prm.plan(start, goal) 64 | assert len(orig_path.poses) >= 2 65 | 66 | np.random.seed(1234) # Use same seed for reproducibility 67 | prm.compress_path = True 68 | new_path = prm.plan(start, goal) 69 | assert len(new_path.poses) >= 2 70 | assert new_path.length < orig_path.length 71 | -------------------------------------------------------------------------------- /pyrobosim/test/navigation/test_world_graph_planner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Unit tests for the World Graph planner""" 4 | 5 | import os 6 | from pytest import LogCaptureFixture 7 | 8 | from pyrobosim.core import WorldYamlLoader 9 | from pyrobosim.navigation.world_graph import WorldGraphPlanner 10 | from pyrobosim.utils.general import get_data_folder 11 | from pyrobosim.utils.pose import Pose 12 | 13 | 14 | def test_world_graph_default() -> None: 15 | """Tests planning with default world graph planner settings.""" 16 | world = WorldYamlLoader().from_file( 17 | os.path.join(get_data_folder(), "test_world.yaml") 18 | ) 19 | planner = WorldGraphPlanner() 20 | world.robots[0].set_path_planner(planner) 21 | 22 | start = Pose(x=-1.6, y=2.8) 23 | goal = Pose(x=2.5, y=3.0) 24 | path = planner.plan(start, goal) 25 | 26 | assert len(path.poses) >= 2 27 | assert path.poses[0] == start 28 | assert path.poses[-1] == goal 29 | 30 | 31 | def test_world_graph_short_connection_distance(caplog: LogCaptureFixture) -> None: 32 | """Tests planning with short connection distance, which should fail.""" 33 | world = WorldYamlLoader().from_file( 34 | os.path.join(get_data_folder(), "test_world.yaml") 35 | ) 36 | planner_config = { 37 | "collision_check_step_dist": 0.025, 38 | "max_connection_dist": 1.0, 39 | } 40 | planner = WorldGraphPlanner(**planner_config) 41 | world.robots[0].set_path_planner(planner) 42 | 43 | start = Pose(x=-1.6, y=2.8) 44 | goal = Pose(x=2.5, y=3.0) 45 | path = planner.plan(start, goal) 46 | 47 | assert len(path.poses) == 0 48 | assert "Could not find a path from start to goal." in caplog.text 49 | -------------------------------------------------------------------------------- /pyrobosim/test/sensors/test_lidar.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Unit tests for lidar sensor.""" 4 | 5 | import os 6 | import numpy as np 7 | 8 | from pyrobosim.core import WorldYamlLoader 9 | from pyrobosim.sensors.lidar import Lidar2D 10 | from pyrobosim.utils.general import get_data_folder 11 | from pyrobosim.utils.pose import Pose 12 | 13 | 14 | def test_lidar_2d() -> None: 15 | # Setup a robot with a lidar sensor. 16 | world = WorldYamlLoader().from_file( 17 | os.path.join(get_data_folder(), "test_world.yaml") 18 | ) 19 | lidar = Lidar2D( 20 | update_rate_s=0.1, 21 | angle_units="degrees", 22 | min_angle=-120.0, 23 | max_angle=120.0, 24 | angular_resolution=5.0, 25 | max_range_m=2.0, 26 | ) 27 | robot = world.robots[0] 28 | robot.set_sensors({"lidar": lidar}) 29 | 30 | # Verify that the sensor is not active. 31 | latest_measurement = lidar.get_measurement() 32 | assert len(latest_measurement) == 0 33 | 34 | # Verify that the sensor returns values after being updated. 35 | lidar.update() 36 | latest_measurement = lidar.get_measurement() 37 | assert len(latest_measurement) == len(lidar.angles) 38 | assert np.all(latest_measurement <= lidar.max_range_m) 39 | -------------------------------------------------------------------------------- /pyrobosim/test/system/test_system_world.yaml: -------------------------------------------------------------------------------- 1 | ################################# 2 | # System test world description # 3 | ################################# 4 | 5 | # WORLD PARAMETERS 6 | params: 7 | name: test_system_world 8 | object_radius: 0.0375 # Radius around objects 9 | wall_height: 2.0 # Wall height for exporting to Gazebo 10 | 11 | 12 | # METADATA: Describes information about locations and objects 13 | metadata: 14 | locations: $DATA/example_location_data.yaml 15 | objects: $DATA/example_object_data.yaml 16 | 17 | 18 | # ROBOTS 19 | robots: 20 | - name: robot 21 | radius: 0.1 22 | location: kitchen 23 | pose: 24 | position: 25 | x: 0.0 26 | y: 0.0 27 | # Rapidly-expanding Random Tree (RRT) planner 28 | path_planner: 29 | type: rrt 30 | collision_check_step_dist: 0.025 31 | max_connection_dist: 0.25 32 | bidirectional: true 33 | rrt_star: true 34 | rewire_radius: 1.0 35 | compress_path: false 36 | # Linear motion path executor 37 | path_executor: 38 | type: constant_velocity 39 | linear_velocity: 5.0 40 | validate_during_execution: false 41 | # Grasp generation 42 | grasping: 43 | generator: parallel_grasp 44 | max_width: 0.175 45 | depth: 0.1 46 | height: 0.04 47 | width_clearance: 0.01 48 | depth_clearance: 0.01 49 | 50 | # ROOMS: Polygonal regions that can contain object locations 51 | rooms: 52 | - name: kitchen 53 | footprint: 54 | type: polygon 55 | coords: 56 | - [-1, -1] 57 | - [1.5, -1] 58 | - [1.5, 1.5] 59 | - [0.5, 1.5] 60 | nav_poses: 61 | - position: 62 | x: 0.75 63 | y: 0.5 64 | wall_width: 0.2 65 | color: [1, 0, 0] 66 | 67 | - name: bedroom 68 | footprint: 69 | type: box 70 | dims: [1.75, 1.5] 71 | pose: 72 | position: 73 | x: 2.625 74 | y: 3.25 75 | wall_width: 0.2 76 | color: [0, 0.6, 0] 77 | 78 | - name: bathroom 79 | footprint: 80 | type: polygon 81 | coords: 82 | - [-1, 1] 83 | - [-1, 3.5] 84 | - [-3, 3.5] 85 | - [-2.5, 1] 86 | wall_width: 0.2 87 | color: [0, 0, 0.6] 88 | 89 | 90 | # HALLWAYS: Connect rooms 91 | hallways: 92 | - room_start: kitchen 93 | room_end: bathroom 94 | width: 0.7 95 | conn_method: auto 96 | is_open: true 97 | is_locked: false 98 | 99 | - room_start: bathroom 100 | room_end: bedroom 101 | width: 0.5 102 | conn_method: angle 103 | conn_angle: 0.0 104 | offset: 0.8 105 | is_open: true 106 | is_locked: false 107 | 108 | - room_start: kitchen 109 | room_end: bedroom 110 | width: 0.6 111 | conn_method: points 112 | conn_points: 113 | - [1.0, 0.5] 114 | - [2.5, 0.5] 115 | - [2.5, 3.0] 116 | is_open: true 117 | is_locked: false 118 | 119 | 120 | # LOCATIONS: Can contain objects 121 | locations: 122 | - name: table0 123 | category: table 124 | parent: kitchen 125 | pose: 126 | position: 127 | x: 0.85 128 | y: -0.5 129 | rotation_eul: 130 | yaw: -1.57 131 | is_open: true 132 | is_locked: true 133 | 134 | - name: my_desk 135 | category: desk 136 | parent: bedroom 137 | pose: 138 | position: 139 | x: 0.525 140 | y: 0.4 141 | relative_to: bedroom 142 | is_open: true 143 | is_locked: false 144 | 145 | - name: counter0 146 | category: counter 147 | parent: bathroom 148 | pose: 149 | position: 150 | x: -2.45 151 | y: 2.5 152 | rotation_eul: 153 | yaw: 1.767 154 | is_open: true 155 | is_locked: true 156 | 157 | 158 | # OBJECTS: Can be picked, placed, and moved by robot 159 | objects: 160 | - category: apple 161 | parent: my_desk 162 | pose: 163 | position: 164 | x: 0.05 165 | y: -0.15 166 | relative_to: my_desk 167 | 168 | - name: gala 169 | category: apple 170 | parent: table0 171 | 172 | - category: water 173 | parent: counter0_left 174 | 175 | - category: banana 176 | parent: counter0_right 177 | 178 | - category: water 179 | parent: my_desk 180 | -------------------------------------------------------------------------------- /pyrobosim/test/test_pyrobosim.py: -------------------------------------------------------------------------------- 1 | """ 2 | Basic sanity checks for PyRoboSim module. 3 | """ 4 | 5 | import importlib 6 | from importlib.metadata import version 7 | 8 | 9 | def test_import() -> None: 10 | assert importlib.util.find_spec("pyrobosim") 11 | 12 | 13 | def test_version() -> None: 14 | ver = version("pyrobosim") 15 | assert ver == "4.0.0", "Incorrect pyrobosim version" 16 | -------------------------------------------------------------------------------- /pyrobosim/test/utils/test_general_utils.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | from matplotlib.colors import CSS4_COLORS, to_rgb 3 | 4 | from pyrobosim.utils.general import parse_color 5 | 6 | 7 | def test_parse_color() -> None: 8 | """Testing parse color with different input color formats""" 9 | # Test with RGB list 10 | color_rgb_list = (1.0, 0.0, 0.0) 11 | assert parse_color([1.0, 0.0, 0.0]) == color_rgb_list 12 | 13 | # Test with RGB tuple 14 | color_rgb_tuple = (0.0, 1.0, 0.0) 15 | assert parse_color((0.0, 1.0, 0.0)) == color_rgb_tuple 16 | 17 | # Test with named color 18 | color_by_name = "red" 19 | assert parse_color("red") == to_rgb(CSS4_COLORS[color_by_name]) 20 | 21 | # Test with hexadecimal color format 22 | color_hex = "#00FFFF" 23 | assert parse_color("#00FFFF") == to_rgb(color_hex) 24 | 25 | # Test with invalid RGB list 26 | with pytest.raises(ValueError) as exc_info: 27 | parse_color([1.0, 0.0]) 28 | assert ( 29 | str(exc_info.value) 30 | == "Incorrect number of elements. RGB color must have exactly 3 elements." 31 | ) 32 | 33 | # Test with invalid RGB tuple 34 | with pytest.raises(ValueError) as exc_info: 35 | parse_color((1.0, 0.0)) 36 | assert ( 37 | str(exc_info.value) 38 | == "Incorrect number of elements. RGB color must have exactly 3 elements." 39 | ) 40 | 41 | # Test with invalid named color 42 | with pytest.raises(ValueError) as exc_info: 43 | parse_color("notavalidcolor") 44 | assert ( 45 | str(exc_info.value) 46 | == "Invalid color name or hexadecimal value: notavalidcolor." 47 | ) 48 | 49 | # Test with invalid hexadecimal color format 50 | with pytest.raises(ValueError) as exc_info: 51 | parse_color("#ZZZ") 52 | assert str(exc_info.value) == "Invalid color name or hexadecimal value: #ZZZ." 53 | 54 | # Test with unsupported input type 55 | with pytest.raises(ValueError) as exc_info: 56 | parse_color(12345) 57 | assert ( 58 | str(exc_info.value) 59 | == "Unsupported input type. Expected a list, tuple, or string representing a color." 60 | ) 61 | -------------------------------------------------------------------------------- /pyrobosim_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(pyrobosim_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Enforce dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rclpy REQUIRED) 12 | find_package(rosidl_default_generators REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | 15 | # Generate custom interfaces 16 | set(msg_files 17 | "msg/ExecutionResult.msg" 18 | "msg/GoalPredicate.msg" 19 | "msg/GoalSpecification.msg" 20 | "msg/HallwayState.msg" 21 | "msg/LocationState.msg" 22 | "msg/ObjectState.msg" 23 | "msg/Path.msg" 24 | "msg/RobotState.msg" 25 | "msg/TaskAction.msg" 26 | "msg/TaskPlan.msg" 27 | "msg/WorldState.msg" 28 | ) 29 | 30 | set(srv_files 31 | "srv/RequestWorldState.srv" 32 | "srv/ResetWorld.srv" 33 | "srv/SetLocationState.srv" 34 | ) 35 | 36 | set(action_files 37 | "action/DetectObjects.action" 38 | "action/ExecuteTaskAction.action" 39 | "action/ExecuteTaskPlan.action" 40 | "action/FollowPath.action" 41 | "action/PlanPath.action" 42 | ) 43 | 44 | rosidl_generate_interfaces(${PROJECT_NAME} 45 | ${msg_files} 46 | ${srv_files} 47 | ${action_files} 48 | DEPENDENCIES geometry_msgs 49 | ) 50 | 51 | ament_package() 52 | -------------------------------------------------------------------------------- /pyrobosim_msgs/action/DetectObjects.action: -------------------------------------------------------------------------------- 1 | # ROS Action Definition for Object Detection 2 | 3 | # Goal 4 | # This can be a specific object name or an object category. 5 | string target_object 6 | 7 | --- 8 | 9 | # Result 10 | pyrobosim_msgs/ExecutionResult execution_result 11 | ObjectState[] detected_objects 12 | 13 | --- 14 | 15 | # No Feedback 16 | -------------------------------------------------------------------------------- /pyrobosim_msgs/action/ExecuteTaskAction.action: -------------------------------------------------------------------------------- 1 | # Execute Task Action -- ROS Action Definition 2 | 3 | # Goal 4 | pyrobosim_msgs/TaskAction action 5 | 6 | --- 7 | 8 | # Result 9 | pyrobosim_msgs/ExecutionResult execution_result 10 | 11 | --- 12 | 13 | # No Feedback 14 | -------------------------------------------------------------------------------- /pyrobosim_msgs/action/ExecuteTaskPlan.action: -------------------------------------------------------------------------------- 1 | # Execute Task Plan -- ROS Action Definition 2 | 3 | # Goal 4 | pyrobosim_msgs/TaskPlan plan 5 | 6 | --- 7 | 8 | # Result 9 | pyrobosim_msgs/ExecutionResult execution_result 10 | int64 num_completed 11 | int64 num_total 12 | 13 | --- 14 | 15 | # No Feedback 16 | -------------------------------------------------------------------------------- /pyrobosim_msgs/action/FollowPath.action: -------------------------------------------------------------------------------- 1 | # ROS Action Definition for Path Following 2 | 3 | # Goal 4 | pyrobosim_msgs/Path path 5 | 6 | --- 7 | 8 | # Result 9 | pyrobosim_msgs/ExecutionResult execution_result 10 | 11 | --- 12 | 13 | # No Feedback 14 | -------------------------------------------------------------------------------- /pyrobosim_msgs/action/PlanPath.action: -------------------------------------------------------------------------------- 1 | # ROS Action Definition for Path Planning 2 | 3 | # Goal 4 | string target_location 5 | geometry_msgs/Pose target_pose 6 | 7 | --- 8 | 9 | # Result 10 | pyrobosim_msgs/ExecutionResult execution_result 11 | pyrobosim_msgs/Path path 12 | 13 | --- 14 | 15 | # No Feedback 16 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/ExecutionResult.msg: -------------------------------------------------------------------------------- 1 | # Execution Result ROS Message 2 | 3 | ######################################## 4 | # Constant values for the status codes # 5 | ######################################## 6 | int32 UNKNOWN=-1 7 | 8 | # Action executed successfully. 9 | int32 SUCCESS=0 10 | 11 | # Preconditions not sufficient to execute the action. 12 | # For example, the action was to pick an object but there was no object visible. 13 | int32 PRECONDITION_FAILURE=1 14 | 15 | # Planning failed, for example a path planner or grasp planner did not produce a solution. 16 | int32 PLANNING_FAILURE=2 17 | 18 | # Preconditions were met and planning succeeded, but execution failed. 19 | int32 EXECUTION_FAILURE=3 20 | 21 | # Execution succeeded, but post-execution validation failed. 22 | int32 POSTCONDITION_FAILURE=4 23 | 24 | # Invalid action type. 25 | int32 INVALID_ACTION=5 26 | 27 | # The action was canceled by a user or upstream program. 28 | int32 CANCELED=6 29 | 30 | ################## 31 | # Message Fields # 32 | ################## 33 | # The status code. 34 | int32 status -1 35 | 36 | # A message describing the result. 37 | string message 38 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/GoalPredicate.msg: -------------------------------------------------------------------------------- 1 | # Goal Predicate ROS Message 2 | 3 | # Predicate type 4 | string type 5 | 6 | # Predicate arguments 7 | string[] args 8 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/GoalSpecification.msg: -------------------------------------------------------------------------------- 1 | # Goal Specification ROS Message 2 | # A goal specification consists of a list of predicates. 3 | 4 | pyrobosim_msgs/GoalPredicate[] predicates 5 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/HallwayState.msg: -------------------------------------------------------------------------------- 1 | # Hallway state definition message 2 | 3 | # Fixed data 4 | string name 5 | string room_start 6 | string room_end 7 | 8 | # Dynamic data 9 | bool is_open 10 | bool is_locked 11 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/LocationState.msg: -------------------------------------------------------------------------------- 1 | # Location state definition message 2 | 3 | # Fixed data 4 | string name 5 | string category 6 | 7 | # Dynamic data 8 | string parent 9 | geometry_msgs/Pose pose 10 | bool is_open 11 | bool is_locked 12 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/ObjectState.msg: -------------------------------------------------------------------------------- 1 | # Object state definition message 2 | 3 | # Fixed data 4 | string name 5 | string category 6 | 7 | # Dynamic data 8 | string parent 9 | geometry_msgs/Pose pose 10 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/Path.msg: -------------------------------------------------------------------------------- 1 | # Path ROS Message 2 | 3 | geometry_msgs/Pose[] poses 4 | float64 length 5 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/RobotState.msg: -------------------------------------------------------------------------------- 1 | # Robot state definition message 2 | 3 | # Header containing a timestamp 4 | std_msgs/Header header 5 | 6 | # Robot information 7 | string name 8 | 9 | # Continuous state 10 | geometry_msgs/Pose pose 11 | float64 battery_level 12 | 13 | # Discrete state 14 | bool executing_action 15 | bool holding_object 16 | string manipulated_object 17 | string last_visited_location 18 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/TaskAction.msg: -------------------------------------------------------------------------------- 1 | # Task Action ROS Message 2 | 3 | # Main action information 4 | string robot 5 | string type 6 | string object 7 | string room 8 | string source_location 9 | string target_location 10 | 11 | # Action cost (from the output of a planner) 12 | float32 cost 13 | 14 | # Other parameters 15 | bool has_pose 16 | geometry_msgs/Pose pose 17 | pyrobosim_msgs/Path path 18 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/TaskPlan.msg: -------------------------------------------------------------------------------- 1 | # Task Plan ROS Message 2 | 3 | string robot 4 | pyrobosim_msgs/TaskAction[] actions 5 | float32 cost 6 | -------------------------------------------------------------------------------- /pyrobosim_msgs/msg/WorldState.msg: -------------------------------------------------------------------------------- 1 | # World state definition message 2 | 3 | RobotState[] robots 4 | LocationState[] locations 5 | HallwayState[] hallways 6 | ObjectState[] objects 7 | -------------------------------------------------------------------------------- /pyrobosim_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pyrobosim_msgs 5 | 4.0.0 6 | ROS interface definitions for PyRoboSim. 7 | Sebastian Castro 8 | MIT 9 | 10 | 11 | ament_cmake 12 | rosidl_default_generators 13 | 14 | 15 | rclcpp 16 | rclpy 17 | rosidl_default_runtime 18 | geometry_msgs 19 | 20 | rosidl_interface_packages 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /pyrobosim_msgs/srv/RequestWorldState.srv: -------------------------------------------------------------------------------- 1 | # ROS service to request the world state 2 | 3 | # Optional robot name. 4 | # If specified, gets the known world state of that robot. 5 | # If not specified, gets the full world state. 6 | string robot 7 | 8 | --- 9 | 10 | # The world state 11 | WorldState state 12 | -------------------------------------------------------------------------------- /pyrobosim_msgs/srv/ResetWorld.srv: -------------------------------------------------------------------------------- 1 | # ROS service to reset the world state. 2 | 3 | # Request 4 | # If true, resets the world to exactly the state when it was first loading. 5 | # Otherwise, if loaded from YAML, randomly sampled positions will change. 6 | bool deterministic 7 | 8 | --- 9 | 10 | # Response 11 | # Indicates whether the world reset operation was successful: 12 | bool success 13 | -------------------------------------------------------------------------------- /pyrobosim_msgs/srv/SetLocationState.srv: -------------------------------------------------------------------------------- 1 | # ROS service to set the state of a location 2 | 3 | # Request 4 | string location_name 5 | bool open 6 | bool lock 7 | 8 | --- 9 | # Response 10 | ExecutionResult result 11 | -------------------------------------------------------------------------------- /pyrobosim_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(pyrobosim_ros) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # Enforce dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(rclpy REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(pyrobosim_msgs REQUIRED) 15 | find_package(std_srvs REQUIRED) 16 | 17 | # Install examples 18 | install(PROGRAMS 19 | examples/demo.py 20 | examples/demo_commands.py 21 | examples/demo_pddl_world.py 22 | examples/demo_pddl_planner.py 23 | examples/demo_pddl_goal_publisher.py 24 | examples/demo_velocity_publisher.py 25 | DESTINATION lib/${PROJECT_NAME} 26 | ) 27 | 28 | # Install pyrobosim_ros Python package 29 | ament_python_install_package( 30 | pyrobosim_ros 31 | PACKAGE_DIR pyrobosim_ros) 32 | 33 | # Install launch files 34 | install(DIRECTORY 35 | launch 36 | DESTINATION share/${PROJECT_NAME} 37 | ) 38 | 39 | # Build tests if enabled 40 | if(BUILD_TESTING) 41 | add_subdirectory(test) 42 | endif() 43 | 44 | ament_package() 45 | -------------------------------------------------------------------------------- /pyrobosim_ros/examples/demo_pddl_goal_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Example showing how to publish a goal specification to a PDDLStream planner node. 5 | """ 6 | 7 | import rclpy 8 | from rclpy.node import Node 9 | import time 10 | 11 | from pyrobosim_msgs.msg import GoalPredicate, GoalSpecification # type: ignore 12 | from pyrobosim_msgs.srv import SetLocationState # type: ignore 13 | 14 | 15 | class GoalPublisher(Node): # type: ignore[misc] 16 | def __init__(self) -> None: 17 | super().__init__("demo_pddlstream_goal_publisher") 18 | 19 | # Declare parameters 20 | self.declare_parameter("example", value="01_simple") 21 | self.declare_parameter("verbose", value=True) 22 | 23 | # Create a service to set the location state (for the open/close/detect example) 24 | self.set_location_state_client = self.create_client( 25 | SetLocationState, "set_location_state" 26 | ) 27 | 28 | # Publisher for a goal specification 29 | self.goalspec_pub = self.create_publisher( 30 | GoalSpecification, "goal_specification", 10 31 | ) 32 | self.get_logger().info("Waiting for subscription") 33 | while rclpy.ok() and self.goalspec_pub.get_subscription_count() < 1: 34 | time.sleep(2.0) 35 | if self.goalspec_pub.get_subscription_count() < 1: 36 | self.get_logger().error( 37 | "Error while waiting for a goal specification subscriber." 38 | ) 39 | return 40 | 41 | # Create goal specifications for different examples 42 | example = self.get_parameter("example").value 43 | if example == "01_simple": 44 | # Goal specification for simple example. 45 | goal_predicates = [ 46 | GoalPredicate(type="At", args=("robot", "bedroom")), 47 | GoalPredicate(type="At", args=("apple0", "table0_tabletop")), 48 | GoalPredicate(type="At", args=("banana0", "counter0_left")), 49 | GoalPredicate(type="Holding", args=("robot", "water0")), 50 | ] 51 | elif example in [ 52 | "02_derived", 53 | "03_nav_stream", 54 | "04_nav_manip_stream", 55 | "05_nav_grasp_stream", 56 | "06_open_close_detect", 57 | ]: 58 | # Goal specification for derived predicate example. 59 | goal_predicates = [ 60 | GoalPredicate(type="Has", args=("desk0_desktop", "banana0")), 61 | GoalPredicate(type="Has", args=("counter", "apple1")), 62 | GoalPredicate(type="HasNone", args=("bathroom", "banana")), 63 | GoalPredicate(type="HasAll", args=("table", "water")), 64 | ] 65 | # If running the open/close/detect example, close the desk location. 66 | if example == "06_open_close_detect": 67 | future = self.set_location_state_client.call_async( 68 | SetLocationState.Request( 69 | location_name="desk0", 70 | open=False, 71 | lock=False, 72 | ) 73 | ) 74 | start_time = time.time() 75 | while not future.done(): 76 | rclpy.spin_once(self, timeout_sec=0.1) 77 | if time.time() - start_time > 2.0: 78 | raise TimeoutError("Failed to close location before planning.") 79 | else: 80 | self.get_logger().info(f"Invalid example: {example}") 81 | return 82 | 83 | # Publish and optionally display the goal specification message. 84 | goal_msg = GoalSpecification(predicates=goal_predicates) 85 | if self.get_parameter("verbose").value == True: 86 | msg = "Published goal specification:" 87 | for pred in goal_msg.predicates: 88 | msg += f"\n{pred}" 89 | self.get_logger().info(msg) 90 | self.goalspec_pub.publish(goal_msg) 91 | 92 | 93 | def main() -> None: 94 | rclpy.init() 95 | goal_node = GoalPublisher() 96 | 97 | rclpy.spin(goal_node) 98 | goal_node.destroy_node() 99 | rclpy.shutdown() 100 | 101 | 102 | if __name__ == "__main__": 103 | main() 104 | -------------------------------------------------------------------------------- /pyrobosim_ros/examples/demo_pddl_world.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Example showing how to start a PyRoboSim world that receives a plan from a 5 | Task and Motion Planner such as PDDLStream. 6 | """ 7 | 8 | import os 9 | import rclpy 10 | import threading 11 | 12 | from pyrobosim.core import World, WorldYamlLoader 13 | from pyrobosim.gui import start_gui 14 | from pyrobosim.utils.general import get_data_folder 15 | from pyrobosim_ros.ros_interface import WorldROSWrapper 16 | 17 | 18 | def load_world() -> World: 19 | """Load a test world.""" 20 | world_file = os.path.join(get_data_folder(), "pddlstream_simple_world.yaml") 21 | return WorldYamlLoader().from_file(world_file) 22 | 23 | 24 | def create_ros_node() -> WorldROSWrapper: 25 | """Initializes ROS node""" 26 | rclpy.init() 27 | world = load_world() 28 | node = WorldROSWrapper(world=world, name="pddl_demo", state_pub_rate=0.1) 29 | return node 30 | 31 | 32 | if __name__ == "__main__": 33 | node = create_ros_node() 34 | 35 | # Start ROS Node in separate thread 36 | ros_thread = threading.Thread(target=lambda: node.start(wait_for_gui=True)) 37 | ros_thread.start() 38 | 39 | # Start GUI in main thread 40 | start_gui(node.world) 41 | -------------------------------------------------------------------------------- /pyrobosim_ros/examples/demo_velocity_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | Example showing how to publish velocity commands to a PyRoboSim robot. 5 | """ 6 | 7 | import rclpy 8 | from rclpy.node import Node 9 | import time 10 | 11 | from geometry_msgs.msg import Twist 12 | 13 | 14 | class VelocityPublisher(Node): # type: ignore[misc] 15 | def __init__(self) -> None: 16 | super().__init__("demo_velocity_publisher") 17 | 18 | # Declare parameters 19 | self.declare_parameter("robot_name", value="robot") 20 | self.declare_parameter("pub_period", value=0.1) 21 | self.declare_parameter("lin_vel", value=0.2) 22 | self.declare_parameter("ang_vel", value=0.5) 23 | 24 | # Publisher for velocity commands 25 | robot_name = self.get_parameter("robot_name").value 26 | self.vel_pub = self.create_publisher(Twist, f"{robot_name}/cmd_vel", 10) 27 | self.get_logger().info("Waiting for subscription") 28 | while rclpy.ok() and self.vel_pub.get_subscription_count() < 1: 29 | time.sleep(2.0) 30 | if self.vel_pub.get_subscription_count() < 1: 31 | self.get_logger().error( 32 | f"Error while waiting for a velocity command subscriber to {self.vel_pub.topic_name}." 33 | ) 34 | return 35 | 36 | # Create a publisher timer 37 | pub_period = self.get_parameter("pub_period").value 38 | self.timer = self.create_timer(pub_period, self.vel_pub_callback) 39 | 40 | def vel_pub_callback(self) -> None: 41 | """Publisher callback for velocity commands.""" 42 | lin_vel = self.get_parameter("lin_vel").value 43 | ang_vel = self.get_parameter("ang_vel").value 44 | 45 | pub_cmd = Twist() 46 | pub_cmd.linear.x = lin_vel 47 | pub_cmd.angular.z = ang_vel 48 | self.vel_pub.publish(pub_cmd) 49 | 50 | 51 | def main() -> None: 52 | rclpy.init() 53 | pub_node = VelocityPublisher() 54 | rclpy.spin(pub_node) 55 | pub_node.destroy_node() 56 | rclpy.shutdown() 57 | 58 | 59 | if __name__ == "__main__": 60 | main() 61 | -------------------------------------------------------------------------------- /pyrobosim_ros/launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description() -> LaunchDescription: 8 | # Arguments 9 | world_file_arg = DeclareLaunchArgument( 10 | "world_file", 11 | default_value="", 12 | description="YAML file name (should be in the pyrobosim/data folder). " 13 | + "If not specified, a world will be created programmatically.", 14 | ) 15 | 16 | # Nodes 17 | demo_node = Node( 18 | package="pyrobosim_ros", 19 | executable="demo.py", 20 | name="demo", 21 | parameters=[{"world_file": LaunchConfiguration("world_file")}], 22 | ) 23 | 24 | return LaunchDescription([world_file_arg, demo_node]) 25 | -------------------------------------------------------------------------------- /pyrobosim_ros/launch/demo_commands.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description() -> LaunchDescription: 8 | # Arguments 9 | world_file_arg = DeclareLaunchArgument( 10 | "world_file", 11 | default_value="", 12 | description="YAML file name (should be in the pyrobosim/data folder). " 13 | + "If not specified, a world will be created programmatically.", 14 | ) 15 | mode_arg = DeclareLaunchArgument( 16 | "mode", 17 | default_value="plan", 18 | description="Command mode (action or plan)", 19 | ) 20 | action_delay_arg = DeclareLaunchArgument( 21 | "action_delay", 22 | default_value="0.0", 23 | description="The action delay, in seconds", 24 | ) 25 | action_success_probability_arg = DeclareLaunchArgument( 26 | "action_success_probability", 27 | default_value="1.0", 28 | description="The action success probability, in the range (0, 1)", 29 | ) 30 | action_rng_seed_arg = DeclareLaunchArgument( 31 | "action_rng_seed", 32 | default_value="-1", 33 | description="The random number generator seed. Defaults to -1, or nondeterministic.", 34 | ) 35 | send_cancel_arg = DeclareLaunchArgument( 36 | "send_cancel", 37 | default_value="False", 38 | description="If True, cancels running actions after some time.", 39 | ) 40 | 41 | # Nodes 42 | world_node = Node( 43 | package="pyrobosim_ros", 44 | executable="demo.py", 45 | name="demo_world", 46 | parameters=[{"world_file": LaunchConfiguration("world_file")}], 47 | ) 48 | command_node = Node( 49 | package="pyrobosim_ros", 50 | executable="demo_commands.py", 51 | name="demo_commands", 52 | parameters=[ 53 | { 54 | "mode": LaunchConfiguration("mode"), 55 | "action_delay": LaunchConfiguration("action_delay"), 56 | "action_success_probability": LaunchConfiguration( 57 | "action_success_probability" 58 | ), 59 | "action_rng_seed": LaunchConfiguration("action_rng_seed"), 60 | "send_cancel": LaunchConfiguration("send_cancel"), 61 | } 62 | ], 63 | ) 64 | 65 | return LaunchDescription( 66 | [ 67 | world_file_arg, 68 | mode_arg, 69 | action_delay_arg, 70 | action_success_probability_arg, 71 | action_rng_seed_arg, 72 | send_cancel_arg, 73 | world_node, 74 | command_node, 75 | ] 76 | ) 77 | -------------------------------------------------------------------------------- /pyrobosim_ros/launch/demo_commands_multirobot.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | 5 | def generate_launch_description() -> LaunchDescription: 6 | # Nodes 7 | world_node = Node( 8 | package="pyrobosim_ros", 9 | executable="demo.py", 10 | name="demo_world", 11 | parameters=[ 12 | { 13 | # Use multirobot file option. 14 | "world_file": "test_world_multirobot.yaml" 15 | } 16 | ], 17 | output="screen", 18 | emulate_tty=True, 19 | ) 20 | command_node = Node( 21 | package="pyrobosim_ros", 22 | executable="demo_commands.py", 23 | name="demo_commands", 24 | parameters=[ 25 | { 26 | # Use multirobot plan mode option. 27 | "mode": "multirobot-plan" 28 | } 29 | ], 30 | ) 31 | 32 | return LaunchDescription([world_node, command_node]) 33 | -------------------------------------------------------------------------------- /pyrobosim_ros/launch/demo_pddl.launch.py: -------------------------------------------------------------------------------- 1 | from typing import Any 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 4 | from launch.conditions import IfCondition 5 | from launch.launch_context import LaunchContext 6 | from launch.substitutions import LaunchConfiguration 7 | from launch.substitutions import TextSubstitution 8 | from launch_ros.actions import Node 9 | 10 | 11 | def launch_planner_node( 12 | context: LaunchContext, *args: Any, **kwargs: Any 13 | ) -> list[Node]: 14 | verbose_bool = LaunchConfiguration("verbose").perform(context).lower() == "true" 15 | planner_node = Node( 16 | package="pyrobosim_ros", 17 | executable="demo_pddl_planner.py", 18 | name="pddl_demo_planner", 19 | parameters=[ 20 | { 21 | "example": LaunchConfiguration("example"), 22 | "subscribe": LaunchConfiguration("subscribe"), 23 | "verbose": LaunchConfiguration("verbose"), 24 | "search_sample_ratio": LaunchConfiguration("search_sample_ratio"), 25 | } 26 | ], 27 | output="screen", 28 | emulate_tty=verbose_bool, 29 | ) 30 | return [planner_node] 31 | 32 | 33 | def generate_launch_description() -> LaunchDescription: 34 | # Arguments 35 | example_arg = DeclareLaunchArgument( 36 | "example", 37 | default_value=TextSubstitution(text="01_simple"), 38 | description="Example name, must be one of " 39 | + "(01_simple, 02_derived, 03_nav_stream, 04_nav_manip_stream, 05_nav_grasp_stream, 06_open_close_detect)", 40 | ) 41 | verbose_arg = DeclareLaunchArgument( 42 | "verbose", 43 | default_value=TextSubstitution(text="true"), 44 | description="Print planner output (true/false)", 45 | ) 46 | subscribe_arg = DeclareLaunchArgument( 47 | "subscribe", 48 | default_value=TextSubstitution(text="true"), 49 | description="If true, waits to receive goal on a subscriber.", 50 | ) 51 | search_sample_ratio_arg = DeclareLaunchArgument( 52 | "search_sample_ratio", 53 | default_value=TextSubstitution(text="0.5"), 54 | description="Search to sample ratio for planner", 55 | ) 56 | 57 | # Nodes 58 | world_node = Node( 59 | package="pyrobosim_ros", 60 | executable="demo_pddl_world.py", 61 | name="pddl_demo", 62 | ) 63 | planner_node = OpaqueFunction(function=launch_planner_node) 64 | goalspec_node = Node( 65 | package="pyrobosim_ros", 66 | executable="demo_pddl_goal_publisher.py", 67 | name="pddl_demo_goal_publisher", 68 | parameters=[ 69 | { 70 | "example": LaunchConfiguration("example"), 71 | "verbose": LaunchConfiguration("verbose"), 72 | } 73 | ], 74 | condition=IfCondition(LaunchConfiguration("subscribe")), 75 | ) 76 | 77 | return LaunchDescription( 78 | [ 79 | example_arg, 80 | verbose_arg, 81 | subscribe_arg, 82 | search_sample_ratio_arg, 83 | world_node, 84 | planner_node, 85 | goalspec_node, 86 | ] 87 | ) 88 | -------------------------------------------------------------------------------- /pyrobosim_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pyrobosim_ros 5 | 4.0.0 6 | ROS 2 wrapper around PyRoboSim. 7 | Sebastian Castro 8 | MIT 9 | 10 | 11 | ament_cmake 12 | ament_cmake_python 13 | 14 | 15 | geometry_msgs 16 | rclcpp 17 | rclpy 18 | ros2launch 19 | pyrobosim_msgs 20 | std_srvs 21 | 22 | 23 | ament_cmake_pytest 24 | ament_copyright 25 | ament_flake8 26 | ament_pep257 27 | python3-pytest 28 | python3-pytest-cov 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /pyrobosim_ros/pyrobosim_ros/__init__.py: -------------------------------------------------------------------------------- 1 | """ROS 2 interface to PyRoboSim. 2 | 3 | This module contains tools for using PyRoboSim with ROS 2, including interfaces 4 | to world and robot models, as well as general conversion utilities. 5 | """ 6 | -------------------------------------------------------------------------------- /pyrobosim_ros/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | 4 | project_name = "pyrobosim_ros" 5 | 6 | install_requires = [ 7 | "pyrobosim", 8 | ] 9 | 10 | setup( 11 | name=project_name, 12 | version="4.0.0", 13 | url="https://github.com/sea-bass/pyrobosim", 14 | author="Sebastian Castro", 15 | author_email="sebas.a.castro@gmail.com", 16 | description="ROS 2 interface to PyRoboSim.", 17 | license="MIT", 18 | install_requires=install_requires, 19 | packages=find_packages(), 20 | zip_safe=True, 21 | ) 22 | -------------------------------------------------------------------------------- /pyrobosim_ros/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMakeLists.txt for pyrobosim_ros tests 2 | find_package(ament_cmake_pytest REQUIRED) 3 | 4 | set(AMENT_CMAKE_PYTEST_WITH_COVERAGE ON) 5 | 6 | set(_pytest_tests 7 | test_ros_conversions.py 8 | test_ros_interface.py 9 | ) 10 | foreach(_test_path ${_pytest_tests}) 11 | get_filename_component(_test_name ${_test_path} NAME_WE) 12 | ament_add_pytest_test(${_test_name} ${_test_path} 13 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 14 | TIMEOUT 60 15 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 16 | ) 17 | endforeach() 18 | -------------------------------------------------------------------------------- /pytest.ini: -------------------------------------------------------------------------------- 1 | [pytest] 2 | addopts = --ignore=dependencies 3 | -------------------------------------------------------------------------------- /setup/configure_pddlstream.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Set up PDDLStream 4 | 5 | # Set up the dependencies folder 6 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 7 | DEPENDS_DIR=${SCRIPT_DIR}/../dependencies 8 | if [ ! -d "${DEPENDS_DIR}" ] 9 | then 10 | mkdir ${DEPENDS_DIR} 11 | fi 12 | 13 | # Clone and build PDDLStream 14 | pushd ${SCRIPT_DIR}/../dependencies > /dev/null || exit 15 | git clone https://github.com/caelan/pddlstream.git 16 | pushd pddlstream > /dev/null || exit 17 | touch COLCON_IGNORE 18 | git submodule update --init --recursive 19 | ./downward/build.py 20 | popd > /dev/null || exit 21 | popd > /dev/null || exit 22 | -------------------------------------------------------------------------------- /setup/setup_pyrobosim.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Sets up the PyRoboSim virtual environment 4 | 5 | # User variables 6 | # Please modify these for your environment 7 | VIRTUALENV_FOLDER=~/python-virtualenvs/pyrobosim 8 | 9 | # Create a Python virtual environment 10 | [ ! -d "${VIRTUALENV_FOLDER}" ] && mkdir -p ${VIRTUALENV_FOLDER} 11 | python3 -m venv ${VIRTUALENV_FOLDER} 12 | echo -e "Created Python virtual environment in ${VIRTUALENV_FOLDER}\n" 13 | 14 | # Install all the Python dependencies required 15 | # Note that these overlay over whatever ROS 2 already contains 16 | source "${VIRTUALENV_FOLDER}/bin/activate" 17 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 18 | pushd "${SCRIPT_DIR}/.." > /dev/null 19 | pip3 install setuptools 20 | python3 pyrobosim/setup.py egg_info 21 | pip3 install -r pyrobosim.egg-info/requires.txt 22 | rm -rf pyrobosim.egg-info/ 23 | pip3 install -r test/python_test_requirements.txt 24 | 25 | # Write key variables to file 26 | ENV_FILE="pyrobosim.env" 27 | if [ -f "${ENV_FILE}" ] 28 | then 29 | rm ${ENV_FILE} 30 | fi 31 | echo "# This is an autogenerated environment file for PyRoboSim" >> ${ENV_FILE} 32 | echo "PYROBOSIM_VENV=${VIRTUALENV_FOLDER}" >> ${ENV_FILE} 33 | 34 | # If setting up with ROS, perform additional setup. 35 | echo -e "" 36 | read -p "Do you want to set up ROS? (y/n) : " USE_ROS 37 | if [ "${USE_ROS,,}" == "y" ]; then 38 | # Attempt to auto-detect by going up the tree and looking for a src/ folder 39 | SEARCH_DIR="$(dirname "$(dirname "$(dirname "$SCRIPT_DIR")")")" 40 | while [ "$SEARCH_DIR" != "/" ]; do 41 | if [ -d "${SEARCH_DIR}/src" ]; then 42 | ROS_WORKSPACE="${SEARCH_DIR}" 43 | echo "[INFO] Found ROS workspace at: ${ROS_WORKSPACE}" 44 | break 45 | fi 46 | SEARCH_DIR="$(dirname "${SEARCH_DIR}")" 47 | done 48 | 49 | # If not found, ask the user to input the path manually 50 | if [ -z "${ROS_WORKSPACE}" ]; then 51 | echo -e "\n[WARN] Could not auto-detect a valid ROS workspace." 52 | read -p "Please enter the path to your ROS workspace manually: " USER_INPUT_PATH 53 | MATCHED_DIR=$(find "${USER_INPUT_PATH}/src" -type d -name "pyrobosim" 2>/dev/null | head -n 1) 54 | if [ -d "${USER_INPUT_PATH}/src" ] && [ -n "${MATCHED_DIR}" ]; then 55 | ROS_WORKSPACE="${USER_INPUT_PATH}" 56 | echo "[INFO] Using manually provided ROS workspace: ${ROS_WORKSPACE}" 57 | else 58 | # If no valid workspace found, fail 59 | echo -e "\n[ERROR] The path provided is not valid." 60 | echo "[INFO] Either:" 61 | echo " - The path was written incorrectly or with special characters (e.g., ~/path/to/my_ws)" 62 | echo " - The path does not follow the ROS workspace structure (e.g., my_ws/src), in which case it's not a valid ROS workspace for building" 63 | echo " - The src directory does not include PyRoboSim" 64 | type deactivate &> /dev/null && deactivate 65 | exit 1 66 | fi 67 | fi 68 | 69 | rm -rf ${ROS_WORKSPACE}/build ${ROS_WORKSPACE}/install ${ROS_WORKSPACE}/log 70 | 71 | read -p "What ROS distro are you using? (humble, jazzy, kilted, rolling) : " ROS_DISTRO 72 | echo "" 73 | echo "Installing additional packages for ROS ${ROS_DISTRO,,} setup" 74 | echo "PYROBOSIM_ROS_WORKSPACE=${ROS_WORKSPACE}" >> ${ENV_FILE} 75 | echo "PYROBOSIM_ROS_DISTRO=${ROS_DISTRO,,}" >> ${ENV_FILE} 76 | 77 | # Install packages needed to run colcon build and use rclpy from within our virtual environment. 78 | pip3 install colcon-common-extensions typing_extensions 79 | 80 | # Install any ROS package dependencies that may be missing. 81 | pushd ${ROS_WORKSPACE} > /dev/null 82 | rosdep install --from-paths src -y --ignore-src --rosdistro ${ROS_DISTRO} 83 | popd 84 | else 85 | # Install PyRoboSim using pip in the non-ROS case. 86 | pip3 install -e ./pyrobosim 87 | fi 88 | 89 | # Optionally configure PDDLStream for task and motion planning 90 | echo -e "" 91 | read -p "Do you want to set up PDDLStream for task and motion planning? (y/n) : " USE_PDDLSTREAM 92 | if [ "${USE_PDDLSTREAM,,}" == "y" ] 93 | then 94 | ./setup/configure_pddlstream.bash 95 | fi 96 | 97 | popd > /dev/null 98 | deactivate 99 | 100 | # Print confirmation and instructions at the end 101 | echo -e " 102 | 103 | 104 | Created Python virtual environment and installed packages at the following location: 105 | 106 | ${VIRTUALENV_FOLDER} 107 | 108 | Environment data has been written to the file ${ENV_FILE}. 109 | 110 | Source the environment using the following command: 111 | 112 | source setup/source_pyrobosim.bash 113 | " 114 | -------------------------------------------------------------------------------- /setup/source_pyrobosim.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Sets up the PyRoboSim environment for development. 4 | # 5 | # One recommendation is to make a bash function for this script in 6 | # your ~/.bashrc file as follows: 7 | # 8 | # For non-ROS workflows: 9 | # 10 | # pyrobosim() { 11 | # source /path/to/pyrobosim/setup/source_pyrobosim.bash 12 | # } 13 | # 14 | # So you can then run this from your Terminal: 15 | # pyrobosim 16 | # 17 | 18 | # Set up the environment 19 | ENV_FILE="pyrobosim.env" 20 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 21 | pushd "${SCRIPT_DIR}/.." > /dev/null 22 | if [ ! -f "${ENV_FILE}" ] 23 | then 24 | popd > /dev/null 25 | echo "Did not find a file named '${ENV_FILE}' in the root pyrobosim folder." 26 | echo "Please rerun the 'setup_pyrobosim.bash' script to set up your environment." 27 | return 1 28 | fi 29 | unset PYROBOSIM_VENV PYROBOSIM_ROS_WORKSPACE PYROBOSIM_ROS_DISTRO 30 | source "${ENV_FILE}" 31 | popd > /dev/null 32 | 33 | if [ -n "${VIRTUAL_ENV}" ] 34 | then 35 | deactivate 36 | fi 37 | 38 | # Activate the Python virtual environment. 39 | echo "Activated virtual environment at ${PYROBOSIM_VENV}." 40 | source ${PYROBOSIM_VENV}/bin/activate 41 | 42 | # Parse ROS workspace and distro arguments. 43 | if [ -z "${PYROBOSIM_ROS_WORKSPACE}" ] 44 | then 45 | echo "Setting up PyRoboSim with no ROS distro." 46 | else 47 | echo "Setting up PyRoboSim with ROS ${PYROBOSIM_ROS_DISTRO}." 48 | source /opt/ros/${PYROBOSIM_ROS_DISTRO}/setup.bash 49 | 50 | if [ ! -d "${PYROBOSIM_ROS_WORKSPACE}/src" ] 51 | then 52 | echo -e "\nFolder '${PYROBOSIM_ROS_WORKSPACE}/src' does not exist." 53 | echo "Please rerun the 'setup_pyrobosim.bash' script to set up your environment." 54 | return 1 55 | fi 56 | 57 | pushd "${PYROBOSIM_ROS_WORKSPACE}" > /dev/null 58 | if [ ! -f "install/setup.bash" ] 59 | then 60 | echo "Building ROS workspace at ${PYROBOSIM_ROS_WORKSPACE}..." 61 | colcon build --symlink-install 62 | fi 63 | echo "Sourcing ROS workspace at ${PYROBOSIM_ROS_WORKSPACE}." 64 | source install/setup.bash 65 | popd > /dev/null 66 | fi 67 | 68 | # Add dependencies to path 69 | PDDLSTREAM_PATH=${SCRIPT_DIR}/../dependencies/pddlstream 70 | if [ -d "${PDDLSTREAM_PATH}" ] 71 | then 72 | echo "Added PDDLStream to Python path." 73 | export PYTHONPATH=${PDDLSTREAM_PATH}:$PYTHONPATH 74 | fi 75 | -------------------------------------------------------------------------------- /test/python_test_requirements.txt: -------------------------------------------------------------------------------- 1 | lark 2 | py 3 | pytest 4 | pytest-cov 5 | pytest-dependency 6 | pytest-html 7 | setuptools>=78.1.1 8 | -------------------------------------------------------------------------------- /test/run_tests.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Runs all unit tests 4 | # 5 | # If not using ROS, simply run 6 | # ./run_tests.bash 7 | # 8 | # If using ROS, additionally pass in your ROS_DISTRO argument (e.g., humble or rolling) 9 | # ./run_tests.bash ${ROS_DISTRO} 10 | 11 | SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 12 | TEST_RESULTS_DIR="${SCRIPT_DIR}/results" 13 | mkdir -p "${TEST_RESULTS_DIR}" 14 | 15 | # Ensure we run everything for coverage purposes, but ensure failures are returned by the script 16 | SUCCESS=0 17 | 18 | # Do not swallow errors with tee 19 | set -o pipefail 20 | 21 | # Run regular pytest tests 22 | echo "Running Python package unit tests..." 23 | pushd "${SCRIPT_DIR}/../pyrobosim" || exit 24 | python3 -m pytest . \ 25 | --cov="pyrobosim" --cov-branch \ 26 | --cov-report term \ 27 | --cov-report html:"${TEST_RESULTS_DIR}/test_results_coverage_html" \ 28 | --cov-report xml:"${TEST_RESULTS_DIR}/test_results_coverage.xml" \ 29 | --junitxml="${TEST_RESULTS_DIR}/test_results.xml" \ 30 | --html="${TEST_RESULTS_DIR}/test_results.html" \ 31 | --self-contained-html \ 32 | | tee "${TEST_RESULTS_DIR}/pytest-coverage.txt" || SUCCESS=$? 33 | echo "" 34 | popd || exit 35 | 36 | # Run ROS package tests, if using a ROS distro. 37 | ROS_DISTRO=$1 38 | if [[ -n "${ROS_DISTRO}" && -n "${COLCON_PREFIX_PATH}" ]] 39 | then 40 | WORKSPACE_DIR="${COLCON_PREFIX_PATH}/../" 41 | echo "Running ROS package unit tests from ${WORKSPACE_DIR}..." 42 | pushd "${WORKSPACE_DIR}" > /dev/null || exit 43 | colcon test \ 44 | --packages-select pyrobosim_ros \ 45 | --event-handlers console_cohesion+ \ 46 | --return-code-on-test-failure \ 47 | --pytest-with-coverage || SUCCESS=$? 48 | echo "" 49 | colcon test-result --verbose \ 50 | | tee "${TEST_RESULTS_DIR}/test_results_ros.xml" 51 | popd > /dev/null || exit 52 | fi 53 | 54 | exit ${SUCCESS} 55 | --------------------------------------------------------------------------------