From c9e5a8d86772f92f468daa52ef29c5f34a513ebd Mon Sep 17 00:00:00 2001 From: Michael Lu Date: Mon, 21 Oct 2024 13:29:05 -0700 Subject: [PATCH 1/3] init commit --- .devcontainer/Dockerfile | 17 ++ .devcontainer/devcontainer.json | 48 ++++ .devcontainer/repos_to_submodules.py | 38 +++ .env | 1 + .gitignore | 4 + .vscode/c_cpp_properties.json | 18 ++ .vscode/launch.json | 90 +++++++ .vscode/settings.json | 75 ++++++ .vscode/tasks.json | 254 ++++++++++++++++++ LICENSE | 201 ++++++++++++++ README.md | 33 +++ build.sh | 10 + setup.sh | 7 + src/controller/controller/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 158 bytes .../__pycache__/controller.cpython-310.pyc | Bin 0 -> 2214 bytes src/controller/controller/controller.py | 57 ++++ src/controller/launch/experiment.launch.py | 22 ++ src/controller/package.xml | 18 ++ src/controller/resource/controller | 0 src/controller/setup.cfg | 4 + src/controller/setup.py | 30 +++ src/ros2.repos | 10 + src/vicon_simulator/package.xml | 18 ++ src/vicon_simulator/resource/vicon_simulator | 0 src/vicon_simulator/setup.cfg | 4 + src/vicon_simulator/setup.py | 26 ++ .../vicon_simulator/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 168 bytes .../__pycache__/robot.cpython-310.pyc | Bin 0 -> 2809 bytes .../robot_pose_publisher.cpython-310.pyc | Bin 0 -> 2805 bytes src/vicon_simulator/vicon_simulator/robot.py | 76 ++++++ test.sh | 6 + 33 files changed, 1067 insertions(+) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100755 .devcontainer/repos_to_submodules.py create mode 100644 .env create mode 100644 .gitignore create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json create mode 100644 LICENSE create mode 100644 README.md create mode 100755 build.sh create mode 100755 setup.sh create mode 100644 src/controller/controller/__init__.py create mode 100644 src/controller/controller/__pycache__/__init__.cpython-310.pyc create mode 100644 src/controller/controller/__pycache__/controller.cpython-310.pyc create mode 100644 src/controller/controller/controller.py create mode 100644 src/controller/launch/experiment.launch.py create mode 100644 src/controller/package.xml create mode 100644 src/controller/resource/controller create mode 100644 src/controller/setup.cfg create mode 100644 src/controller/setup.py create mode 100644 src/ros2.repos create mode 100644 src/vicon_simulator/package.xml create mode 100644 src/vicon_simulator/resource/vicon_simulator create mode 100644 src/vicon_simulator/setup.cfg create mode 100644 src/vicon_simulator/setup.py create mode 100644 src/vicon_simulator/vicon_simulator/__init__.py create mode 100644 src/vicon_simulator/vicon_simulator/__pycache__/__init__.cpython-310.pyc create mode 100644 src/vicon_simulator/vicon_simulator/__pycache__/robot.cpython-310.pyc create mode 100644 src/vicon_simulator/vicon_simulator/__pycache__/robot_pose_publisher.cpython-310.pyc create mode 100644 src/vicon_simulator/vicon_simulator/robot.py create mode 100755 test.sh diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..119f947 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,17 @@ +FROM althack/ros2:humble-dev + +# ** [Optional] Uncomment this section to install additional packages. ** +# +# ENV DEBIAN_FRONTEND=noninteractive +# RUN apt-get update \ +# && apt-get -y install --no-install-recommends \ +# # +# # Clean up +# && apt-get autoremove -y \ +# && apt-get clean -y \ +# && rm -rf /var/lib/apt/lists/* +# ENV DEBIAN_FRONTEND=dialog + +# Set up auto-source of workspace for ros user +ARG WORKSPACE +RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..52c7979 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,48 @@ +// See https://aka.ms/vscode-remote/devcontainer.json for format details. +{ + "dockerFile": "Dockerfile", + "build": { + "args": { + "WORKSPACE": "${containerWorkspaceFolder}" + } + }, + "remoteUser": "ros", + "runArgs": [ + "--network=host", + "--cap-add=SYS_PTRACE", + "--security-opt=seccomp:unconfined", + "--security-opt=apparmor:unconfined", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix", + "--volume=/mnt/wslg:/mnt/wslg", + "--ipc=host" + // uncomment to use intel iGPU + // "--device=/dev/dri" + ], + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}", // Needed for GUI try ":0" for windows + "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", + "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", + "PULSE_SERVER": "${localEnv:PULSE_SERVER}", + "LIBGL_ALWAYS_SOFTWARE": "1" // Needed for software rendering of opengl + }, + // Set *default* container specific settings.json values on container create. + "customizations": { + "vscode": { + "extensions": [ + "althack.ament-task-provider", + "betwo.b2-catkin-tools", + "DotJoshJohnson.xml", + "ms-azuretools.vscode-docker", + "ms-iot.vscode-ros", + "ms-python.python", + "ms-vscode.cpptools", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "yzhang.markdown-all-in-one", + "zachflower.uncrustify" + ] + } + } +} diff --git a/.devcontainer/repos_to_submodules.py b/.devcontainer/repos_to_submodules.py new file mode 100755 index 0000000..70713f7 --- /dev/null +++ b/.devcontainer/repos_to_submodules.py @@ -0,0 +1,38 @@ +import glob +import os +import subprocess +import yaml + +prefix="src" + +def add_git_submodule(repo_name, repo_url, repo_version): + subprocess.call(['git', 'submodule', 'add', '-b', repo_version, repo_url, repo_name]) + +def is_submodule(repo_name): + try: + subprocess.check_output(['git', 'submodule', 'status', repo_name], stderr=subprocess.DEVNULL) + return True + except subprocess.CalledProcessError: + return False + +def parse_repos_file(file_path): + with open(file_path, 'r') as file: + repos_data = yaml.safe_load(file) + repositories = repos_data['repositories'] + + for repo_name, repo_info in repositories.items(): + if 'type' in repo_info and repo_info['type'] == 'git': + repo_url = repo_info['url'] + repo_version = repo_info['version'] + submodule_name = os.path.join(prefix, repo_name) + + if not is_submodule(submodule_name): + add_git_submodule(submodule_name, repo_url, repo_version) + print(f"Added {repo_name} as a submodule.") + +# Find .repos files within the src directory +repos_files = glob.glob('src/**/*.repos', recursive=True) + +# Process each .repos file +for repos_file in repos_files: + parse_repos_file(repos_file) diff --git a/.env b/.env new file mode 100644 index 0000000..edd746e --- /dev/null +++ b/.env @@ -0,0 +1 @@ +PYTHONPATH=src:install/lib/python3.10/site-packages diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9e5eef2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build/* +install/* +log/* +site/* \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..40d2c8e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/humble/include/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "compileCommands": "${workspaceFolder}/build/compile_commands.json", + "cStandard": "c99", + "cppStandard": "c++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..6dc8dd6 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,90 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + // Example launch of a python file + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal", + }, + // Example gdb launch of a ros executable + { + "name": "(gdb) Launch (merge-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "(gdb) Launch (isolated-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/${input:package}/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + //Example of a ROS Launch file + { + "name": "ROS: Launch File (merge-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/share/${input:package}/launch/${input:ros_launch}", + }, + { + "name": "ROS: Launch File (isolated-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/${input:package}/share/${input:package}/launch/${input:ros_launch}", + }, + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name", + "default": "examples_rclcpp_minimal_publisher" + }, + { + "id": "program", + "type": "promptString", + "description": "Program name", + "default": "publisher_member_function" + }, + { + "id": "ros_launch", + "type": "promptString", + "description": "ROS launch name", + "default": "file_name_launch.py" + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..98e8421 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,75 @@ +{ + "editor.tabSize": 8, + "editor.rulers": [ + 100 + ], + "files.associations": { + "*.repos": "yaml", + "*.world": "xml", + "*.xacro": "xml" + }, + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Autocomplete from ros python packages + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Environment file lets vscode find python files within workspace + "python.envFile": "${workspaceFolder}/.env", + // Use the system installed version of autopep8 + "python.formatting.autopep8Path": "/usr/bin/autopep8", + "python.formatting.autopep8Args": [ + "--max-line-length=100" + ], + "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", + "C_Cpp.formatting": "disabled", + "uncrustify.configPath.linux": "/opt/ros/humble/lib/python3.10/site-packages/ament_uncrustify/configuration/ament_code_style.cfg", + "[cpp]": { + "editor.defaultFormatter": "zachflower.uncrustify" + }, + "search.exclude": { + "**/node_modules": true, + "**/bower_components": true, + "**/*.code-search": true, + "**/build": true, + "**/install": true, + "**/log": true + }, + "cSpell.words": [ + "RTPS", + "athackst", + "autopep", + "cmake", + "cppcheck", + "cpplint", + "DCMAKE", + "deque", + "devcontainer", + "ints", + "noqa", + "pytest", + "rclcpp", + "rclpy", + "repos", + "rosdep", + "rosdistro", + "rosidl", + "RTPS", + "uncrustify", + "Wextra", + "Wpedantic", + "xmllint" + ], + "cSpell.allowCompoundWords": true, + "cSpell.ignorePaths": [ + "**/package-lock.json", + "**/node_modules/**", + "**/vscode-extension/**", + "**/.git/objects/**", + ".vscode", + ".vscode-insiders", + ".devcontainer/devcontainer.json" + ], + "ament-task-provider.envSetup": "source /opt/ros/humble/setup.bash", +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..c08a89b --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,254 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + // Build tasks + { + "label": "build", + "detail": "Build workspace (default)", + "type": "shell", + "command": "./build.sh", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": "$gcc" + }, + { + "label": "debug", + "detail": "Build workspace (debug)", + "type": "shell", + "command": "./build.sh", + "options": { + "env": { + "BUILD_TYPE": "Debug" + } + }, + "group": "build", + "problemMatcher": "$gcc" + }, + // Test tasks + { + "label": "test", + "detail": "Run all unit tests and show results.", + "type": "shell", + "command": "./test.sh", + "group": { + "kind": "test", + "isDefault": true + } + }, + // Clean + { + "label": "clean", + "detail": "Run the clean target", + "type": "shell", + "command": "colcon build --cmake-target clean", + "problemMatcher": "$gcc" + }, + { + "label": "purge", + "detail": "Purge workspace by deleting all generated files.", + "type": "shell", + "command": "sudo rm -fr build install log; sudo py3clean .", + "problemMatcher": [] + }, + { + "label": "source", + "detail": "Source workspace", + "type": "shell", + "command": "source /opt/ros/humble/setup.bash", + "problemMatcher": [] + }, + // Linting and static code analysis tasks + { + "label": "fix", + "detail": "Reformat files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify --reformat src/", + "problemMatcher": [] + }, + { + "label": "uncrustify", + "detail": "Lint files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify src/", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "problemMatcher": [ + { + "owner": "uncrustify", + "source": "uncrustify", + "fileLocation": "relative", + "pattern": [ + // just the file name message + { + "regexp": "^(.*)'(.*)':", + "kind": "file", + "file": 2, + "message": 1 + } + ] + } + ] + }, + { + "label": "cpplint", + "detail": "Lint files with cpplint.", + "type": "ament", + "task": "cpplint", + "path": "src/", + "problemMatcher": "$ament_cpplint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + }, + { + "label": "cppcheck", + "detail": "Run static code checker cppcheck.", + "type": "ament", + "task": "cppcheck", + "path": "src/", + "problemMatcher": "$ament_cppcheck", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "options": { + "env": { + "AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS": "1" + } + } + }, + { + "label": "lint_cmake", + "detail": "Run lint on cmake files.", + "type": "ament", + "task": "lint_cmake", + "path": "src/", + "problemMatcher": "$ament_lint_cmake", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "flake8", + "detail": "Run flake8 on python files.", + "type": "ament", + "task": "flake8", + "path": "src/", + "problemMatcher": "$ament_flake8", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "pep257", + "detail": "Run pep257 on python files.", + "type": "ament", + "task": "pep257", + "path": "src/", + "problemMatcher": "$ament_pep257", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "xmllint", + "detail": "Run xmllint on xml files.", + "type": "ament", + "task": "xmllint", + "path": "src/", + "problemMatcher": "$ament_xmllint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "lint all", + "detail": "Run all linters.", + "dependsOn": [ + "cppcheck", + "cpplint", + "flake8", + "lint_cmake", + "pep257", + "xmllint", + "uncrustify" + ], + "problemMatcher": [] + }, + // Workspace editing tasks + { + "label": "new ament_cmake package", + "detail": "Create a new ROS cpp package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_cmake ${input:package}", + "problemMatcher": [] + }, + { + "label": "new ament_python package", + "detail": "Create a new ROS python package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_python ${input:package}", + "problemMatcher": [] + }, + { + "label": "import from workspace file", + "detail": "Use vcs to import modules specified by a workspace/rosinstall file.", + "type": "shell", + "command": "vcs import < src/ros2.repos src", + "problemMatcher": [] + }, + { + "label": "update workspace file", + "detail": "Use vcs to update repositories in src to workspace file.", + "type": "shell", + "command": "vcs export src > src/ros2.repos", + "problemMatcher": [] + }, + { + "label": "install dependencies", + "detail": "Install all dependencies specified in the workspaces package.xml files.", + "type": "shell", + "command": "sudo apt-get update && rosdep update && rosdep install --from-paths src --ignore-src -y", + "problemMatcher": [] + }, + { + "label": "setup", + "detail": "Set up the workspace", + "type": "shell", + "command": "./setup.sh", + "problemMatcher": [] + }, + { + "label": "add submodules from .repos", + "detail": "Create a git submodule for all repositories in your .repos file", + "type": "shell", + "command": "python3 .devcontainer/repos_to_submodules.py", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name" + } + ] +} diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 0000000..bdc84a6 --- /dev/null +++ b/README.md @@ -0,0 +1,33 @@ +# ROS2 Examples +# Installation +``` +colcon build --symlink-install +ros2 launch controller experiment.launch.py +``` + +## Prerequisites + +You should already have Docker and VSCode with the remote containers plugin installed on your system. + +* [docker](https://docs.docker.com/engine/install/) +* [vscode](https://code.visualstudio.com/) +* [vscode remote containers plugin](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) + +## Open it in vscode + +Now that you've cloned your repo onto your computer, you can open it in VSCode (File->Open Folder). + +When you open it for the first time, you should see a little popup that asks you if you would like to open it in a container. Say yes! + +![template_vscode](https://user-images.githubusercontent.com/6098197/91332551-36898100-e781-11ea-9080-729964373719.png) + +If you don't see the pop-up, click on the little green square in the bottom left corner, which should bring up the container dialog + +![template_vscode_bottom](https://user-images.githubusercontent.com/6098197/91332638-5d47b780-e781-11ea-9fb6-4d134dbfc464.png) + +In the dialog, select "Remote Containers: Reopen in container" + +VSCode will build the dockerfile inside of `.devcontainer` for you. If you open a terminal inside VSCode (Terminal->New Terminal), you should see that your username has been changed to `ros`, and the bottom left green corner should say "Dev Container" + +![template_container](https://user-images.githubusercontent.com/6098197/91332895-adbf1500-e781-11ea-8afc-7a22a5340d4a.png) + diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..7bf6bd8 --- /dev/null +++ b/build.sh @@ -0,0 +1,10 @@ +#!/bin/bash +set -e + +# Set the default build type +BUILD_TYPE=RelWithDebInfo +colcon build \ + --merge-install \ + --symlink-install \ + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + -Wall -Wextra -Wpedantic diff --git a/setup.sh b/setup.sh new file mode 100755 index 0000000..beab00e --- /dev/null +++ b/setup.sh @@ -0,0 +1,7 @@ +#!/bin/bash +set -e + +vcs import < src/ros2.repos src +sudo apt-get update +rosdep update --rosdistro=$ROS_DISTRO +rosdep install --from-paths src --ignore-src -y --rosdistro=$ROS_DISTRO diff --git a/src/controller/controller/__init__.py b/src/controller/controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/controller/controller/__pycache__/__init__.cpython-310.pyc b/src/controller/controller/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8b3127e2aa9af1b2bfef8a6977d65101827c3620 GIT binary patch literal 158 zcmd1j<>g`k0-m*E=^*+sh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6vJzdXMvySN}R zIki~7D8JY!zNEAyzbG>?M?a}FGbcqqIX|zYC_g7BwFsFRAD@|*SrQ+wS5SG2!zMRB Rr8Fni4rFvO6OdqG002W$CBXmy literal 0 HcmV?d00001 diff --git a/src/controller/controller/__pycache__/controller.cpython-310.pyc b/src/controller/controller/__pycache__/controller.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d426b0a3e562be6268d4258e7319d5de60720b9e GIT binary patch literal 2214 zcmZuy&2Jnv6u0MVXJf)Rq!m7)X%q)O26)dROsHHgCk8g16z&9t*Ki|y?u ztJw>Q)KmY03Mo1Ef;b}apZLltH_lw(_q-d@RM_&H=V$x*`MuwJQ?0Bd4DHVEH-^7P zjQvTI7h8bI7Z~~gL@>b%)}^mk@R8T`IPrerkAiM63cKMb>PAk+i$*sVfe24nxA`>_ zk!YMS(U9J;-%UhI#G-k^yDixk$sT}w?Y%BPN822#rZy+^$$mSfB&pl9Y?t}XCK=3-K`5LRe z+FxTg*&3UN!h;muNxg-1oy{W?oQ8E+M`zx=;m%>L_G#Zbor|#^)$tjhH{B}qLCf;| zJaJk*n&Zs!E@*8ztx4U2)^^>fn{|8O@3D?Q`y)xw?p5Vusfr=H-qh)M;pk5K{UH`M(%D8j{n4#{C5xh( zruv|o6e8W1sgh2sNT&y~OvjZnc{xl6l}aCcvzKnC_m9j0tT2PD?~K#1NvF9^MODg8 zr}Nx{&(A3$ordLymLFL@vwUj3a%>wt)i=Fz+eTn?UUpV&a%rN9aY{9b5GYj(#5S=_ z;W|CGP0%G9&}(ggwXqG|o888v%Z*FchbNvhwGLH(-`kp2>X9D9Z+c5rdOI@{;3w}D zTlL@`ALB}+ z{Gz5eKqTvO!MdJ^pRfTJz6eg*U0;MU7pb<*@pPZQe=Ij3hQIlt}b_uC;LUN52RA$E9+xUbwSHw@SNLbmi3ViI?G&))$7=! zQjm8rG(8+qK%d>b>a5NGEb>Qs*eB3lG%}q)=qkw1U~2vl)dQ8oTXO!~H>e$uhR6H| z)Dm>(w=;H1^}!R~Gx)QHXYmbc)Rkw!FA;g%#K%-mnj%C%Q52v0b(AHT)9hm2LOj~1 zE2uX(;rt)(&aOWoXComQIVIjr;ym?a8mS? zdY`0?Wz0Gab&IxkoNxh_L)%y|s0*2GEqG_TZ5k@;8RD^sB_-`5pznfv4@0klBs}5Q z`7Is=s~p{%t-lo7|A5r-)LS^Pi%J9pmc5Y#0+TEoRRSdwd^@8YE*2{eHV%u5?z1aP zX!TJgFW}`&`T(m7yWRx%HiiRQ%&zepp(5Fp3?yt3!X2J&gE2r;jUU}p!h;7qaKldV zH*xr2r@RQ&?=2NB4Py97Bf&PyBXgNYHLM5nA>PtER-_UB7b@*;)McpM?;%?h?s^|gsm7%Og(x@YfkKUvp z(W73#LRd~l<0I83DWS+2>26SWXzF}-Nhnioe0LFx&!|l4HIUdPY|LYCbv;>&ql5?l F{0nyoE4%;z literal 0 HcmV?d00001 diff --git a/src/controller/controller/controller.py b/src/controller/controller/controller.py new file mode 100644 index 0000000..2637a78 --- /dev/null +++ b/src/controller/controller/controller.py @@ -0,0 +1,57 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, Twist, Vector3 +import numpy as np + +def euler_from_quaternion(quaternion): + """ + Converts quaternion (w in last place) to euler roll, pitch, yaw + quaternion = [x, y, z, w] + Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. + """ + x = quaternion.x + y = quaternion.y + z = quaternion.z + w = quaternion.w + + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.arcsin(sinp) + + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + +class ControllerNode(Node): + def __init__(self) -> None: + super().__init__('robot_controller') + self.sub = self.create_subscription(PoseStamped, '/robot_pose', self.send_ctrl, 10) + self.pub = self.create_publisher(Twist, '/cmd_vel', 1) + + def send_ctrl(self, msg: PoseStamped): + robot_x = msg.pose.position.x + robot_y = msg.pose.position.y + _, _, robot_theta = euler_from_quaternion(msg.pose.orientation) + self.get_logger().info(f'Robot position: x={robot_x:0.2f} y={robot_y:0.2f} theta={robot_theta:0.2f}') + + # Do research code + + twist_msg = Twist() + twist_msg.linear = Vector3(x=1.0, y=0.0, z=0.0) + twist_msg.angular = Vector3(x=0.0, y=0.0, z=0.0) + + self.pub.publish(twist_msg) + +def main(args=None): + rclpy.init(args=args) + node = ControllerNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/controller/launch/experiment.launch.py b/src/controller/launch/experiment.launch.py new file mode 100644 index 0000000..2e43bda --- /dev/null +++ b/src/controller/launch/experiment.launch.py @@ -0,0 +1,22 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + ld = LaunchDescription() + + controller_node = Node( + package="controller", + executable="robot_controller", + ) + sim_node = Node( + package="vicon_simulator", + executable="robot_pose_simulator", + ) + + ld.add_action(controller_node) + ld.add_action(sim_node) + + return ld + + + diff --git a/src/controller/package.xml b/src/controller/package.xml new file mode 100644 index 0000000..451857e --- /dev/null +++ b/src/controller/package.xml @@ -0,0 +1,18 @@ + + + + controller + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/controller/resource/controller b/src/controller/resource/controller new file mode 100644 index 0000000..e69de29 diff --git a/src/controller/setup.cfg b/src/controller/setup.cfg new file mode 100644 index 0000000..1049f8a --- /dev/null +++ b/src/controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/controller +[install] +install_scripts=$base/lib/controller diff --git a/src/controller/setup.py b/src/controller/setup.py new file mode 100644 index 0000000..b5f1fcc --- /dev/null +++ b/src/controller/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'controller' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.py')) + + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ros', + maintainer_email='lu.michael@outlook.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'robot_controller = controller.controller:main' + ], + }, +) diff --git a/src/ros2.repos b/src/ros2.repos new file mode 100644 index 0000000..5a499c0 --- /dev/null +++ b/src/ros2.repos @@ -0,0 +1,10 @@ +# List of repositories to use within your workspace +# See https://github.com/dirk-thomas/vcstool +############ +# Example: +############ +repositories: + examples: + type: git + url: https://github.com/ros2/examples.git + version: humble diff --git a/src/vicon_simulator/package.xml b/src/vicon_simulator/package.xml new file mode 100644 index 0000000..c14cf25 --- /dev/null +++ b/src/vicon_simulator/package.xml @@ -0,0 +1,18 @@ + + + + vicon_simulator + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/vicon_simulator/resource/vicon_simulator b/src/vicon_simulator/resource/vicon_simulator new file mode 100644 index 0000000..e69de29 diff --git a/src/vicon_simulator/setup.cfg b/src/vicon_simulator/setup.cfg new file mode 100644 index 0000000..f7fe44e --- /dev/null +++ b/src/vicon_simulator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vicon_simulator +[install] +install_scripts=$base/lib/vicon_simulator diff --git a/src/vicon_simulator/setup.py b/src/vicon_simulator/setup.py new file mode 100644 index 0000000..496c7a2 --- /dev/null +++ b/src/vicon_simulator/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'vicon_simulator' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ros', + maintainer_email='lu.michael@outlook.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'robot_pose_simulator = vicon_simulator.robot:main' + ], + }, +) diff --git a/src/vicon_simulator/vicon_simulator/__init__.py b/src/vicon_simulator/vicon_simulator/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/vicon_simulator/vicon_simulator/__pycache__/__init__.cpython-310.pyc b/src/vicon_simulator/vicon_simulator/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e02000ddef0fd83f7ee06122aeae2a6392217a99 GIT binary patch literal 168 zcmd1j<>g`k0^W6E=^*+sh(HF6K#l_t7qb9~6oz01O-8?!3`HPe1o6vTzdXMvySN}R zIki~7D8JY!zNEAyzbG>?M?a}FGbcsAEHgPjFTOZ4w=^daD1pI`kI&4@EQycTE2zB1 VVUwGmQks)$2ePD?2}rOo000ZZDggih literal 0 HcmV?d00001 diff --git a/src/vicon_simulator/vicon_simulator/__pycache__/robot.cpython-310.pyc b/src/vicon_simulator/vicon_simulator/__pycache__/robot.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..624a9a89fd3fa8097bb73258c7e559f6e5c68641 GIT binary patch literal 2809 zcmZ`*&2Jk;6rb5IukCf3wr*$&rJ)Ce1xjfV2ueR9RS3kv>IV{%FDuuxaW=Jg-I;aU zSQ{>>)KmWeKu8I<-nsB^aGV1|;!x$rnG5{hTZbeR#+o;8-n^YR^FDs#ywz$5H1*Tc z@SB6YHmr7vC+ z+kWZ8FE0DV?e3FNO_n-=m5(%~qICzk(Ghbd&2*Dj^#TbTRiHmX-?D)!Hj<)m{aIX0 zF0;!jvPhcIbfSar|)FA5|;*G<5YzBCivnm>nT>3Qc~bZfM_7c(NfQm!$*B$b&|AqdBM1FJMZf z(0Q>fO39-vk*k0XtSQrGH5^sZPp7QYoQ2g z-QoPYYaO{yMWd;D_KE0I#E}O&9_N6jTGD4gte=c0#a3s*I{g%V6a9g82H*|)0CR&r zfXtA5fNsEJG{(w@|B8q`g8gPH73RDN^ z#~R&<8gKLu#?+pPb*WtCJ&k}iz6yYV-ZP*^&xcn+yTp0OaCT70gmG0Sz5+Hr_9s7? zk3b5sgM3hkxJ1V6I*QA4&*7O@0LhfM=c3$#qP7E#IOMYPySB~H5KjQ2hxqaGyKkxe zd~?tfD% zZ++fcw;%=J)#G?D>KhZshqcQ#-orXqO(YlOqKxG1!hbvJ5Wpyt4jTE1fc6%;`4xnq z>cCfpvm>@qN4xHx*!9r;JyhCe5ZB;A8pEPa`kM{qF7ub*yk)#%DtPXq3Y)5?YIBWn zS?su|$Eb8!8$QQ3cvqO#@m1rw2SB8qTk!&_jm8tOz6i@FRrAo2ioW+$Yd$A?G+#Kh zL-QYkD%zW;c2Acad5-CcuKp0Wv_77Uk>bkYz|Cv!x0`ruA|rdx)aD zN&FfS244Aij*lw{YFgk$y}I(``pkQO^~(CpUD>)mtF18lUhlN^r}P;^Acwj>ObP@w zM@hO_{@4aZ9*@n?1{+Bqd5uJ(XWNGBngBLcnz4Y)a#?8#o%J}G!UOBJiFYr_faN%f zZIFdgJ{)2flU0QFjMdW*8etb#CPRB1uUz_?N6 s2DyXHRE|D87IWl98(yoP>l*|I9Mur09s#=-oUpZcws9^D8q)jgZ!BYai2wiq literal 0 HcmV?d00001 diff --git a/src/vicon_simulator/vicon_simulator/__pycache__/robot_pose_publisher.cpython-310.pyc b/src/vicon_simulator/vicon_simulator/__pycache__/robot_pose_publisher.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..baed0b2cc6e7dc7dd131ec51118b612f0b004575 GIT binary patch literal 2805 zcmZ`*&2Jk;6rb5IukCf3rfz5orJ)Ce1xjcEsg#yPst^(fi?&FWd|A1kjkBq}>&~v5 z+S;5Fsi*z{fRGYyJ#*sU;5a8F4pnZPxxnwebrKiKtme&|H*db)$8R=mG-?9n;xEhn zv?9bG)R`|1I-983QxGMT9EgtO*6LVjt${ssI*z0+{Rrs@Z>6GROaJht@bQLrztqI|%$wQ!8RMY|=9 z(@s^?vQ7QlG>abO-Qg%w#{D*plibwqjk|fIlQ>O!q(5J5bT(15YakKd0H>E78z-R? z;s&$RxypVZT2kI_?hEAC2RIT7Me%(j?5{;ds!+z<>IhZV--OscEh9NUx11#t*|diSsb@ z>DU%XQ%)E7zH28#JrbW34-8_Xyykac3b7+{d1#@vm0S@=vMKhcM^cBS)=X+EdygfB z#F$%HgNC`d@l?JY$h6X@hySmzE|g zwQ&}hE$I%UFf_F=9HweKAU+7gC*$s*9MKn`tIrd8n+W-Fj?gJI8Ix*8>-xfxH5u3} z?NOAT6uKMrIkbKQQy{Fah&@qAZe@Y^Mc<;3E8u}W8ytK9&S4osaL;8KaURPoa21vj z=ff8jc&>`pM_Vf#?$^;$KY*sKNV5Sy)tjR5ids`#5=BGVw*_PY%c|r{$Q+hI1A5SA zJ(p$BfDGEK=Rwv$VpQ7w*>yKt@|Y5dBS0^KJEj0UCJVG2jsZ>ir7waQHyV!eNo&Da z-57NT^|7&f;4{=7`Wb2uvW(TW|ngiVgJsnY28#+ZFP3JT=`W*1a z1Z>J)Vc1j73Z66Ny|9sbHC0g+9`#qmzKs;D z7XUnH!nsT#2VcdU+6xCjNNWaR0|`d$0`@(JJr`B|WDgbm=%@zwXea7H3;Saw>iy)E zuH89*xKyy4*@frY`ZHY5xOg)L>MV_B3HJL2UK{z$$O)rp`3Sv-37Q^6WY`Arn?x98 z|9G{!r2Z?zxgaAb=7vy*h! zczGHQv%c}Rqb`CN!9(x1@wYUgX{z6lv^n=OL`(zBb$^UKzfwaOWP z(^{0%YyT5yb78LE!St4=ub|QI61hr*Ujf>}G-h$#iw1*}*yf|$COtmNC*VFr&FE$D z#e!V)G|65|c0xq5Fvsx+U~;j8$W`(MlD&*$0_-v>z#*FHx2*gF!Oulk_p_FZr0Wgv z7Xa(UId6jBj!~Q#KieMXD&0+5mcED8jgt`Qylyn(47+ji6^3UC>CyWo^+TF~7m`N9 zuqE3-XEzxSN0Sn~@6&jVcV6RV=`!YgKT3yDt|t^A*%}BVL>Unj3=9(tt$b46rZn`~ lv6v$(8vl0rEPqMa&tCO`-T^Rr!SWl6mui=NuO^+p{{cc2e3t+K literal 0 HcmV?d00001 diff --git a/src/vicon_simulator/vicon_simulator/robot.py b/src/vicon_simulator/vicon_simulator/robot.py new file mode 100644 index 0000000..0b6910d --- /dev/null +++ b/src/vicon_simulator/vicon_simulator/robot.py @@ -0,0 +1,76 @@ +import numpy as np +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, Point, Quaternion + +class Robot: + v = 1.0 + def __init__(self) -> None: + self.z = np.array([0.0, 0.0, 0.0]) + + def update_state(self, dt): + dx = self.v * np.cos(self.z[2]) + dy = self.v * np.sin(self.z[2]) + dtheta = 1.0 + + z_tp1 = self.z + dt * np.array([dx, dy, dtheta]) + self.z = z_tp1 + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = np.cos(ai) + si = np.sin(ai) + cj = np.cos(aj) + sj = np.sin(aj) + ck = np.cos(ak) + sk = np.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class RobotPoseSimulatorNode(Node): + def __init__(self) -> None: + super().__init__('robot_pose_simulator') + self.create_timer(0.1, self.update_pose) + self.pub = self.create_publisher(PoseStamped, '/robot_pose', 1) + self.robot = Robot() + + def update_pose(self): + self.robot.update_state(0.1) + + + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'world' + + point = Point(x=self.robot.z[0], y=self.robot.z[1], z=0.5) + + q = quaternion_from_euler(0.0, 0.0, self.robot.z[2]) + orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + msg.pose.position = point + msg.pose.orientation = orientation + + self.get_logger().debug(f'Robot position: x={self.robot.z[0]:0.2f} y={self.robot.z[1]:0.2f} theta={self.robot.z[2]:0.2f}') + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = RobotPoseSimulatorNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/test.sh b/test.sh new file mode 100755 index 0000000..793de56 --- /dev/null +++ b/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +if [ -f install/setup.bash ]; then source install/setup.bash; fi +colcon test --merge-install +colcon test-result --verbose From 157f0d4474973167f68a88bbf0c9f9f4eb65cec7 Mon Sep 17 00:00:00 2001 From: Mo Chen Date: Tue, 22 Oct 2024 16:07:34 -0700 Subject: [PATCH 2/3] add tips and resources --- README.md | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/README.md b/README.md index bdc84a6..cec2723 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ # ROS2 Examples +[Installation](#installation) + # Installation ``` colcon build --symlink-install @@ -13,6 +15,14 @@ You should already have Docker and VSCode with the remote containers plugin inst * [vscode](https://code.visualstudio.com/) * [vscode remote containers plugin](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) +To use docker with sudo and connect to the container with VS code following the [postinstall instructions](https://docs.docker.com/engine/install/linux-postinstall/). + +`sudo groupadd docker` + +`sudo usermod -aG docker $USER` + +`newgrp docker` + ## Open it in vscode Now that you've cloned your repo onto your computer, you can open it in VSCode (File->Open Folder). @@ -31,3 +41,21 @@ VSCode will build the dockerfile inside of `.devcontainer` for you. If you open ![template_container](https://user-images.githubusercontent.com/6098197/91332895-adbf1500-e781-11ea-8afc-7a22a5340d4a.png) +# Tips / other resources +## Rosbags +Rosbags are used to record ROS data. + +[Basic tutorial](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Recording-And-Playing-Back-Data/Recording-And-Playing-Back-Data.html) | [Advanced tutorials](https://docs.ros.org/en/humble/Tutorials/Advanced.html) | [Repo](https://github.com/ros2/rosbag2) + +### Tools for working with rosbags (and ROS2 in general) +- PlotJuggler: [Website](https://plotjuggler.io/) | [Repo](https://github.com/facontidavide/PlotJuggler) +- This [Medium post](https://medium.com/evocargo/9-awesome-open-source-tools-to-manage-your-rosbags-b350fdb651c8) contains some other tools (potentially outdated) + +### Recording and playing back data using rosbags and plotjuggler +1. `sudo apt install ros-${ROS_DISTRO}-plotjuggler-ros` +2. Record the desired topics to some output folder: `ros2 bag record -o ` + - Example: `ros2 bag record /tf /cmd_vel -o test` + - A default file name will be used if the `-o` option is not used + - **Warning**: Using `ros2 bag record --all` will usually result in _gigantic_ file sizes +3. `ros2 run plotjuggler plotjuggler` +4. Load data or stream from current topics \ No newline at end of file From 76ebf2240cfb169a4ac96cd9847e87f7ce8b02db Mon Sep 17 00:00:00 2001 From: Mo Chen Date: Tue, 22 Oct 2024 16:08:39 -0700 Subject: [PATCH 3/3] add table of contents at top --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index cec2723..424d16e 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ROS2 Examples -[Installation](#installation) +[Installation](#installation) | [Misc tips](#tips--other-resources) # Installation ```