├── .github └── workflows │ ├── lint.yaml │ └── test.yaml ├── CONTRIBUTING.md ├── LICENSE ├── geometry_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── turtle_tf2_cpp ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── turtle_tf2_demo.launch.py │ ├── turtle_tf2_dynamic_frame_demo.launch.py │ └── turtle_tf2_fixed_frame_demo.launch.py ├── package.xml └── src │ ├── dynamic_frame_tf2_broadcaster.cpp │ ├── fixed_frame_tf2_broadcaster.cpp │ ├── static_turtle_tf2_broadcaster.cpp │ ├── turtle_tf2_broadcaster.cpp │ ├── turtle_tf2_listener.cpp │ └── turtle_tf2_message_filter.cpp └── turtle_tf2_py ├── CHANGELOG.rst ├── launch ├── turtle_tf2_demo.launch.py ├── turtle_tf2_dynamic_frame_demo.launch.py ├── turtle_tf2_fixed_frame_demo.launch.py └── turtle_tf2_sensor_message.launch.py ├── package.xml ├── resource └── turtle_tf2_py ├── rviz └── turtle_rviz.rviz ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── turtle_tf2_py ├── __init__.py ├── dynamic_frame_tf2_broadcaster.py ├── fixed_frame_tf2_broadcaster.py ├── static_turtle_tf2_broadcaster.py ├── turtle_tf2_broadcaster.py ├── turtle_tf2_listener.py └── turtle_tf2_message_broadcaster.py /.github/workflows/lint.yaml: -------------------------------------------------------------------------------- 1 | name: Lint diagnostics 2 | on: 3 | pull_request: 4 | 5 | jobs: 6 | ament_lint: 7 | name: ament_${{ matrix.linter }} 8 | runs-on: ubuntu-latest 9 | container: 10 | image: ubuntu:jammy 11 | strategy: 12 | fail-fast: false 13 | matrix: 14 | linter: [copyright, flake8, pep257] 15 | steps: 16 | - uses: actions/checkout@v2 17 | - uses: ros-tooling/setup-ros@v0.7 18 | with: 19 | required-ros-distributions: rolling 20 | - uses: ros-tooling/action-ros-lint@v0.1 21 | with: 22 | distribution: rolling 23 | linter: ${{ matrix.linter }} 24 | package-name: turtle_tf2_py 25 | ament_lint_cpp: 26 | name: ament_${{ matrix.linter }} 27 | runs-on: ubuntu-latest 28 | container: 29 | image: ubuntu:jammy 30 | strategy: 31 | fail-fast: false 32 | matrix: 33 | linter: [copyright, cpplint, uncrustify] 34 | steps: 35 | - uses: actions/checkout@v4 36 | - uses: ros-tooling/setup-ros@v0.7 37 | with: 38 | required-ros-distributions: rolling 39 | - uses: ros-tooling/action-ros-lint@v0.1 40 | with: 41 | distribution: rolling 42 | linter: ${{ matrix.linter }} 43 | package-name: turtle_tf2_cpp 44 | -------------------------------------------------------------------------------- /.github/workflows/test.yaml: -------------------------------------------------------------------------------- 1 | name: Test geometry_tutorials 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - ros2 7 | 8 | jobs: 9 | build_and_test_source_rolling: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-testing 13 | steps: 14 | - name: Build and run tests 15 | id: action-ros-ci 16 | uses: ros-tooling/action-ros-ci@v0.4 17 | with: 18 | package-name: | 19 | turtle_tf2_py 20 | turtle_tf2_cpp 21 | target-ros2-distro: rolling 22 | vcs-repo-file-url: https://raw.githubusercontent.com/ros2/ros2/rolling/ros2.repos 23 | colcon-defaults: | 24 | { 25 | "build": { 26 | "cmake-args": [ 27 | "-DCMAKE_CXX_FLAGS=\"-Werror\"" 28 | ] 29 | }, 30 | "test": { 31 | "ctest-args": ["-LE", "xfail"], 32 | "pytest-args": ["-m", "not xfail"] 33 | } 34 | } 35 | build_and_test_binaries_rolling: 36 | runs-on: ubuntu-latest 37 | container: 38 | image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-testing 39 | steps: 40 | - name: Build and run tests 41 | id: action-ros-ci 42 | uses: ros-tooling/action-ros-ci@v0.4 43 | with: 44 | package-name: | 45 | turtle_tf2_py 46 | turtle_tf2_cpp 47 | target-ros2-distro: rolling 48 | colcon-defaults: | 49 | { 50 | "build": { 51 | "cmake-args": [ 52 | "-DCMAKE_CXX_FLAGS=\"-Werror\"" 53 | ] 54 | }, 55 | "test": { 56 | "ctest-args": ["-LE", "xfail"], 57 | "pytest-args": ["-m", "not xfail"] 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | 15 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 16 | line to commit messages to certify that they have the right to submit 17 | the code they are contributing to the project according to the 18 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /geometry_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package geometry_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.0 (2025-05-05) 6 | ------------------ 7 | 8 | 0.6.3 (2024-12-27) 9 | ------------------ 10 | 11 | 0.6.2 (2024-07-19) 12 | ------------------ 13 | 14 | 0.6.1 (2024-07-16) 15 | ------------------ 16 | 17 | 0.6.0 (2024-07-10) 18 | ------------------ 19 | 20 | 0.3.6 (2022-09-15) 21 | ------------------ 22 | 23 | 0.3.5 (2022-09-08) 24 | ------------------ 25 | 26 | 0.3.4 (2021-10-11) 27 | ------------------ 28 | 29 | 0.3.3 (2021-08-27) 30 | ------------------ 31 | 32 | 0.3.2 (2021-08-09) 33 | ------------------ 34 | 35 | 0.3.1 (2021-06-23) 36 | ------------------ 37 | 38 | * Add Audrow as a maintainer and move Shyngys to author (`#37 `_) 39 | * Migrate turtle_tf2 tutorial package to ROS2 (`#34 `_) 40 | * Contributors: kurshakuz, Audrow Nash 41 | -------------------------------------------------------------------------------- /geometry_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(geometry_tutorials NONE) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() -------------------------------------------------------------------------------- /geometry_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | geometry_tutorials 5 | 0.7.0 6 | Metapackage of geometry tutorials ROS. 7 | Alejandro Hernández Cordero 8 | Audrow Nash 9 | Apache License, Version 2.0 10 | 11 | http://www.ros.org/wiki/geometry_tutorials 12 | https://github.com/ros/geometry_tutorials 13 | https://github.com/ros/geometry_tutorials/issues 14 | 15 | Shyngyskhan Abilkassov 16 | 17 | ament_cmake 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtle_tf2_cpp 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.0 (2025-05-05) 6 | ------------------ 7 | 8 | 0.6.3 (2024-12-27) 9 | ------------------ 10 | * Deprecating tf2 C Headers (`#87 `_) 11 | * Contributors: Lucas Wendland 12 | 13 | 0.6.2 (2024-07-19) 14 | ------------------ 15 | * Updated deprecated message filter headers (`#84 `_) 16 | * Contributors: Alejandro Hernández Cordero 17 | 18 | 0.6.1 (2024-07-16) 19 | ------------------ 20 | * Clean rolling CI (`#82 `_) 21 | * Use target_link_libraries (`#83 `_) 22 | * Contributors: Alejandro Hernández Cordero 23 | 24 | 0.6.0 (2024-07-10) 25 | ------------------ 26 | * Used turtlesim_msgs (`#78 `_) 27 | * Migrate std::bind calls to lambda expressions (`#76 `_) 28 | * ♻️ Geometry msgs lambda refactor 29 | Co-authored-by: Chris Lalancette 30 | * Fix a few more minor nitpicks. (`#72 `_) 31 | 1. Remove dependencies from the targets that don't need them. 32 | 2. Remove a totally unnecessary typedef. 33 | 3. Remove unnecessary casts to float. 34 | * Contributors: Alejandro Hernández Cordero, Chris Lalancette, Felipe Gomes de Melo 35 | 36 | 0.3.6 (2022-09-15) 37 | ------------------ 38 | * Minor cleanups across the tutorials. (`#71 `_) 39 | * Contributors: Chris Lalancette 40 | 41 | 0.3.5 (2022-09-08) 42 | ------------------ 43 | * Cleanup CI (`#70 `_) 44 | * Contributors: Chris Lalancette 45 | 46 | 0.3.4 (2021-10-11) 47 | ------------------ 48 | * Add source code and launch file for tf2 PointStamped message publisher and listener/filter (`#62 `_) 49 | Add Python Code of PointStamped Messages Broadcaster Node 50 | Add launch File turtle_tf2_sensor_message.launch.py 51 | Update CMakeLists.txt for turtle_tf2_message_filter node 52 | Update package.xml for dependencies of turtle_tf2_message_filter.cpp 53 | Update turtle_tf2_message_filter.cpp after checking linters. 54 | for the copyright year from 2015 to 2021 and delete the line "self.sub". 55 | sort include headers; change two node instances to one instance; change turtle3 spawning manner. 56 | remove whitespaces in line 39 and 42 57 | some little updates 58 | * update static_turtle_tf2_broadcaster.cpp to assign stamp value to now() (`#63 `_) 59 | * Added compile and test Github action (`#60 `_) 60 | * assign now variable to node now() value in frame broadcasters (`#61 `_) 61 | * Udpate C and CPP standard (`#59 `_) 62 | * Contributors: Alejandro Hernández Cordero, kenny_wang, kurshakuz 63 | 64 | 0.3.3 (2021-08-27) 65 | ------------------ 66 | * update listener node (`#53 `_) 67 | * assign now value to the this->get_clock()->now() (`#54 `_) 68 | * Fixed and dynamic frame broacaster nodes and launch files C++ (`#52 `_) 69 | * update copyright year (`#51 `_) 70 | * Update tags in the package.xml (`#50 `_) 71 | * replace exec_depend to build_depend in package.xml (`#49 `_) 72 | * Contributors: kurshakuz 73 | 74 | 0.3.2 (2021-08-09) 75 | ------------------ 76 | * turtle_tf2_cpp tutorial package (`#44 `_) 77 | Co-authored-by: Alejandro Hernández Cordero 78 | * Contributors: kurshakuz 79 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(turtle_tf2_cpp) 3 | 4 | # Default to C11 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 11) 7 | endif() 8 | # Default to C++14 9 | if(NOT CMAKE_CXX_STANDARD) 10 | set(CMAKE_CXX_STANDARD 14) 11 | endif() 12 | 13 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 14 | add_compile_options(-Wall -Wextra -Wpedantic) 15 | endif() 16 | 17 | find_package(ament_cmake REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | find_package(message_filters REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(tf2 REQUIRED) 22 | find_package(tf2_geometry_msgs REQUIRED) 23 | find_package(tf2_ros REQUIRED) 24 | find_package(turtlesim_msgs REQUIRED) 25 | 26 | if(TARGET tf2_geometry_msgs::tf2_geometry_msgs) 27 | get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES) 28 | else() 29 | set(_include_dirs ${tf2_geometry_msgs_INCLUDE_DIRS}) 30 | endif() 31 | 32 | find_file(TF2_CPP_HEADERS 33 | NAMES tf2_geometry_msgs.hpp 34 | PATHS ${_include_dirs} 35 | NO_CACHE 36 | PATH_SUFFIXES tf2_geometry_msgs 37 | ) 38 | 39 | add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp) 40 | target_link_libraries(static_turtle_tf2_broadcaster PRIVATE 41 | ${geometry_msgs_TARGET} 42 | rclcpp::rclcpp 43 | tf2::tf2 44 | tf2_ros::tf2_ros 45 | ) 46 | 47 | add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) 48 | target_link_libraries(turtle_tf2_broadcaster PRIVATE 49 | ${geometry_msgs_TARGET} 50 | rclcpp::rclcpp 51 | tf2::tf2 52 | tf2_ros::tf2_ros 53 | ${turtlesim_msgs_TARGETS} 54 | ) 55 | 56 | add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) 57 | target_link_libraries(turtle_tf2_listener PRIVATE 58 | ${geometry_msgs_TARGET} 59 | rclcpp::rclcpp 60 | tf2::tf2 61 | tf2_ros::tf2_ros 62 | ${turtlesim_msgs_TARGETS} 63 | ) 64 | 65 | add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp) 66 | target_link_libraries(fixed_frame_tf2_broadcaster PRIVATE 67 | ${geometry_msgs_TARGET} 68 | rclcpp::rclcpp 69 | tf2_ros::tf2_ros 70 | ) 71 | 72 | add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp) 73 | target_link_libraries(dynamic_frame_tf2_broadcaster PRIVATE 74 | ${geometry_msgs_TARGETS} 75 | rclcpp::rclcpp 76 | tf2_ros::tf2_ros 77 | ) 78 | 79 | add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp) 80 | target_link_libraries(turtle_tf2_message_filter PRIVATE 81 | ${geometry_msgs_TARGETS} 82 | message_filters::message_filters 83 | rclcpp::rclcpp 84 | ${tf2_geometry_msgs_TARGETS} 85 | tf2_ros::tf2_ros 86 | ) 87 | 88 | if(EXISTS ${TF2_CPP_HEADERS}) 89 | target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS) 90 | endif() 91 | 92 | install(TARGETS 93 | static_turtle_tf2_broadcaster 94 | turtle_tf2_broadcaster 95 | turtle_tf2_listener 96 | fixed_frame_tf2_broadcaster 97 | dynamic_frame_tf2_broadcaster 98 | turtle_tf2_message_filter 99 | DESTINATION lib/${PROJECT_NAME} 100 | ) 101 | 102 | install(DIRECTORY 103 | launch 104 | DESTINATION share/${PROJECT_NAME} 105 | ) 106 | 107 | if(BUILD_TESTING) 108 | find_package(ament_lint_auto REQUIRED) 109 | ament_lint_auto_find_test_dependencies() 110 | endif() 111 | 112 | ament_package() 113 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/launch/turtle_tf2_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch.substitutions import LaunchConfiguration 18 | 19 | from launch_ros.actions import Node 20 | 21 | 22 | def generate_launch_description(): 23 | return LaunchDescription([ 24 | DeclareLaunchArgument( 25 | 'target_frame', default_value='turtle1', 26 | description='Target frame name.' 27 | ), 28 | Node( 29 | package='turtlesim', 30 | executable='turtlesim_node', 31 | name='sim' 32 | ), 33 | Node( 34 | package='turtle_tf2_cpp', 35 | executable='turtle_tf2_broadcaster', 36 | name='broadcaster1', 37 | parameters=[ 38 | {'turtlename': 'turtle1'} 39 | ] 40 | ), 41 | Node( 42 | package='turtle_tf2_cpp', 43 | executable='turtle_tf2_broadcaster', 44 | name='broadcaster2', 45 | parameters=[ 46 | {'turtlename': 'turtle2'} 47 | ] 48 | ), 49 | Node( 50 | package='turtle_tf2_cpp', 51 | executable='turtle_tf2_listener', 52 | name='listener', 53 | parameters=[ 54 | {'target_frame': LaunchConfiguration('target_frame')} 55 | ] 56 | ), 57 | ]) 58 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/launch/turtle_tf2_dynamic_frame_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import IncludeLaunchDescription 21 | from launch.launch_description_sources import PythonLaunchDescriptionSource 22 | 23 | from launch_ros.actions import Node 24 | 25 | 26 | def generate_launch_description(): 27 | demo_nodes = IncludeLaunchDescription( 28 | PythonLaunchDescriptionSource([os.path.join( 29 | get_package_share_directory('turtle_tf2_cpp'), 'launch'), 30 | '/turtle_tf2_demo.launch.py']), 31 | launch_arguments={'target_frame': 'carrot1'}.items(), 32 | ) 33 | 34 | return LaunchDescription([ 35 | demo_nodes, 36 | Node( 37 | package='turtle_tf2_cpp', 38 | executable='dynamic_frame_tf2_broadcaster', 39 | name='dynamic_broadcaster', 40 | ), 41 | ]) 42 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/launch/turtle_tf2_fixed_frame_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import IncludeLaunchDescription 21 | from launch.launch_description_sources import PythonLaunchDescriptionSource 22 | 23 | from launch_ros.actions import Node 24 | 25 | 26 | def generate_launch_description(): 27 | demo_nodes = IncludeLaunchDescription( 28 | PythonLaunchDescriptionSource([os.path.join( 29 | get_package_share_directory('turtle_tf2_cpp'), 'launch'), 30 | '/turtle_tf2_demo.launch.py']), 31 | launch_arguments={'target_frame': 'carrot1'}.items(), 32 | ) 33 | 34 | return LaunchDescription([ 35 | demo_nodes, 36 | Node( 37 | package='turtle_tf2_cpp', 38 | executable='fixed_frame_tf2_broadcaster', 39 | name='fixed_broadcaster', 40 | ), 41 | ]) 42 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turtle_tf2_cpp 5 | 0.7.0 6 | 7 | turtle_tf2_cpp demonstrates how to write a ROS2 C++ tf2 broadcaster and listener with the turtlesim. The turtle_tf2_listener commands turtle2 to follow turtle1 around as you drive turtle1 using the keyboard. 8 | 9 | Alejandro Hernández Cordero 10 | Audrow Nash 11 | Apache License, Version 2.0 12 | 13 | Shyngyskhan Abilkassov 14 | 15 | ament_cmake 16 | 17 | geometry_msgs 18 | launch 19 | launch_ros 20 | message_filters 21 | rclcpp 22 | tf2 23 | tf2_geometry_msgs 24 | tf2_ros 25 | turtlesim_msgs 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "geometry_msgs/msg/transform_stamped.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "tf2_ros/transform_broadcaster.h" 22 | 23 | using namespace std::chrono_literals; 24 | 25 | const double PI = 3.141592653589793238463; 26 | 27 | class DynamicFrameBroadcaster : public rclcpp::Node 28 | { 29 | public: 30 | DynamicFrameBroadcaster() 31 | : Node("dynamic_frame_tf2_broadcaster") 32 | { 33 | tf_broadcaster_ = std::make_shared(this); 34 | timer_ = this->create_wall_timer( 35 | 100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this)); 36 | } 37 | 38 | private: 39 | void broadcast_timer_callback() 40 | { 41 | rclcpp::Time now = this->get_clock()->now(); 42 | double x = now.seconds() * PI; 43 | 44 | geometry_msgs::msg::TransformStamped t; 45 | t.header.stamp = now; 46 | t.header.frame_id = "turtle1"; 47 | t.child_frame_id = "carrot1"; 48 | t.transform.translation.x = 10 * sin(x); 49 | t.transform.translation.y = 10 * cos(x); 50 | t.transform.translation.z = 0.0; 51 | t.transform.rotation.x = 0.0; 52 | t.transform.rotation.y = 0.0; 53 | t.transform.rotation.z = 0.0; 54 | t.transform.rotation.w = 1.0; 55 | 56 | tf_broadcaster_->sendTransform(t); 57 | } 58 | 59 | rclcpp::TimerBase::SharedPtr timer_; 60 | std::shared_ptr tf_broadcaster_; 61 | }; 62 | 63 | int main(int argc, char * argv[]) 64 | { 65 | rclcpp::init(argc, argv); 66 | rclcpp::spin(std::make_shared()); 67 | rclcpp::shutdown(); 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "geometry_msgs/msg/transform_stamped.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "tf2_ros/transform_broadcaster.h" 22 | 23 | using namespace std::chrono_literals; 24 | 25 | class FixedFrameBroadcaster : public rclcpp::Node 26 | { 27 | public: 28 | FixedFrameBroadcaster() 29 | : Node("fixed_frame_tf2_broadcaster") 30 | { 31 | tf_broadcaster_ = std::make_shared(this); 32 | timer_ = this->create_wall_timer( 33 | 100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this)); 34 | } 35 | 36 | private: 37 | void broadcast_timer_callback() 38 | { 39 | geometry_msgs::msg::TransformStamped t; 40 | 41 | t.header.stamp = this->get_clock()->now(); 42 | t.header.frame_id = "turtle1"; 43 | t.child_frame_id = "carrot1"; 44 | t.transform.translation.x = 0.0; 45 | t.transform.translation.y = 2.0; 46 | t.transform.translation.z = 0.0; 47 | t.transform.rotation.x = 0.0; 48 | t.transform.rotation.y = 0.0; 49 | t.transform.rotation.z = 0.0; 50 | t.transform.rotation.w = 1.0; 51 | 52 | tf_broadcaster_->sendTransform(t); 53 | } 54 | 55 | rclcpp::TimerBase::SharedPtr timer_; 56 | std::shared_ptr tf_broadcaster_; 57 | }; 58 | 59 | int main(int argc, char * argv[]) 60 | { 61 | rclcpp::init(argc, argv); 62 | rclcpp::spin(std::make_shared()); 63 | rclcpp::shutdown(); 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "geometry_msgs/msg/transform_stamped.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "tf2/LinearMath/Quaternion.hpp" 20 | #include "tf2_ros/static_transform_broadcaster.h" 21 | 22 | class StaticFramePublisher : public rclcpp::Node 23 | { 24 | public: 25 | explicit StaticFramePublisher(char * transformation[]) 26 | : Node("static_turtle_tf2_broadcaster") 27 | { 28 | tf_static_broadcaster_ = std::make_shared(this); 29 | 30 | // Publish static transforms once at startup 31 | this->make_transforms(transformation); 32 | } 33 | 34 | private: 35 | void make_transforms(char * transformation[]) 36 | { 37 | geometry_msgs::msg::TransformStamped t; 38 | 39 | t.header.stamp = this->get_clock()->now(); 40 | t.header.frame_id = "world"; 41 | t.child_frame_id = transformation[1]; 42 | 43 | t.transform.translation.x = atof(transformation[2]); 44 | t.transform.translation.y = atof(transformation[3]); 45 | t.transform.translation.z = atof(transformation[4]); 46 | tf2::Quaternion q; 47 | q.setRPY( 48 | atof(transformation[5]), 49 | atof(transformation[6]), 50 | atof(transformation[7])); 51 | t.transform.rotation.x = q.x(); 52 | t.transform.rotation.y = q.y(); 53 | t.transform.rotation.z = q.z(); 54 | t.transform.rotation.w = q.w(); 55 | 56 | tf_static_broadcaster_->sendTransform(t); 57 | } 58 | 59 | std::shared_ptr tf_static_broadcaster_; 60 | }; 61 | 62 | int main(int argc, char * argv[]) 63 | { 64 | auto logger = rclcpp::get_logger("logger"); 65 | 66 | // Obtain parameters from command line arguments 67 | if (argc != 8) { 68 | RCLCPP_INFO( 69 | logger, "Invalid number of parameters\nusage: " 70 | "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster " 71 | "child_frame_name x y z roll pitch yaw"); 72 | return 1; 73 | } 74 | 75 | // As the parent frame of the transform is `world`, it is 76 | // necessary to check that the frame name passed is different 77 | if (strcmp(argv[1], "world") == 0) { 78 | RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'"); 79 | return 2; 80 | } 81 | 82 | // Pass parameters and initialize node 83 | rclcpp::init(argc, argv); 84 | rclcpp::spin(std::make_shared(argv)); 85 | rclcpp::shutdown(); 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "geometry_msgs/msg/transform_stamped.hpp" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "tf2/LinearMath/Quaternion.hpp" 23 | #include "tf2_ros/transform_broadcaster.h" 24 | #include "turtlesim_msgs/msg/pose.hpp" 25 | 26 | class FramePublisher : public rclcpp::Node 27 | { 28 | public: 29 | FramePublisher() 30 | : Node("turtle_tf2_frame_publisher") 31 | { 32 | // Declare and acquire `turtlename` parameter 33 | turtlename_ = this->declare_parameter("turtlename", "turtle"); 34 | 35 | // Initialize the transform broadcaster 36 | tf_broadcaster_ = 37 | std::make_unique(*this); 38 | 39 | // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose 40 | // callback function on each message 41 | std::ostringstream stream; 42 | stream << "/" << turtlename_.c_str() << "/pose"; 43 | std::string topic_name = stream.str(); 44 | 45 | auto handle_turtle_pose = [this](const std::shared_ptr msg) { 46 | geometry_msgs::msg::TransformStamped t; 47 | 48 | // Read message content and assign it to 49 | // corresponding tf variables 50 | t.header.stamp = this->get_clock()->now(); 51 | t.header.frame_id = "world"; 52 | t.child_frame_id = turtlename_.c_str(); 53 | 54 | // Turtle only exists in 2D, thus we get x and y translation 55 | // coordinates from the message and set the z coordinate to 0 56 | t.transform.translation.x = msg->x; 57 | t.transform.translation.y = msg->y; 58 | t.transform.translation.z = 0.0; 59 | 60 | // For the same reason, turtle can only rotate around one axis 61 | // and this why we set rotation in x and y to 0 and obtain 62 | // rotation in z axis from the message 63 | tf2::Quaternion q; 64 | q.setRPY(0, 0, msg->theta); 65 | t.transform.rotation.x = q.x(); 66 | t.transform.rotation.y = q.y(); 67 | t.transform.rotation.z = q.z(); 68 | t.transform.rotation.w = q.w(); 69 | 70 | // Send the transformation 71 | tf_broadcaster_->sendTransform(t); 72 | }; 73 | subscription_ = this->create_subscription( 74 | topic_name, 10, 75 | handle_turtle_pose); 76 | } 77 | 78 | private: 79 | rclcpp::Subscription::SharedPtr subscription_; 80 | std::unique_ptr tf_broadcaster_; 81 | std::string turtlename_; 82 | }; 83 | 84 | int main(int argc, char * argv[]) 85 | { 86 | rclcpp::init(argc, argv); 87 | rclcpp::spin(std::make_shared()); 88 | rclcpp::shutdown(); 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/turtle_tf2_listener.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "geometry_msgs/msg/transform_stamped.hpp" 21 | #include "geometry_msgs/msg/twist.hpp" 22 | #include "rclcpp/rclcpp.hpp" 23 | #include "tf2/exceptions.hpp" 24 | #include "tf2_ros/transform_listener.h" 25 | #include "tf2_ros/buffer.h" 26 | #include "turtlesim_msgs/srv/spawn.hpp" 27 | 28 | using namespace std::chrono_literals; 29 | 30 | class FrameListener : public rclcpp::Node 31 | { 32 | public: 33 | FrameListener() 34 | : Node("turtle_tf2_frame_listener"), 35 | turtle_spawning_service_ready_(false), 36 | turtle_spawned_(false) 37 | { 38 | // Declare and acquire `target_frame` parameter 39 | target_frame_ = this->declare_parameter("target_frame", "turtle1"); 40 | 41 | tf_buffer_ = 42 | std::make_unique(this->get_clock()); 43 | tf_listener_ = 44 | std::make_shared(*tf_buffer_); 45 | 46 | // Create a client to spawn a turtle 47 | spawner_ = 48 | this->create_client("spawn"); 49 | 50 | // Create turtle2 velocity publisher 51 | publisher_ = 52 | this->create_publisher("turtle2/cmd_vel", 1); 53 | 54 | // Call on_timer function every second 55 | timer_ = this->create_wall_timer( 56 | 1s, [this]() {return this->on_timer();}); 57 | } 58 | 59 | private: 60 | void on_timer() 61 | { 62 | // Store frame names in variables that will be used to 63 | // compute transformations 64 | std::string fromFrameRel = target_frame_.c_str(); 65 | std::string toFrameRel = "turtle2"; 66 | 67 | if (turtle_spawning_service_ready_) { 68 | if (turtle_spawned_) { 69 | geometry_msgs::msg::TransformStamped t; 70 | 71 | // Look up for the transformation between target_frame and turtle2 frames 72 | // and send velocity commands for turtle2 to reach target_frame 73 | try { 74 | t = tf_buffer_->lookupTransform( 75 | toFrameRel, fromFrameRel, 76 | tf2::TimePointZero); 77 | } catch (const tf2::TransformException & ex) { 78 | RCLCPP_INFO( 79 | this->get_logger(), "Could not transform %s to %s: %s", 80 | toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); 81 | return; 82 | } 83 | 84 | geometry_msgs::msg::Twist msg; 85 | 86 | static const double scaleRotationRate = 1.0; 87 | msg.angular.z = scaleRotationRate * atan2( 88 | t.transform.translation.y, 89 | t.transform.translation.x); 90 | 91 | static const double scaleForwardSpeed = 0.5; 92 | msg.linear.x = scaleForwardSpeed * sqrt( 93 | pow(t.transform.translation.x, 2) + 94 | pow(t.transform.translation.y, 2)); 95 | 96 | publisher_->publish(msg); 97 | } else { 98 | RCLCPP_INFO(this->get_logger(), "Successfully spawned"); 99 | turtle_spawned_ = true; 100 | } 101 | } else { 102 | // Check if the service is ready 103 | if (spawner_->service_is_ready()) { 104 | // Initialize request with turtle name and coordinates 105 | // Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn 106 | auto request = std::make_shared(); 107 | request->x = 4.0; 108 | request->y = 2.0; 109 | request->theta = 0.0; 110 | request->name = "turtle2"; 111 | 112 | // Call request 113 | using ServiceResponseFuture = 114 | rclcpp::Client::SharedFuture; 115 | auto response_received_callback = [this](ServiceResponseFuture future) { 116 | auto result = future.get(); 117 | if (strcmp(result->name.c_str(), "turtle2") == 0) { 118 | turtle_spawning_service_ready_ = true; 119 | } else { 120 | RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch"); 121 | } 122 | }; 123 | auto result = spawner_->async_send_request(request, response_received_callback); 124 | } else { 125 | RCLCPP_INFO(this->get_logger(), "Service is not ready"); 126 | } 127 | } 128 | } 129 | 130 | // Boolean values to store the information 131 | // if the service for spawning turtle is available 132 | bool turtle_spawning_service_ready_; 133 | // if the turtle was successfully spawned 134 | bool turtle_spawned_; 135 | rclcpp::Client::SharedPtr spawner_{nullptr}; 136 | rclcpp::TimerBase::SharedPtr timer_{nullptr}; 137 | rclcpp::Publisher::SharedPtr publisher_{nullptr}; 138 | std::shared_ptr tf_listener_{nullptr}; 139 | std::unique_ptr tf_buffer_; 140 | std::string target_frame_; 141 | }; 142 | 143 | int main(int argc, char * argv[]) 144 | { 145 | rclcpp::init(argc, argv); 146 | rclcpp::spin(std::make_shared()); 147 | rclcpp::shutdown(); 148 | return 0; 149 | } 150 | -------------------------------------------------------------------------------- /turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "geometry_msgs/msg/point_stamped.hpp" 20 | #include "message_filters/subscriber.hpp" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "tf2_ros/buffer.h" 23 | #include "tf2_ros/create_timer_ros.h" 24 | #include "tf2_ros/message_filter.h" 25 | #include "tf2_ros/transform_listener.h" 26 | #ifdef TF2_CPP_HEADERS 27 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 28 | #else 29 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 30 | #endif 31 | 32 | using namespace std::chrono_literals; 33 | 34 | class PoseDrawer : public rclcpp::Node 35 | { 36 | public: 37 | PoseDrawer() 38 | : Node("turtle_tf2_pose_drawer") 39 | { 40 | // Declare and acquire `target_frame` parameter 41 | target_frame_ = this->declare_parameter("target_frame", "turtle1"); 42 | 43 | std::chrono::duration buffer_timeout(1); 44 | 45 | tf2_buffer_ = std::make_shared(this->get_clock()); 46 | // Create the timer interface before call to waitForTransform, 47 | // to avoid a tf2_ros::CreateTimerInterfaceException exception 48 | auto timer_interface = std::make_shared( 49 | this->get_node_base_interface(), 50 | this->get_node_timers_interface()); 51 | tf2_buffer_->setCreateTimerInterface(timer_interface); 52 | tf2_listener_ = 53 | std::make_shared(*tf2_buffer_); 54 | 55 | point_sub_.subscribe(this, "/turtle3/turtle_point_stamped", rclcpp::QoS(10)); 56 | tf2_filter_ = std::make_shared>( 57 | point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(), 58 | this->get_node_clock_interface(), buffer_timeout); 59 | // Register a callback with tf2_ros::MessageFilter to be called when transforms are available 60 | tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this); 61 | } 62 | 63 | private: 64 | void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr) 65 | { 66 | geometry_msgs::msg::PointStamped point_out; 67 | try { 68 | tf2_buffer_->transform(*point_ptr, point_out, target_frame_); 69 | RCLCPP_INFO( 70 | this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n", 71 | point_out.point.x, 72 | point_out.point.y, 73 | point_out.point.z); 74 | } catch (const tf2::TransformException & ex) { 75 | RCLCPP_WARN( 76 | // Print exception which was caught 77 | this->get_logger(), "Failure %s\n", ex.what()); 78 | } 79 | } 80 | std::string target_frame_; 81 | std::shared_ptr tf2_buffer_; 82 | std::shared_ptr tf2_listener_; 83 | message_filters::Subscriber point_sub_; 84 | std::shared_ptr> tf2_filter_; 85 | }; 86 | 87 | int main(int argc, char * argv[]) 88 | { 89 | rclcpp::init(argc, argv); 90 | rclcpp::spin(std::make_shared()); 91 | rclcpp::shutdown(); 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /turtle_tf2_py/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtle_tf2_py 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.0 (2025-05-05) 6 | ------------------ 7 | * Fix incorrect srv import (`#88 `_) 8 | * Contributors: suchetanrs 9 | 10 | 0.6.3 (2024-12-27) 11 | ------------------ 12 | * Switch to using a context manager for rclpy initialization. (`#85 `_) 13 | This ensures that everything will be properly cleaned up 14 | when we leave the context manager. 15 | * Contributors: Chris Lalancette 16 | 17 | 0.6.2 (2024-07-19) 18 | ------------------ 19 | 20 | 0.6.1 (2024-07-16) 21 | ------------------ 22 | 23 | 0.6.0 (2024-07-10) 24 | ------------------ 25 | * Used turtlesim_msgs (`#78 `_) 26 | * Fix a few more minor nitpicks. (`#72 `_) 27 | 1. Remove dependencies from the targets that don't need them. 28 | 2. Remove a totally unnecessary typedef. 29 | 3. Remove unnecessary casts to float. 30 | * Contributors: Alejandro Hernández Cordero, Chris Lalancette 31 | 32 | 0.3.6 (2022-09-15) 33 | ------------------ 34 | * Minor cleanups across the tutorials. (`#71 `_) 35 | * Contributors: Chris Lalancette 36 | 37 | 0.3.5 (2022-09-08) 38 | ------------------ 39 | * Remove the dependency on tf_transformations. (`#69 `_) 40 | * Contributors: Chris Lalancette 41 | 42 | 0.3.4 (2021-10-11) 43 | ------------------ 44 | * Add source code and launch file for tf2 PointStamped message publisher and listener/filter (`#62 `_) 45 | Add Python Code of PointStamped Messages Broadcaster Node 46 | Add launch File turtle_tf2_sensor_message.launch.py 47 | Update CMakeLists.txt for turtle_tf2_message_filter node 48 | Update package.xml for dependencies of turtle_tf2_message_filter.cpp 49 | Update turtle_tf2_message_filter.cpp after checking linters. 50 | for the copyright year from 2015 to 2021 and delete the line "self.sub". 51 | sort include headers; change two node instances to one instance; change turtle3 spawning manner. 52 | remove whitespaces in line 39 and 42 53 | some little updates 54 | * Contributors: kenny_wang 55 | 56 | 0.3.3 (2021-08-27) 57 | ------------------ 58 | * update python tf2 listener node (`#58 `_) 59 | * restructure code in static transform broadcaster (`#57 `_) 60 | * move TransformBroadcaster to the init call to resolve memory leak (`#56 `_) 61 | * Contributors: kurshakuz 62 | 63 | 0.3.2 (2021-08-09) 64 | ------------------ 65 | * Add fixed and dynamic frame broadcaster nodes (`#40 `_) 66 | Co-authored-by: Alejandro Hernández Cordero 67 | * Add target frame parameter to the listener node (`#41 `_) 68 | * fix linter style issues and remove spin_thread=False statement (`#39 `_) 69 | * Contributors: kurshakuz 70 | 71 | 0.3.1 (2021-06-23) 72 | ------------------ 73 | * Replace dependency on transforms3d pip package to tf_transformations ROS2 package (`#38 `_) 74 | * Contributors: kurshakuz 75 | 76 | 0.0.3 77 | ----- 78 | * Add Audrow as a maintainer and move Shyngys to author (`#37 `_) 79 | * Fix linters (`#35 `_) 80 | * Migrate turtle_tf2 tutorial package to ROS2 (`#34 `_) 81 | * Contributors: Alejandro Hernández Cordero, kurshakuz, Audrow Nash 82 | -------------------------------------------------------------------------------- /turtle_tf2_py/launch/turtle_tf2_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch.substitutions import LaunchConfiguration 18 | 19 | from launch_ros.actions import Node 20 | 21 | 22 | def generate_launch_description(): 23 | return LaunchDescription([ 24 | DeclareLaunchArgument( 25 | 'target_frame', default_value='turtle1', 26 | description='Target frame name.' 27 | ), 28 | Node( 29 | package='turtlesim', 30 | executable='turtlesim_node', 31 | name='sim' 32 | ), 33 | Node( 34 | package='turtle_tf2_py', 35 | executable='turtle_tf2_broadcaster', 36 | name='broadcaster1', 37 | parameters=[ 38 | {'turtlename': 'turtle1'} 39 | ] 40 | ), 41 | Node( 42 | package='turtle_tf2_py', 43 | executable='turtle_tf2_broadcaster', 44 | name='broadcaster2', 45 | parameters=[ 46 | {'turtlename': 'turtle2'} 47 | ] 48 | ), 49 | Node( 50 | package='turtle_tf2_py', 51 | executable='turtle_tf2_listener', 52 | name='listener', 53 | parameters=[ 54 | {'target_frame': LaunchConfiguration('target_frame')} 55 | ] 56 | ), 57 | ]) 58 | -------------------------------------------------------------------------------- /turtle_tf2_py/launch/turtle_tf2_dynamic_frame_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import IncludeLaunchDescription 21 | from launch.launch_description_sources import PythonLaunchDescriptionSource 22 | 23 | from launch_ros.actions import Node 24 | 25 | 26 | def generate_launch_description(): 27 | demo_nodes = IncludeLaunchDescription( 28 | PythonLaunchDescriptionSource([os.path.join( 29 | get_package_share_directory('turtle_tf2_py'), 'launch'), 30 | '/turtle_tf2_demo.launch.py']), 31 | launch_arguments={'target_frame': 'carrot1'}.items(), 32 | ) 33 | 34 | return LaunchDescription([ 35 | demo_nodes, 36 | Node( 37 | package='turtle_tf2_py', 38 | executable='dynamic_frame_tf2_broadcaster', 39 | name='dynamic_broadcaster', 40 | ), 41 | ]) 42 | -------------------------------------------------------------------------------- /turtle_tf2_py/launch/turtle_tf2_fixed_frame_demo.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import os 16 | 17 | from ament_index_python.packages import get_package_share_directory 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import IncludeLaunchDescription 21 | from launch.launch_description_sources import PythonLaunchDescriptionSource 22 | 23 | from launch_ros.actions import Node 24 | 25 | 26 | def generate_launch_description(): 27 | demo_nodes = IncludeLaunchDescription( 28 | PythonLaunchDescriptionSource([os.path.join( 29 | get_package_share_directory('turtle_tf2_py'), 'launch'), 30 | '/turtle_tf2_demo.launch.py']), 31 | launch_arguments={'target_frame': 'carrot1'}.items(), 32 | ) 33 | 34 | return LaunchDescription([ 35 | demo_nodes, 36 | Node( 37 | package='turtle_tf2_py', 38 | executable='fixed_frame_tf2_broadcaster', 39 | name='fixed_broadcaster', 40 | ), 41 | ]) 42 | -------------------------------------------------------------------------------- /turtle_tf2_py/launch/turtle_tf2_sensor_message.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from launch import LaunchDescription 16 | from launch.actions import DeclareLaunchArgument 17 | from launch_ros.actions import Node 18 | 19 | 20 | def generate_launch_description(): 21 | return LaunchDescription([ 22 | DeclareLaunchArgument( 23 | 'target_frame', default_value='turtle1', 24 | description='Target frame name.' 25 | ), 26 | Node( 27 | package='turtlesim', 28 | executable='turtlesim_node', 29 | name='sim', 30 | output='screen' 31 | ), 32 | Node( 33 | package='turtle_tf2_py', 34 | executable='turtle_tf2_broadcaster', 35 | name='broadcaster1', 36 | parameters=[ 37 | {'turtlename': 'turtle1'} 38 | ] 39 | ), 40 | Node( 41 | package='turtle_tf2_py', 42 | executable='turtle_tf2_broadcaster', 43 | name='broadcaster2', 44 | parameters=[ 45 | {'turtlename': 'turtle3'} 46 | ] 47 | ), 48 | Node( 49 | package='turtle_tf2_py', 50 | executable='turtle_tf2_message_broadcaster', 51 | name='message_broadcaster', 52 | ), 53 | ]) 54 | -------------------------------------------------------------------------------- /turtle_tf2_py/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turtle_tf2_py 5 | 0.7.0 6 | 7 | turtle_tf2_py demonstrates how to write a ROS2 Python tf2 broadcaster and listener with the turtlesim. The turtle_tf2_listener commands turtle2 to follow turtle1 around as you drive turtle1 using the keyboard. 8 | 9 | Alejandro Hernández Cordero 10 | Audrow Nash 11 | Apache License, Version 2.0 12 | BSD 13 | 14 | Shyngyskhan Abilkassov 15 | 16 | geometry_msgs 17 | launch 18 | launch_ros 19 | python3-numpy 20 | rclpy 21 | tf2_ros 22 | turtlesim_msgs 23 | 24 | ament_copyright 25 | ament_flake8 26 | ament_pep257 27 | python3-pytest 28 | 29 | 30 | ament_python 31 | 32 | 33 | -------------------------------------------------------------------------------- /turtle_tf2_py/resource/turtle_tf2_py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/geometry_tutorials/79ee77b908c8fc3f3b7fbbfa5ebfa65ad0018882/turtle_tf2_py/resource/turtle_tf2_py -------------------------------------------------------------------------------- /turtle_tf2_py/rviz/turtle_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1 9 | - /TF1/Frames1 10 | - /TF1/Frames1/turtle11 11 | - /TF1/Frames1/turtle21 12 | Splitter Ratio: 0.5 13 | Tree Height: 553 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | Visualization Manager: 28 | Class: "" 29 | Displays: 30 | - Alpha: 0.5 31 | Cell Size: 1 32 | Class: rviz_default_plugins/Grid 33 | Color: 160; 160; 164 34 | Enabled: true 35 | Line Style: 36 | Line Width: 0.029999999329447746 37 | Value: Lines 38 | Name: Grid 39 | Normal Cell Count: 0 40 | Offset: 41 | X: 0 42 | Y: 0 43 | Z: 0 44 | Plane: XY 45 | Plane Cell Count: 10 46 | Reference Frame: 47 | Value: true 48 | - Class: rviz_default_plugins/TF 49 | Enabled: true 50 | Frame Timeout: 15 51 | Frames: 52 | All Enabled: true 53 | turtle1: 54 | Value: true 55 | turtle2: 56 | Value: true 57 | world: 58 | Value: true 59 | Marker Scale: 1 60 | Name: TF 61 | Show Arrows: true 62 | Show Axes: true 63 | Show Names: false 64 | Tree: 65 | world: 66 | turtle1: 67 | {} 68 | turtle2: 69 | {} 70 | Update Interval: 0 71 | Value: true 72 | Enabled: true 73 | Global Options: 74 | Background Color: 48; 48; 48 75 | Fixed Frame: world 76 | Frame Rate: 30 77 | Name: root 78 | Tools: 79 | - Class: rviz_default_plugins/Interact 80 | Hide Inactive Objects: true 81 | - Class: rviz_default_plugins/MoveCamera 82 | - Class: rviz_default_plugins/Select 83 | - Class: rviz_default_plugins/FocusCamera 84 | - Class: rviz_default_plugins/Measure 85 | Line color: 128; 128; 0 86 | - Class: rviz_default_plugins/SetInitialPose 87 | Topic: 88 | Depth: 5 89 | Durability Policy: Volatile 90 | History Policy: Keep Last 91 | Reliability Policy: Reliable 92 | Value: /initialpose 93 | - Class: rviz_default_plugins/SetGoal 94 | Topic: 95 | Depth: 5 96 | Durability Policy: Volatile 97 | History Policy: Keep Last 98 | Reliability Policy: Reliable 99 | Value: /goal_pose 100 | - Class: rviz_default_plugins/PublishPoint 101 | Single click: true 102 | Topic: 103 | Depth: 5 104 | Durability Policy: Volatile 105 | History Policy: Keep Last 106 | Reliability Policy: Reliable 107 | Value: /clicked_point 108 | Transformation: 109 | Current: 110 | Class: rviz_default_plugins/TF 111 | Value: true 112 | Views: 113 | Current: 114 | Angle: 0 115 | Class: rviz_default_plugins/TopDownOrtho 116 | Enable Stereo Rendering: 117 | Stereo Eye Separation: 0.05999999865889549 118 | Stereo Focal Distance: 1 119 | Swap Stereo Eyes: false 120 | Value: false 121 | Invert Z Axis: false 122 | Name: Current View 123 | Near Clip Distance: 0.009999999776482582 124 | Scale: 48.638980865478516 125 | Target Frame: 126 | Value: TopDownOrtho (rviz_default_plugins) 127 | X: 0 128 | Y: 0 129 | Saved: 130 | - Angle: 0 131 | Class: rviz_default_plugins/TopDownOrtho 132 | Enable Stereo Rendering: 133 | Stereo Eye Separation: 0.05999999865889549 134 | Stereo Focal Distance: 1 135 | Swap Stereo Eyes: false 136 | Value: false 137 | Invert Z Axis: false 138 | Name: TopDownOrtho 139 | Near Clip Distance: 0.009999999776482582 140 | Scale: 10 141 | Target Frame: 142 | Value: TopDownOrtho (rviz_default_plugins) 143 | X: 0 144 | Y: 0 145 | Window Geometry: 146 | Displays: 147 | collapsed: false 148 | Height: 704 149 | Hide Left Dock: false 150 | Hide Right Dock: false 151 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000266fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000266000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000266fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000266000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003be0000026600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 152 | Selection: 153 | collapsed: false 154 | Tool Properties: 155 | collapsed: false 156 | Views: 157 | collapsed: false 158 | Width: 1306 159 | X: 60 160 | Y: 27 161 | -------------------------------------------------------------------------------- /turtle_tf2_py/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/turtle_tf2_py 3 | [install] 4 | install_scripts=$base/lib/turtle_tf2_py 5 | -------------------------------------------------------------------------------- /turtle_tf2_py/setup.py: -------------------------------------------------------------------------------- 1 | from glob import glob 2 | import os 3 | 4 | from setuptools import setup 5 | 6 | package_name = 'turtle_tf2_py' 7 | 8 | setup( 9 | name=package_name, 10 | version='0.7.0', 11 | packages=[package_name], 12 | data_files=[ 13 | ('share/ament_index/resource_index/packages', ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name, 'launch'), 16 | glob(os.path.join('launch', '*.launch.py'))), 17 | (os.path.join('share', package_name, 'rviz'), 18 | glob(os.path.join('rviz', '*.rviz'))), 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | author='Shyngyskhan Abilkassov', 23 | author_email='abilkasov@gmail.com', 24 | maintainer='Alejandro Hernández Cordero, Audrow Nash', 25 | maintainer_email='alejandro@openrobotics.org, audrow@openrobotics.org', 26 | description=( 27 | 'turtle_tf2_py demonstrates how to write a ROS2 Python tf2 broadcaster and ' 28 | 'listener with the turtlesim. The turtle_tf2_listener commands turtle2 to ' 29 | 'follow turtle1 around as you drive turtle1 using the keyboard.' 30 | ), 31 | license='Apache License, Version 2.0', 32 | tests_require=['pytest'], 33 | entry_points={ 34 | 'console_scripts': [ 35 | 'static_turtle_tf2_broadcaster = turtle_tf2_py.static_turtle_tf2_broadcaster:main', 36 | 'turtle_tf2_broadcaster = turtle_tf2_py.turtle_tf2_broadcaster:main', 37 | 'turtle_tf2_listener = turtle_tf2_py.turtle_tf2_listener:main', 38 | 'fixed_frame_tf2_broadcaster = turtle_tf2_py.fixed_frame_tf2_broadcaster:main', 39 | 'dynamic_frame_tf2_broadcaster = turtle_tf2_py.dynamic_frame_tf2_broadcaster:main', 40 | 'turtle_tf2_message_broadcaster = turtle_tf2_py.turtle_tf2_message_broadcaster:main', 41 | ], 42 | }, 43 | ) 44 | -------------------------------------------------------------------------------- /turtle_tf2_py/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /turtle_tf2_py/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /turtle_tf2_py/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/geometry_tutorials/79ee77b908c8fc3f3b7fbbfa5ebfa65ad0018882/turtle_tf2_py/turtle_tf2_py/__init__.py -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import math 16 | 17 | from geometry_msgs.msg import TransformStamped 18 | 19 | import rclpy 20 | from rclpy.executors import ExternalShutdownException 21 | from rclpy.node import Node 22 | 23 | from tf2_ros import TransformBroadcaster 24 | 25 | 26 | class DynamicFrameBroadcaster(Node): 27 | 28 | def __init__(self): 29 | super().__init__('dynamic_frame_tf2_broadcaster') 30 | self.tf_broadcaster = TransformBroadcaster(self) 31 | self.timer = self.create_timer(0.1, self.broadcast_timer_callback) 32 | 33 | def broadcast_timer_callback(self): 34 | seconds, _ = self.get_clock().now().seconds_nanoseconds() 35 | x = seconds * math.pi 36 | 37 | t = TransformStamped() 38 | t.header.stamp = self.get_clock().now().to_msg() 39 | t.header.frame_id = 'turtle1' 40 | t.child_frame_id = 'carrot1' 41 | t.transform.translation.x = 10 * math.sin(x) 42 | t.transform.translation.y = 10 * math.cos(x) 43 | t.transform.translation.z = 0.0 44 | t.transform.rotation.x = 0.0 45 | t.transform.rotation.y = 0.0 46 | t.transform.rotation.z = 0.0 47 | t.transform.rotation.w = 1.0 48 | 49 | self.tf_broadcaster.sendTransform(t) 50 | 51 | 52 | def main(): 53 | try: 54 | with rclpy.init(): 55 | node = DynamicFrameBroadcaster() 56 | rclpy.spin(node) 57 | except (KeyboardInterrupt, ExternalShutdownException): 58 | pass 59 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from geometry_msgs.msg import TransformStamped 16 | 17 | import rclpy 18 | from rclpy.executors import ExternalShutdownException 19 | from rclpy.node import Node 20 | 21 | from tf2_ros import TransformBroadcaster 22 | 23 | 24 | class FixedFrameBroadcaster(Node): 25 | 26 | def __init__(self): 27 | super().__init__('fixed_frame_tf2_broadcaster') 28 | self.tf_broadcaster = TransformBroadcaster(self) 29 | self.timer = self.create_timer(0.1, self.broadcast_timer_callback) 30 | 31 | def broadcast_timer_callback(self): 32 | t = TransformStamped() 33 | 34 | t.header.stamp = self.get_clock().now().to_msg() 35 | t.header.frame_id = 'turtle1' 36 | t.child_frame_id = 'carrot1' 37 | t.transform.translation.x = 0.0 38 | t.transform.translation.y = 2.0 39 | t.transform.translation.z = 0.0 40 | t.transform.rotation.x = 0.0 41 | t.transform.rotation.y = 0.0 42 | t.transform.rotation.z = 0.0 43 | t.transform.rotation.w = 1.0 44 | 45 | self.tf_broadcaster.sendTransform(t) 46 | 47 | 48 | def main(): 49 | try: 50 | with rclpy.init(): 51 | node = FixedFrameBroadcaster() 52 | rclpy.spin(node) 53 | except (KeyboardInterrupt, ExternalShutdownException): 54 | pass 55 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import math 16 | import sys 17 | 18 | from geometry_msgs.msg import TransformStamped 19 | 20 | import numpy as np 21 | 22 | import rclpy 23 | from rclpy.executors import ExternalShutdownException 24 | from rclpy.node import Node 25 | 26 | from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster 27 | 28 | 29 | # This function is a stripped down version of the code in 30 | # https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py 31 | # Besides simplifying it, this version also inverts the order to return x,y,z,w, which is 32 | # the way that ROS prefers it. 33 | def quaternion_from_euler(ai, aj, ak): 34 | ai /= 2.0 35 | aj /= 2.0 36 | ak /= 2.0 37 | ci = math.cos(ai) 38 | si = math.sin(ai) 39 | cj = math.cos(aj) 40 | sj = math.sin(aj) 41 | ck = math.cos(ak) 42 | sk = math.sin(ak) 43 | cc = ci*ck 44 | cs = ci*sk 45 | sc = si*ck 46 | ss = si*sk 47 | 48 | q = np.empty((4, )) 49 | q[0] = cj*sc - sj*cs 50 | q[1] = cj*ss + sj*cc 51 | q[2] = cj*cs - sj*sc 52 | q[3] = cj*cc + sj*ss 53 | 54 | return q 55 | 56 | 57 | class StaticFramePublisher(Node): 58 | """ 59 | Broadcast transforms that never change. 60 | 61 | This example publishes transforms from `world` to a static turtle frame. 62 | The transforms are only published once at startup, and are constant for all 63 | time. 64 | """ 65 | 66 | def __init__(self, transformation): 67 | super().__init__('static_turtle_tf2_broadcaster') 68 | 69 | self.tf_static_broadcaster = StaticTransformBroadcaster(self) 70 | 71 | # Publish static transforms once at startup 72 | self.make_transforms(transformation) 73 | 74 | def make_transforms(self, transformation): 75 | t = TransformStamped() 76 | 77 | t.header.stamp = self.get_clock().now().to_msg() 78 | t.header.frame_id = 'world' 79 | t.child_frame_id = transformation[1] 80 | 81 | t.transform.translation.x = float(transformation[2]) 82 | t.transform.translation.y = float(transformation[3]) 83 | t.transform.translation.z = float(transformation[4]) 84 | quat = quaternion_from_euler( 85 | float(transformation[5]), float(transformation[6]), float(transformation[7])) 86 | t.transform.rotation.x = quat[0] 87 | t.transform.rotation.y = quat[1] 88 | t.transform.rotation.z = quat[2] 89 | t.transform.rotation.w = quat[3] 90 | 91 | self.tf_static_broadcaster.sendTransform(t) 92 | 93 | 94 | def main(): 95 | logger = rclpy.logging.get_logger('logger') 96 | 97 | # obtain parameters from command line arguments 98 | if len(sys.argv) != 8: 99 | logger.info('Invalid number of parameters. Usage: \n' 100 | '$ ros2 run turtle_tf2_py static_turtle_tf2_broadcaster' 101 | 'child_frame_name x y z roll pitch yaw') 102 | sys.exit(1) 103 | 104 | if sys.argv[1] == 'world': 105 | logger.info('Your static turtle name cannot be "world"') 106 | sys.exit(2) 107 | 108 | try: 109 | with rclpy.init(): 110 | # pass parameters and initialize node 111 | node = StaticFramePublisher(sys.argv) 112 | rclpy.spin(node) 113 | except (KeyboardInterrupt, ExternalShutdownException): 114 | pass 115 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import math 16 | 17 | from geometry_msgs.msg import TransformStamped 18 | 19 | import numpy as np 20 | 21 | import rclpy 22 | from rclpy.executors import ExternalShutdownException 23 | from rclpy.node import Node 24 | 25 | from tf2_ros import TransformBroadcaster 26 | 27 | from turtlesim_msgs.msg import Pose 28 | 29 | 30 | # This function is a stripped down version of the code in 31 | # https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py 32 | # Besides simplifying it, this version also inverts the order to return x,y,z,w, which is 33 | # the way that ROS prefers it. 34 | def quaternion_from_euler(ai, aj, ak): 35 | ai /= 2.0 36 | aj /= 2.0 37 | ak /= 2.0 38 | ci = math.cos(ai) 39 | si = math.sin(ai) 40 | cj = math.cos(aj) 41 | sj = math.sin(aj) 42 | ck = math.cos(ak) 43 | sk = math.sin(ak) 44 | cc = ci*ck 45 | cs = ci*sk 46 | sc = si*ck 47 | ss = si*sk 48 | 49 | q = np.empty((4, )) 50 | q[0] = cj*sc - sj*cs 51 | q[1] = cj*ss + sj*cc 52 | q[2] = cj*cs - sj*sc 53 | q[3] = cj*cc + sj*ss 54 | 55 | return q 56 | 57 | 58 | class FramePublisher(Node): 59 | 60 | def __init__(self): 61 | super().__init__('turtle_tf2_frame_publisher') 62 | 63 | # Declare and acquire `turtlename` parameter 64 | self.turtlename = self.declare_parameter( 65 | 'turtlename', 'turtle').get_parameter_value().string_value 66 | 67 | # Initialize the transform broadcaster 68 | self.tf_broadcaster = TransformBroadcaster(self) 69 | 70 | # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose 71 | # callback function on each message 72 | self.subscription = self.create_subscription( 73 | Pose, 74 | f'/{self.turtlename}/pose', 75 | self.handle_turtle_pose, 76 | 1) 77 | self.subscription # prevent unused variable warning 78 | 79 | def handle_turtle_pose(self, msg): 80 | t = TransformStamped() 81 | 82 | # Read message content and assign it to 83 | # corresponding tf variables 84 | t.header.stamp = self.get_clock().now().to_msg() 85 | t.header.frame_id = 'world' 86 | t.child_frame_id = self.turtlename 87 | 88 | # Turtle only exists in 2D, thus we get x and y translation 89 | # coordinates from the message and set the z coordinate to 0 90 | t.transform.translation.x = msg.x 91 | t.transform.translation.y = msg.y 92 | t.transform.translation.z = 0.0 93 | 94 | # For the same reason, turtle can only rotate around one axis 95 | # and this why we set rotation in x and y to 0 and obtain 96 | # rotation in z axis from the message 97 | q = quaternion_from_euler(0, 0, msg.theta) 98 | t.transform.rotation.x = q[0] 99 | t.transform.rotation.y = q[1] 100 | t.transform.rotation.z = q[2] 101 | t.transform.rotation.w = q[3] 102 | 103 | # Send the transformation 104 | self.tf_broadcaster.sendTransform(t) 105 | 106 | 107 | def main(): 108 | try: 109 | with rclpy.init(): 110 | node = FramePublisher() 111 | rclpy.spin(node) 112 | except (KeyboardInterrupt, ExternalShutdownException): 113 | pass 114 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import math 16 | 17 | from geometry_msgs.msg import Twist 18 | 19 | import rclpy 20 | from rclpy.executors import ExternalShutdownException 21 | from rclpy.node import Node 22 | 23 | from tf2_ros import TransformException 24 | from tf2_ros.buffer import Buffer 25 | from tf2_ros.transform_listener import TransformListener 26 | 27 | from turtlesim_msgs.srv import Spawn 28 | 29 | 30 | class FrameListener(Node): 31 | 32 | def __init__(self): 33 | super().__init__('turtle_tf2_frame_listener') 34 | 35 | # Declare and acquire `target_frame` parameter 36 | self.target_frame = self.declare_parameter( 37 | 'target_frame', 'turtle1').get_parameter_value().string_value 38 | 39 | self.tf_buffer = Buffer() 40 | self.tf_listener = TransformListener(self.tf_buffer, self) 41 | 42 | # Create a client to spawn a turtle 43 | self.spawner = self.create_client(Spawn, 'spawn') 44 | # Boolean values to store the information 45 | # if the service for spawning turtle is available 46 | self.turtle_spawning_service_ready = False 47 | # if the turtle was successfully spawned 48 | self.turtle_spawned = False 49 | 50 | # Create turtle2 velocity publisher 51 | self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) 52 | 53 | # Call on_timer function every second 54 | self.timer = self.create_timer(1.0, self.on_timer) 55 | 56 | def on_timer(self): 57 | # Store frame names in variables that will be used to 58 | # compute transformations 59 | from_frame_rel = self.target_frame 60 | to_frame_rel = 'turtle2' 61 | 62 | if self.turtle_spawning_service_ready: 63 | if self.turtle_spawned: 64 | # Look up for the transformation between target_frame and turtle2 frames 65 | # and send velocity commands for turtle2 to reach target_frame 66 | try: 67 | t = self.tf_buffer.lookup_transform( 68 | to_frame_rel, 69 | from_frame_rel, 70 | rclpy.time.Time()) 71 | except TransformException as ex: 72 | self.get_logger().info( 73 | f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}') 74 | return 75 | 76 | msg = Twist() 77 | scale_rotation_rate = 1.0 78 | msg.angular.z = scale_rotation_rate * math.atan2( 79 | t.transform.translation.y, 80 | t.transform.translation.x) 81 | 82 | scale_forward_speed = 0.5 83 | msg.linear.x = scale_forward_speed * math.sqrt( 84 | t.transform.translation.x ** 2 + 85 | t.transform.translation.y ** 2) 86 | 87 | self.publisher.publish(msg) 88 | else: 89 | if self.result.done(): 90 | self.get_logger().info( 91 | f'Successfully spawned {self.result.result().name}') 92 | self.turtle_spawned = True 93 | else: 94 | self.get_logger().info('Spawn is not finished') 95 | else: 96 | if self.spawner.service_is_ready(): 97 | # Initialize request with turtle name and coordinates 98 | # Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn 99 | request = Spawn.Request() 100 | request.name = 'turtle2' 101 | request.x = 4.0 102 | request.y = 2.0 103 | request.theta = 0.0 104 | # Call request 105 | self.result = self.spawner.call_async(request) 106 | self.turtle_spawning_service_ready = True 107 | else: 108 | # Check if the service is ready 109 | self.get_logger().info('Service is not ready') 110 | 111 | 112 | def main(): 113 | try: 114 | with rclpy.init(): 115 | node = FrameListener() 116 | rclpy.spin(node) 117 | except (KeyboardInterrupt, ExternalShutdownException): 118 | pass 119 | -------------------------------------------------------------------------------- /turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from geometry_msgs.msg import PointStamped 16 | from geometry_msgs.msg import Twist 17 | 18 | import rclpy 19 | from rclpy.executors import ExternalShutdownException 20 | from rclpy.node import Node 21 | 22 | from turtlesim_msgs.msg import Pose 23 | from turtlesim_msgs.srv import Spawn 24 | 25 | 26 | class PointPublisher(Node): 27 | 28 | def __init__(self): 29 | super().__init__('turtle_tf2_message_broadcaster') 30 | 31 | # Create a client to spawn a turtle 32 | self.spawner = self.create_client(Spawn, 'spawn') 33 | # Boolean values to store the information 34 | # if the service for spawning turtle is available 35 | self.turtle_spawning_service_ready = False 36 | # if the turtle was successfully spawned 37 | self.turtle_spawned = False 38 | # if the topics of turtle3 can be subscribed 39 | self.turtle_pose_cansubscribe = False 40 | 41 | self.timer = self.create_timer(1.0, self.on_timer) 42 | 43 | def on_timer(self): 44 | if self.turtle_spawning_service_ready: 45 | if self.turtle_spawned: 46 | self.turtle_pose_cansubscribe = True 47 | else: 48 | if self.result.done(): 49 | self.get_logger().info( 50 | f'Successfully spawned {self.result.result().name}') 51 | self.turtle_spawned = True 52 | else: 53 | self.get_logger().info('Spawn is not finished') 54 | else: 55 | if self.spawner.service_is_ready(): 56 | # Initialize request with turtle name and coordinates 57 | # Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn 58 | request = Spawn.Request() 59 | request.name = 'turtle3' 60 | request.x = 4.0 61 | request.y = 2.0 62 | request.theta = 0.0 63 | # Call request 64 | self.result = self.spawner.call_async(request) 65 | self.turtle_spawning_service_ready = True 66 | else: 67 | # Check if the service is ready 68 | self.get_logger().info('Service is not ready') 69 | 70 | if self.turtle_pose_cansubscribe: 71 | self.vel_pub = self.create_publisher(Twist, 'turtle3/cmd_vel', 10) 72 | self.sub = self.create_subscription(Pose, 'turtle3/pose', self.handle_turtle_pose, 10) 73 | self.pub = self.create_publisher(PointStamped, 'turtle3/turtle_point_stamped', 10) 74 | 75 | def handle_turtle_pose(self, msg): 76 | vel_msg = Twist() 77 | vel_msg.linear.x = 1.0 78 | vel_msg.angular.z = 1.0 79 | self.vel_pub.publish(vel_msg) 80 | 81 | ps = PointStamped() 82 | ps.header.stamp = self.get_clock().now().to_msg() 83 | ps.header.frame_id = 'world' 84 | ps.point.x = msg.x 85 | ps.point.y = msg.y 86 | ps.point.z = 0.0 87 | self.pub.publish(ps) 88 | 89 | 90 | def main(): 91 | try: 92 | with rclpy.init(): 93 | node = PointPublisher() 94 | rclpy.spin(node) 95 | except (KeyboardInterrupt, ExternalShutdownException): 96 | pass 97 | --------------------------------------------------------------------------------