├── turtlesim ├── CHANGELOG.rst ├── CMakeLists.txt ├── images │ ├── ardent.png │ ├── bouncy.png │ ├── crystal.png │ ├── dashing.png │ ├── eloquent.png │ ├── foxy.png │ ├── galactic.png │ ├── humble.png │ ├── iron.png │ ├── jazzy.png │ ├── kilted.png │ └── rolling.png ├── include │ └── turtlesim │ │ ├── qos.hpp │ │ ├── turtle.hpp │ │ └── turtle_frame.hpp ├── launch │ └── multisim.launch.py ├── package.xml ├── src │ ├── turtle.cpp │ ├── turtle_frame.cpp │ ├── turtlesim.cpp │ └── turtlesim │ │ └── __init__.py └── tutorials │ ├── draw_square.cpp │ ├── mimic.cpp │ └── teleop_turtle_key.cpp └── turtlesim_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── action └── RotateAbsolute.action ├── msg ├── Color.msg └── Pose.msg ├── package.xml └── srv ├── Kill.srv ├── SetPen.srv ├── Spawn.srv ├── TeleportAbsolute.srv └── TeleportRelative.srv /turtlesim/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtlesim 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.1 (2025-05-26) 6 | ------------------- 7 | * Support Qt6 (`#170 `_) 8 | * Add icon for Kilted Kaiju (`#180 `_) 9 | * Contributors: Alejandro Hernández Cordero, Scott K Logan 10 | 11 | 1.10.0 (2025-04-25) 12 | ------------------- 13 | 14 | 1.9.2 (2024-07-09) 15 | ------------------ 16 | * Create turtlesim_msgs (`#169 `_) 17 | * Contributors: Alejandro Hernández Cordero 18 | 19 | 1.9.1 (2024-06-17) 20 | ------------------ 21 | * Add icon for Jazzy. (`#167 `_) 22 | * [teleop_turtle_key] update usage string to match keys captured by keyboard (`#165 `_) 23 | * Contributors: Marco A. Gutierrez, Mikael Arguedas 24 | 25 | 1.9.0 (2024-04-26) 26 | ------------------ 27 | 28 | 1.8.2 (2024-03-28) 29 | ------------------ 30 | * Shorten the callback definition for uncrustify. (`#163 `_) 31 | * Contributors: Chris Lalancette 32 | 33 | 1.8.1 (2024-01-24) 34 | ------------------ 35 | * Use same QoS for all topic pub/subs (`#161 `_) 36 | * Contributors: Yadu 37 | 38 | 1.8.0 (2023-12-26) 39 | ------------------ 40 | * Remove all uses of ament_target_dependencies. (`#159 `_) 41 | * Contributors: Chris Lalancette 42 | 43 | 1.7.4 (2023-10-04) 44 | ------------------ 45 | * Crop galactic.png and rolling.png to 45x45. (`#158 `_) 46 | * Contributors: Jason O'Kane 47 | 48 | 1.7.3 (2023-09-07) 49 | ------------------ 50 | * Remove the unused member variable last_state\_ (`#156 `_) 51 | * Contributors: Chris Lalancette 52 | 53 | 1.7.2 (2023-06-05) 54 | ------------------ 55 | * Added common tests (`#154 `_) 56 | * Heavy cleanup of the draw_square tutorial. (`#152 `_) 57 | * Heavy cleanup of the draw_square tutorial. 58 | In particular: 59 | 1. Make it conform to the current ROS 2 style. 60 | 2. Add in copyright information. 61 | 3. Refactor the entire code into a class, which tidies it 62 | up quite a bit and removes a bunch of globals. 63 | 4. Make sure to wait for the reset to complete before trying 64 | to move the turtle. 65 | * Contributors: Alejandro Hernández Cordero, Chris Lalancette 66 | 67 | 1.7.1 (2023-05-11) 68 | ------------------ 69 | * Remove the range constraints from the holonomic parameter. (`#150 `_) 70 | * Add icon (`#148 `_) 71 | * Contributors: Chris Lalancette, Yadu 72 | 73 | 1.7.0 (2023-04-28) 74 | ------------------ 75 | 76 | 1.6.0 (2023-02-14) 77 | ------------------ 78 | * Update turtlesim to C++17. (`#146 `_) 79 | * [rolling] Update maintainers - 2022-11-07 (`#145 `_) 80 | * Contributors: Audrow Nash, Chris Lalancette 81 | 82 | 1.5.0 (2022-09-13) 83 | ------------------ 84 | * Add parameter to enable holonomic motion (`#131 `_) 85 | * Add humble turtle (`#140 `_) 86 | * Contributors: Audrow Nash, Daisuke Sato 87 | 88 | 1.4.1 (2022-01-14) 89 | ------------------ 90 | * Use ``double`` when handling ``qreal orient\_`` (`#114 `_) 91 | * Add Rolling Icon (`#133 `_) 92 | * Contributors: Katherine Scott, Seulbae Kim 93 | 94 | 1.4.0 (2021-11-18) 95 | ------------------ 96 | * Update maintainers to Audrow Nash and Michael Jeronimo (`#137 `_) 97 | * Fixing deprecated subscriber callback warnings (`#134 `_) 98 | * Use rosidl_get_typesupport_target() (`#132 `_) 99 | * Print out the correct node name on startup. (`#122 `_) 100 | * Contributors: Abrar Rahman Protyasha, Audrow Nash, Chris Lalancette, Shane Loretz 101 | 102 | 1.3.3 (2021-05-21) 103 | ------------------ 104 | * Added galactic turtle icon. (`#123 `_) 105 | * Heavily cleanup teleop_turtle_key. (`#121 `_) 106 | * Contributors: Chris Lalancette, Katherine Scott 107 | 108 | 1.3.2 (2021-04-16) 109 | ------------------ 110 | * Ignore key up events in teleop_turtle_key on Windows (`#118 `_) 111 | * Contributors: Michel Hidalgo 112 | 113 | 1.3.1 (2020-12-10) 114 | ------------------ 115 | * Update maintainers (`#106 `_) 116 | * Contributors: Shane Loretz 117 | 118 | 1.3.0 (2020-09-21) 119 | ------------------ 120 | * Update goal response callback signature (`#100 `_) 121 | * Contributors: Jacob Perron 122 | 123 | 1.2.5 (2020-08-05) 124 | ------------------ 125 | * add holonomic motion for turtlesim (`#98 `_) 126 | 127 | 1.2.4 (2020-06-12) 128 | ------------------ 129 | * add step value to turtlesim color parameters (`#91 `_) 130 | 131 | 1.2.3 (2020-06-05) 132 | ------------------ 133 | * update Foxy turtle (`#90 `_) 134 | 135 | 1.2.2 (2020-06-03) 136 | ------------------ 137 | * add Foxy turtle (`#89 `_) 138 | 139 | 1.2.1 (2020-06-02) 140 | ------------------ 141 | * Fix Qt deprecation warning (`#88 `_) 142 | 143 | 1.2.0 (2020-05-01) 144 | ------------------ 145 | * Replace deprecated launch_ros usage (`#84 `_) 146 | 147 | 1.1.1 (2020-04-16) 148 | ------------------ 149 | * catch reference to fix -Wcatch-value warning (`#78 `_) 150 | 151 | 1.1.0 (2019-11-12) 152 | ------------------ 153 | * Eloquent Elusor turtle icon (`#77 `_) 154 | 155 | 1.0.2 (2019-10-23) 156 | ------------------ 157 | * separate background color from drawn paths, trigger repaint on parameter changes (`#75 `_) 158 | * add descriptor information for background color parameters (`#73 `_) 159 | * Fix Windows compiler warning (`#69 `_) 160 | * Change log messages to use 'goal' instead of 'action' (`#67 `_) 161 | 162 | 1.0.1 (2019-10-02) 163 | ------------------ 164 | * fix mimic tutorial node (`#65 `_) 165 | * fix syntax error in teleop_turtle_key.cpp on Windows (`#66 `_) 166 | * add RotateAbsolute action (`#62 `_) 167 | * fix typo in error message (`#64 `_) 168 | 169 | 1.0.0 (2019-09-24) 170 | ------------------ 171 | * replace images with ROS 2 turtles (`#60 `_) 172 | * add shortcut to quit teleop (`#58 `_) 173 | * fix compiler warnings on Windows (`#57 `_) 174 | * add support for Windows (`#56 `_) 175 | * various fixes for ROS 2 (`#55 `_) 176 | * turtlesim for ROS 2 (`#53 `_) 177 | 178 | 0.9.1 (2019-03-04) 179 | ------------------ 180 | * change formula to avoid rounding with extreme input values (`#51 `_) 181 | * keep theta in the desired interval (`#46 `_) 182 | 183 | 0.9.0 (2018-04-11) 184 | ------------------ 185 | * add melodic turtle (`#41 `_) 186 | 187 | 0.8.1 (2017-07-27) 188 | ------------------ 189 | * theta ranges from -pi to +pi (`#31 `_) 190 | 191 | 0.8.0 (2017-03-10) 192 | ------------------ 193 | * add lunar turtle (`#39 `_) 194 | 195 | 0.7.1 (2016-10-24) 196 | ------------------ 197 | * check pen_on\_ when processing teleport requests (`#35 `_) 198 | 199 | 0.7.0 (2016-03-18) 200 | ------------------ 201 | * add kinetic image 202 | * update to Qt5 203 | * fix size of Jade image to not exceed other images in order to not get positioned incorrectly 204 | * fix compiler warnings 205 | 206 | 0.6.1 (2015-09-19) 207 | ------------------ 208 | * update the coordinate system in /spawn service for consistency (`#25 `_) 209 | 210 | 0.6.0 (2015-05-21) 211 | ------------------ 212 | * add jade turtle (`#22 `_) 213 | 214 | 0.5.3 (2015-05-04) 215 | ------------------ 216 | 217 | 0.5.2 (2014-12-23) 218 | ------------------ 219 | 220 | 0.5.1 (2014-05-08) 221 | ------------------ 222 | 223 | 0.5.0 (2014-05-07) 224 | ------------------ 225 | * add indigo turtle 226 | * add disabled code to easily spawn all available turtle types 227 | 228 | 0.4.3 (2014-01-07) 229 | ------------------ 230 | 231 | 0.4.2 (2013-10-04) 232 | ------------------ 233 | * fix missing install of hydro.svg (`#12 `_) 234 | 235 | 0.4.1 (2013-09-11) 236 | ------------------ 237 | * add hydro image to turtlesim 238 | 239 | 0.4.0 (2013-09-06) 240 | ------------------ 241 | * Adding png version of hydro for wiki linking 242 | * TurtleApp accepts argc by reference 243 | * Restoring all the changes appropriate for Hydro 244 | 245 | 0.3.13 (2013-08-21) 246 | ------------------- 247 | * TurtleApp accepts argc by reference 248 | * add hydro image to turtlesim 249 | * remove mainpage.dox 250 | 251 | 0.3.12 (2013-03-29) 252 | ------------------- 253 | * reverting velocity -> twist for groovy 254 | * Revert "chaning command_velocity to cmd_vel" for groovy 255 | This reverts commit 96e5174d3a5c961b6e1195b90b4024e2858df010. 256 | * Revert "adding geometry_msgs dependency in package.xml and CMakelist" for groovy 257 | This reverts commit c7ac1155d70269909b55af03d13fe2e089d6215d. 258 | * Revert "alaphabetic order" for groovy 259 | This reverts commit f928765ed08773517c195b74c55231c0e4fcc5e5. 260 | 261 | 0.3.11 (2013-03-21) 262 | ------------------- 263 | * update email in package.xml 264 | 265 | 0.3.10 (2013-03-08) 266 | ------------------- 267 | * Fix a moc generation error with boost >= 1.48 268 | See: 269 | https://bugreports.qt-project.org/browse/QTBUG-22829 270 | * Revert "Merge pull request `#6 `_ from ros/fix_qt_moc" 271 | This reverts commit 0e11b41ac53aad0e043b77d4d5950889245eaceb, reversing 272 | changes made to fc19df449d9ac297e8ab829ff22e99323c33ae93. 273 | * Revert "fix missing include (regression of `#5 `_)" 274 | This reverts commit 546dabe05c00e87296952cb2ca655e01895bd5ed. 275 | * fix missing include (regression of `#5 `_) 276 | * Fix a mod generation error with boost >= 1.48 277 | See: 278 | https://bugreports.qt-project.org/browse/QTBUG-22829 279 | * alaphabetic order 280 | * adding geometry_msgs dependency in package.xml and CMakelist 281 | * chaning command_velocity to cmd_vel 282 | * remove turtlesim velocity and use Twist msg 283 | 284 | 0.3.9 (2012-12-21) 285 | ------------------ 286 | * add groovy turtle 287 | * modified dep type of catkin 288 | 289 | 0.3.8 (2012-12-13) 290 | ------------------ 291 | * add missing downstream depend 292 | * switched from langs to message_* packages 293 | 294 | 0.3.7 (2012-12-06) 295 | ------------------ 296 | 297 | 0.3.6 (2012-10-30) 298 | ------------------ 299 | * fix catkin function order 300 | 301 | 0.3.5 (2012-10-18) 302 | ------------------ 303 | 304 | 0.3.4 (2012-10-06) 305 | ------------------ 306 | 307 | 0.3.3 (2012-10-05) 308 | ------------------ 309 | * fixed missing genmsg stuff 310 | * updated to latest catkin 311 | * added package.xml files 312 | 313 | 0.3.2 (2012-09-05) 314 | ------------------ 315 | * updated catkin variables 316 | * updated pkg-config in manifest.xml 317 | 318 | 0.3.1 (2012-09-03) 319 | ------------------ 320 | * use install destination variables, removed manual installation of manifests 321 | 322 | 0.3.0 (2012-08-29) 323 | ------------------ 324 | * updated to current catkin 325 | 326 | 0.2.20 (2013-02-08) 327 | ------------------- 328 | * fixed compilation on platforms with different qreal type 329 | 330 | 0.2.19 (2012-06-15 03:13:40 +0000) 331 | ---------------------------------- 332 | * make find_package REQUIRED 333 | * removed obsolete catkin tag from manifest files 334 | * added missing install of turtlesim images 335 | * using fuerte image in turtlesim 336 | * fuerte icon 337 | * remove old Makefiles and bump to 0.2.13 338 | * fix find boost component for turtlesim 339 | * change deps for turtlesim from wx to qt 340 | * migrate turtlesim from wx to qt 341 | * updated export for messages/catkin 342 | * add missing libs for oneiric 343 | * add missing dependency on wx, and take out conditional build logic from turtlesim 344 | * conditionally build based on wx, for now 345 | * turn on turtlesim 346 | * adding , removing depends and platform tags 347 | * remove old rosbuild2 stuff 348 | * adios rosbuild2 in manifests 349 | * changed number of turtles to a #define to prevent future mistakes with adding new turtles 350 | * electric turtle 351 | * rosbuild2/windows tweaks, they keep on comin' 352 | * rosbuild2 taking shape. 353 | * rosbuild2 taking shape 354 | * moving teleop keyboard into turtlesim to remove tutorial deps on keyboard 355 | * diamondback 356 | * Added Ubuntu platform tags 357 | * fix to actually paint on OSX 358 | * Only update the path image every 3 frames, because ConvertToImage on a 500x500 bitmap is somehow very expensive 359 | * Move bitmap->image conversion outside of loop (that was boneheaded) 360 | * Add color sensor to turtles 361 | * Switch turtlesim to x-forward (theta=0 now faces to the right) 362 | * Optionally name your turtles yourself 363 | * Fix coordinate system 364 | * adding a little more description to manifest 365 | * Add absolute and relative teleport service calls 366 | * changing turtlesim to turtlesim_node for tutorial clarity 367 | * * Multi-turtle support 368 | * turtle_pose and command_velocity now exist per-turtle. turtle_pose has been renamed "pose" 369 | * "spawn" service call to spawn a new turtle, which returns the turtle name 370 | * "kill" service call, to kill a turtle by name 371 | * Switch to "meters" as the distance unit, where 1 meter is defined as the height of the turtle 372 | * adding export to manifest 373 | * Change default background/pen colors 374 | * Randomly choose one of the 3 turtles 375 | * 3 turtle set by metamanda 376 | * throttling refresh rate so that xorg doesn't use all the cpu 377 | * adding debug statements 378 | * the drawing file used to create turtle.png 379 | * new turtle made by melonee 380 | * Apply Melonee's diff to set the background color parameters on the param server at startup 381 | * Add error output if the turtle hits the wall 382 | * Add turtlesim to the ros_tutorials stack 383 | -------------------------------------------------------------------------------- /turtlesim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20) 2 | project(turtlesim) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(ament_index_cpp REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | 17 | if(USE_QT6) 18 | find_package(Qt6 REQUIRED COMPONENTS Widgets) 19 | set(QT_VERSION_MAJOR 6) 20 | else() 21 | find_package(Qt5 REQUIRED COMPONENTS Widgets) 22 | set(QT_VERSION_MAJOR 5) 23 | endif() 24 | 25 | find_package(rclcpp REQUIRED) 26 | find_package(rclcpp_action REQUIRED) 27 | find_package(std_msgs REQUIRED) 28 | find_package(std_srvs REQUIRED) 29 | find_package(turtlesim_msgs REQUIRED) 30 | 31 | if(${QT_VERSION_MAJOR} GREATER "5") 32 | qt_standard_project_setup() 33 | qt_wrap_cpp(turtlesim_node_MOCS include/turtlesim/turtle_frame.hpp) 34 | else() 35 | qt5_wrap_cpp(turtlesim_node_MOCS include/turtlesim/turtle_frame.hpp) 36 | endif() 37 | 38 | add_library(MocHeaders INTERFACE) 39 | target_include_directories(MocHeaders INTERFACE 40 | "$" 41 | "$" 42 | ) 43 | 44 | add_executable(turtlesim_node 45 | src/turtlesim.cpp 46 | src/turtle.cpp 47 | src/turtle_frame.cpp 48 | ${turtlesim_node_MOCS} 49 | ) 50 | 51 | target_link_libraries(turtlesim_node PRIVATE 52 | ament_index_cpp::ament_index_cpp 53 | ${cpp_typesupport_target} 54 | ${geometry_msgs_TARGETS} 55 | ${turtlesim_msgs_TARGETS} 56 | Qt${QT_VERSION_MAJOR}::Widgets 57 | MocHeaders 58 | rclcpp::rclcpp 59 | rclcpp_action::rclcpp_action 60 | ${std_srvs_TARGETS} 61 | ) 62 | 63 | add_executable(turtle_teleop_key tutorials/teleop_turtle_key.cpp) 64 | target_link_libraries(turtle_teleop_key PRIVATE 65 | ${turtlesim_msgs_TARGETS} 66 | ${geometry_msgs_TARGETS} 67 | MocHeaders 68 | rclcpp::rclcpp 69 | rclcpp_action::rclcpp_action 70 | ) 71 | 72 | add_executable(draw_square tutorials/draw_square.cpp) 73 | target_link_libraries(draw_square PRIVATE 74 | ${turtlesim_msgs_TARGETS} 75 | ${geometry_msgs_TARGETS} 76 | MocHeaders 77 | rclcpp::rclcpp 78 | ${std_srvs_TARGETS} 79 | ) 80 | 81 | add_executable(mimic tutorials/mimic.cpp) 82 | target_link_libraries(mimic PRIVATE 83 | ${turtlesim_msgs_TARGETS} 84 | ${geometry_msgs_TARGETS} 85 | MocHeaders 86 | rclcpp::rclcpp 87 | ) 88 | 89 | if(BUILD_TESTING) 90 | find_package(ament_lint_auto REQUIRED) 91 | ament_lint_auto_find_test_dependencies() 92 | endif() 93 | 94 | install(TARGETS 95 | turtlesim_node 96 | turtle_teleop_key 97 | draw_square 98 | mimic 99 | DESTINATION lib/${PROJECT_NAME}) 100 | 101 | install(DIRECTORY images 102 | DESTINATION share/${PROJECT_NAME} 103 | FILES_MATCHING PATTERN "*.png" PATTERN "*.svg") 104 | 105 | install(DIRECTORY launch 106 | DESTINATION share/${PROJECT_NAME}) 107 | 108 | ament_package() 109 | -------------------------------------------------------------------------------- /turtlesim/images/ardent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/ardent.png -------------------------------------------------------------------------------- /turtlesim/images/bouncy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/bouncy.png -------------------------------------------------------------------------------- /turtlesim/images/crystal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/crystal.png -------------------------------------------------------------------------------- /turtlesim/images/dashing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/dashing.png -------------------------------------------------------------------------------- /turtlesim/images/eloquent.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/eloquent.png -------------------------------------------------------------------------------- /turtlesim/images/foxy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/foxy.png -------------------------------------------------------------------------------- /turtlesim/images/galactic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/galactic.png -------------------------------------------------------------------------------- /turtlesim/images/humble.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/humble.png -------------------------------------------------------------------------------- /turtlesim/images/iron.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/iron.png -------------------------------------------------------------------------------- /turtlesim/images/jazzy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/jazzy.png -------------------------------------------------------------------------------- /turtlesim/images/kilted.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/kilted.png -------------------------------------------------------------------------------- /turtlesim/images/rolling.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/images/rolling.png -------------------------------------------------------------------------------- /turtlesim/include/turtlesim/qos.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024, Open Source Robotics Foundation, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef TURTLESIM__QOS_HPP_ 30 | #define TURTLESIM__QOS_HPP_ 31 | 32 | #include 33 | 34 | namespace turtlesim 35 | { 36 | // Return the QoS used for all publishers/subscriptions. 37 | inline rclcpp::QoS topic_qos() 38 | { 39 | return rclcpp::QoS(rclcpp::KeepLast(7)).reliable(); 40 | } 41 | } // namespace turtlesim 42 | 43 | #endif // TURTLESIM__QOS_HPP_ 44 | -------------------------------------------------------------------------------- /turtlesim/include/turtlesim/turtle.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef TURTLESIM__TURTLE_HPP_ 30 | #define TURTLESIM__TURTLE_HPP_ 31 | 32 | // This prevents a MOC error with versions of boost >= 1.48 33 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 34 | # include 35 | # include 36 | 37 | # include 38 | # include 39 | # include 40 | # include 41 | # include 42 | # include 43 | # include 44 | #endif 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | #include 54 | 55 | #define PI 3.14159265 56 | #define TWO_PI 2.0 * PI 57 | 58 | namespace turtlesim 59 | { 60 | class Turtle 61 | { 62 | public: 63 | using RotateAbsoluteGoalHandle = rclcpp_action::ServerGoalHandle< 64 | turtlesim_msgs::action::RotateAbsolute>; 65 | 66 | Turtle( 67 | rclcpp::Node::SharedPtr & nh, const std::string & real_name, const QImage & turtle_image, 68 | const QPointF & pos, float orient); 69 | 70 | bool update( 71 | double dt, QPainter & path_painter, const QImage & path_image, qreal canvas_width, 72 | qreal canvas_height); 73 | void paint(QPainter & painter); 74 | 75 | private: 76 | void velocityCallback(const geometry_msgs::msg::Twist::ConstSharedPtr vel); 77 | bool setPenCallback( 78 | const turtlesim_msgs::srv::SetPen::Request::SharedPtr, 79 | turtlesim_msgs::srv::SetPen::Response::SharedPtr); 80 | bool teleportRelativeCallback( 81 | const turtlesim_msgs::srv::TeleportRelative::Request::SharedPtr, 82 | turtlesim_msgs::srv::TeleportRelative::Response::SharedPtr); 83 | bool teleportAbsoluteCallback( 84 | const turtlesim_msgs::srv::TeleportAbsolute::Request::SharedPtr, 85 | turtlesim_msgs::srv::TeleportAbsolute::Response::SharedPtr); 86 | void rotateAbsoluteAcceptCallback(const std::shared_ptr); 87 | 88 | void rotateImage(); 89 | 90 | rclcpp::Node::SharedPtr nh_; 91 | 92 | QImage turtle_image_; 93 | QImage turtle_rotated_image_; 94 | 95 | QPointF pos_; 96 | qreal orient_; 97 | 98 | qreal lin_vel_x_; 99 | qreal lin_vel_y_; 100 | qreal ang_vel_; 101 | bool pen_on_; 102 | QPen pen_; 103 | 104 | rclcpp::Subscription::SharedPtr velocity_sub_; 105 | rclcpp::Publisher::SharedPtr pose_pub_; 106 | rclcpp::Publisher::SharedPtr color_pub_; 107 | rclcpp::Service::SharedPtr set_pen_srv_; 108 | rclcpp::Service::SharedPtr teleport_relative_srv_; 109 | rclcpp::Service::SharedPtr teleport_absolute_srv_; 110 | rclcpp_action::Server 111 | ::SharedPtr rotate_absolute_action_server_; 112 | 113 | std::shared_ptr rotate_absolute_goal_handle_; 114 | std::shared_ptr rotate_absolute_feedback_; 115 | std::shared_ptr rotate_absolute_result_; 116 | qreal rotate_absolute_start_orient_; 117 | 118 | rclcpp::Time last_command_time_; 119 | 120 | float meter_; 121 | 122 | struct TeleportRequest 123 | { 124 | TeleportRequest(float x, float y, qreal _theta, qreal _linear, bool _relative) 125 | : pos(x, y), 126 | theta(_theta), 127 | linear(_linear), 128 | relative(_relative) 129 | { 130 | } 131 | 132 | QPointF pos; 133 | qreal theta; 134 | qreal linear; 135 | bool relative; 136 | }; 137 | typedef std::vector V_TeleportRequest; 138 | V_TeleportRequest teleport_requests_; 139 | }; 140 | typedef std::shared_ptr TurtlePtr; 141 | } // namespace turtlesim 142 | 143 | #endif // TURTLESIM__TURTLE_HPP_ 144 | -------------------------------------------------------------------------------- /turtlesim/include/turtlesim/turtle_frame.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef TURTLESIM__TURTLE_FRAME_HPP_ 30 | #define TURTLESIM__TURTLE_FRAME_HPP_ 31 | 32 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 33 | #include "turtle.hpp" // NO LINT 34 | #endif 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | // This prevents a MOC error with versions of boost >= 1.48 44 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | #include 54 | #endif 55 | 56 | namespace turtlesim 57 | { 58 | class TurtleFrame : public QFrame 59 | { 60 | Q_OBJECT 61 | 62 | public: 63 | TurtleFrame( 64 | rclcpp::Node::SharedPtr & node_handle, QWidget * parent = 0, 65 | Qt::WindowFlags f = Qt::WindowFlags()); 66 | ~TurtleFrame(); 67 | 68 | std::string spawnTurtle(const std::string & name, float x, float y, float angle); 69 | std::string spawnTurtle(const std::string & name, float x, float y, float angle, size_t index); 70 | 71 | protected: 72 | void paintEvent(QPaintEvent * event); 73 | 74 | private slots: 75 | void onUpdate(); 76 | 77 | private: 78 | void updateTurtles(); 79 | void clear(); 80 | bool hasTurtle(const std::string & name); 81 | 82 | bool clearCallback( 83 | const std_srvs::srv::Empty::Request::SharedPtr, 84 | std_srvs::srv::Empty::Response::SharedPtr); 85 | bool resetCallback( 86 | const std_srvs::srv::Empty::Request::SharedPtr, 87 | std_srvs::srv::Empty::Response::SharedPtr); 88 | bool spawnCallback( 89 | const turtlesim_msgs::srv::Spawn::Request::SharedPtr, 90 | turtlesim_msgs::srv::Spawn::Response::SharedPtr); 91 | bool killCallback( 92 | const turtlesim_msgs::srv::Kill::Request::SharedPtr, 93 | turtlesim_msgs::srv::Kill::Response::SharedPtr); 94 | 95 | void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::ConstSharedPtr); 96 | 97 | rclcpp::Node::SharedPtr nh_; 98 | 99 | QTimer * update_timer_; 100 | QImage path_image_; 101 | QPainter path_painter_; 102 | 103 | uint64_t frame_count_; 104 | 105 | rclcpp::Time last_turtle_update_; 106 | 107 | rclcpp::Service::SharedPtr clear_srv_; 108 | rclcpp::Service::SharedPtr reset_srv_; 109 | rclcpp::Service::SharedPtr spawn_srv_; 110 | rclcpp::Service::SharedPtr kill_srv_; 111 | rclcpp::Subscription::SharedPtr parameter_event_sub_; 112 | 113 | typedef std::map M_Turtle; 114 | M_Turtle turtles_; 115 | uint32_t id_counter_; 116 | 117 | QVector turtle_images_; 118 | 119 | float meter_; 120 | float width_in_meters_; 121 | float height_in_meters_; 122 | }; 123 | } // namespace turtlesim 124 | #endif // TURTLESIM__TURTLE_FRAME_HPP_ 125 | -------------------------------------------------------------------------------- /turtlesim/launch/multisim.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright 2023 Open Source Robotics Foundation, Inc. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | from launch import LaunchDescription 17 | import launch_ros.actions 18 | 19 | 20 | def generate_launch_description(): 21 | return LaunchDescription([ 22 | launch_ros.actions.Node( 23 | namespace='turtlesim1', package='turtlesim', 24 | executable='turtlesim_node', output='screen'), 25 | launch_ros.actions.Node( 26 | namespace='turtlesim2', package='turtlesim', 27 | executable='turtlesim_node', output='screen'), 28 | ]) 29 | -------------------------------------------------------------------------------- /turtlesim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turtlesim 5 | 1.10.1 6 | 7 | turtlesim is a tool made for teaching ROS and ROS packages. 8 | 9 | 10 | Alejandro Hernandez Cordero 11 | 12 | BSD 13 | 14 | http://www.ros.org/wiki/turtlesim 15 | https://github.com/ros/ros_tutorials/issues 16 | https://github.com/ros/ros_tutorials 17 | 18 | Audrow Nash 19 | Dirk Thomas 20 | Josh Faust 21 | Mabel Zhang 22 | Michael Jeronimo 23 | Shane Loretz 24 | 25 | qt5-qmake 26 | qtbase5-dev 27 | 28 | ament_cmake 29 | 30 | libqt5-core 31 | libqt5-gui 32 | 33 | ament_index_cpp 34 | geometry_msgs 35 | rclcpp 36 | rclcpp_action 37 | std_msgs 38 | std_srvs 39 | turtlesim_msgs 40 | 41 | ament_lint_auto 42 | ament_lint_common 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /turtlesim/src/turtle.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "turtlesim/turtle.hpp" 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "geometry_msgs/msg/twist.hpp" 39 | #include "rclcpp/rclcpp.hpp" 40 | 41 | #include "turtlesim_msgs/action/rotate_absolute.hpp" 42 | #include "turtlesim_msgs/msg/pose.hpp" 43 | #include "turtlesim_msgs/msg/color.hpp" 44 | #include "turtlesim_msgs/srv/set_pen.hpp" 45 | #include "turtlesim_msgs/srv/teleport_absolute.hpp" 46 | #include "turtlesim_msgs/srv/teleport_relative.hpp" 47 | #include "turtlesim/qos.hpp" 48 | 49 | #define DEFAULT_PEN_R 0xb3 50 | #define DEFAULT_PEN_G 0xb8 51 | #define DEFAULT_PEN_B 0xff 52 | 53 | namespace turtlesim 54 | { 55 | 56 | static double normalizeAngle(double angle) 57 | { 58 | return angle - (TWO_PI * std::floor((angle + PI) / (TWO_PI))); 59 | } 60 | 61 | Turtle::Turtle( 62 | rclcpp::Node::SharedPtr & nh, const std::string & real_name, 63 | const QImage & turtle_image, const QPointF & pos, float orient) 64 | : nh_(nh) 65 | , turtle_image_(turtle_image) 66 | , pos_(pos) 67 | , orient_(orient) 68 | , lin_vel_x_(0.0) 69 | , lin_vel_y_(0.0) 70 | , ang_vel_(0.0) 71 | , pen_on_(true) 72 | , pen_(QColor(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) 73 | { 74 | pen_.setWidth(3); 75 | 76 | const rclcpp::QoS qos = topic_qos(); 77 | velocity_sub_ = nh_->create_subscription( 78 | real_name + "/cmd_vel", qos, std::bind( 79 | &Turtle::velocityCallback, this, 80 | std::placeholders::_1)); 81 | pose_pub_ = nh_->create_publisher(real_name + "/pose", qos); 82 | color_pub_ = nh_->create_publisher(real_name + "/color_sensor", qos); 83 | set_pen_srv_ = 84 | nh_->create_service( 85 | real_name + "/set_pen", 86 | std::bind(&Turtle::setPenCallback, this, std::placeholders::_1, std::placeholders::_2)); 87 | teleport_relative_srv_ = nh_->create_service( 88 | real_name + "/teleport_relative", 89 | std::bind( 90 | &Turtle::teleportRelativeCallback, this, std::placeholders::_1, 91 | std::placeholders::_2)); 92 | teleport_absolute_srv_ = nh_->create_service( 93 | real_name + "/teleport_absolute", 94 | std::bind( 95 | &Turtle::teleportAbsoluteCallback, this, std::placeholders::_1, 96 | std::placeholders::_2)); 97 | rotate_absolute_action_server_ = 98 | rclcpp_action::create_server( 99 | nh, 100 | real_name + "/rotate_absolute", 101 | [](const rclcpp_action::GoalUUID &, 102 | std::shared_ptr) 103 | { 104 | // Accept all goals 105 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 106 | }, 107 | [](const std::shared_ptr) 108 | { 109 | // Accept all cancel requests 110 | return rclcpp_action::CancelResponse::ACCEPT; 111 | }, 112 | std::bind(&Turtle::rotateAbsoluteAcceptCallback, this, std::placeholders::_1)); 113 | 114 | last_command_time_ = nh_->now(); 115 | 116 | meter_ = turtle_image_.height(); 117 | rotateImage(); 118 | } 119 | 120 | 121 | void Turtle::velocityCallback(const geometry_msgs::msg::Twist::ConstSharedPtr vel) 122 | { 123 | last_command_time_ = nh_->now(); 124 | lin_vel_x_ = vel->linear.x; 125 | bool holonomic = false; 126 | nh_->get_parameter_or("holonomic", holonomic, false); 127 | if (holonomic) { 128 | lin_vel_y_ = vel->linear.y; 129 | } 130 | ang_vel_ = vel->angular.z; 131 | 132 | // Abort any active action 133 | if (rotate_absolute_goal_handle_) { 134 | RCLCPP_WARN(nh_->get_logger(), "Velocity command received during rotation goal. Aborting goal"); 135 | rotate_absolute_goal_handle_->abort(rotate_absolute_result_); 136 | rotate_absolute_goal_handle_ = nullptr; 137 | } 138 | } 139 | 140 | bool Turtle::setPenCallback( 141 | const turtlesim_msgs::srv::SetPen::Request::SharedPtr req, 142 | turtlesim_msgs::srv::SetPen::Response::SharedPtr) 143 | { 144 | pen_on_ = !req->off; 145 | if (req->off) { 146 | return true; 147 | } 148 | 149 | QPen pen(QColor(req->r, req->g, req->b)); 150 | if (req->width != 0) { 151 | pen.setWidth(req->width); 152 | } 153 | 154 | pen_ = pen; 155 | return true; 156 | } 157 | 158 | bool Turtle::teleportRelativeCallback( 159 | const turtlesim_msgs::srv::TeleportRelative::Request::SharedPtr req, 160 | turtlesim_msgs::srv::TeleportRelative::Response::SharedPtr) 161 | { 162 | teleport_requests_.push_back(TeleportRequest(0, 0, req->angular, req->linear, true)); 163 | return true; 164 | } 165 | 166 | bool Turtle::teleportAbsoluteCallback( 167 | const turtlesim_msgs::srv::TeleportAbsolute::Request::SharedPtr req, 168 | turtlesim_msgs::srv::TeleportAbsolute::Response::SharedPtr) 169 | { 170 | teleport_requests_.push_back(TeleportRequest(req->x, req->y, req->theta, 0, false)); 171 | return true; 172 | } 173 | 174 | void Turtle::rotateAbsoluteAcceptCallback( 175 | const std::shared_ptr goal_handle) 176 | { 177 | // Abort any existing goal 178 | if (rotate_absolute_goal_handle_) { 179 | RCLCPP_WARN( 180 | nh_->get_logger(), 181 | "Rotation goal received before a previous goal finished. Aborting previous goal"); 182 | rotate_absolute_goal_handle_->abort(rotate_absolute_result_); 183 | } 184 | rotate_absolute_goal_handle_ = goal_handle; 185 | rotate_absolute_feedback_.reset(new turtlesim_msgs::action::RotateAbsolute::Feedback); 186 | rotate_absolute_result_.reset(new turtlesim_msgs::action::RotateAbsolute::Result); 187 | rotate_absolute_start_orient_ = orient_; 188 | } 189 | 190 | void Turtle::rotateImage() 191 | { 192 | QTransform transform; 193 | transform.rotate(-orient_ * 180.0 / PI + 90.0); 194 | turtle_rotated_image_ = turtle_image_.transformed(transform); 195 | } 196 | 197 | bool Turtle::update( 198 | double dt, QPainter & path_painter, const QImage & path_image, 199 | qreal canvas_width, qreal canvas_height) 200 | { 201 | bool modified = false; 202 | qreal old_orient = orient_; 203 | 204 | // first process any teleportation requests, in order 205 | V_TeleportRequest::iterator it = teleport_requests_.begin(); 206 | V_TeleportRequest::iterator end = teleport_requests_.end(); 207 | for (; it != end; ++it) { 208 | const TeleportRequest & req = *it; 209 | 210 | QPointF old_pos = pos_; 211 | if (req.relative) { 212 | orient_ += req.theta; 213 | pos_.rx() += std::cos(orient_) * req.linear; 214 | pos_.ry() += -std::sin(orient_) * req.linear; 215 | } else { 216 | pos_.setX(req.pos.x()); 217 | pos_.setY(std::max(0.0, static_cast(canvas_height - req.pos.y()))); 218 | orient_ = req.theta; 219 | } 220 | 221 | if (pen_on_) { 222 | path_painter.setPen(pen_); 223 | path_painter.drawLine(pos_ * meter_, old_pos * meter_); 224 | } 225 | modified = true; 226 | } 227 | 228 | teleport_requests_.clear(); 229 | 230 | // Process any action requests 231 | if (rotate_absolute_goal_handle_) { 232 | // Check if there was a cancel request 233 | if (rotate_absolute_goal_handle_->is_canceling()) { 234 | RCLCPP_INFO(nh_->get_logger(), "Rotation goal canceled"); 235 | rotate_absolute_goal_handle_->canceled(rotate_absolute_result_); 236 | rotate_absolute_goal_handle_ = nullptr; 237 | lin_vel_x_ = 0.0; 238 | lin_vel_y_ = 0.0; 239 | ang_vel_ = 0.0; 240 | } else { 241 | double theta = normalizeAngle(rotate_absolute_goal_handle_->get_goal()->theta); 242 | double remaining = normalizeAngle(theta - static_cast(orient_)); 243 | 244 | // Update result 245 | rotate_absolute_result_->delta = 246 | normalizeAngle(static_cast(rotate_absolute_start_orient_ - orient_)); 247 | 248 | // Update feedback 249 | rotate_absolute_feedback_->remaining = remaining; 250 | rotate_absolute_goal_handle_->publish_feedback(rotate_absolute_feedback_); 251 | 252 | // Check stopping condition 253 | if (fabs(normalizeAngle(static_cast(orient_) - theta)) < 0.02) { 254 | RCLCPP_INFO(nh_->get_logger(), "Rotation goal completed successfully"); 255 | rotate_absolute_goal_handle_->succeed(rotate_absolute_result_); 256 | rotate_absolute_goal_handle_ = nullptr; 257 | lin_vel_x_ = 0.0; 258 | lin_vel_y_ = 0.0; 259 | ang_vel_ = 0.0; 260 | } else { 261 | lin_vel_x_ = 0.0; 262 | lin_vel_y_ = 0.0; 263 | ang_vel_ = remaining < 0.0 ? -1.0 : 1.0; 264 | last_command_time_ = nh_->now(); 265 | } 266 | } 267 | } 268 | 269 | if (nh_->now() - last_command_time_ > rclcpp::Duration(1.0, 0)) { 270 | lin_vel_x_ = 0.0; 271 | lin_vel_y_ = 0.0; 272 | ang_vel_ = 0.0; 273 | } 274 | 275 | QPointF old_pos = pos_; 276 | 277 | orient_ = orient_ + ang_vel_ * dt; 278 | // Keep orient_ between -pi and +pi 279 | orient_ = normalizeAngle(orient_); 280 | pos_.rx() += std::cos(orient_) * lin_vel_x_ * dt - 281 | std::sin(orient_) * lin_vel_y_ * dt; 282 | pos_.ry() -= std::cos(orient_) * lin_vel_y_ * dt + 283 | std::sin(orient_) * lin_vel_x_ * dt; 284 | 285 | // Clamp to screen size 286 | if (pos_.x() < 0 || pos_.x() > canvas_width || 287 | pos_.y() < 0 || pos_.y() > canvas_height) 288 | { 289 | RCLCPP_WARN( 290 | nh_->get_logger(), "Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", 291 | pos_.x(), pos_.y()); 292 | } 293 | 294 | pos_.setX( 295 | std::min( 296 | std::max( 297 | static_cast(pos_.x()), 0.0), static_cast(canvas_width))); 298 | pos_.setY( 299 | std::min( 300 | std::max(static_cast(pos_.y()), 0.0), 301 | static_cast(canvas_height))); 302 | 303 | // Publish pose of the turtle 304 | auto p = std::make_unique(); 305 | p->x = pos_.x(); 306 | p->y = canvas_height - pos_.y(); 307 | p->theta = orient_; 308 | p->linear_velocity = std::sqrt(lin_vel_x_ * lin_vel_x_ + lin_vel_y_ * lin_vel_y_); 309 | p->angular_velocity = ang_vel_; 310 | pose_pub_->publish(std::move(p)); 311 | 312 | // Figure out (and publish) the color underneath the turtle 313 | { 314 | auto color = std::make_unique(); 315 | QRgb pixel = path_image.pixel((pos_ * meter_).toPoint()); 316 | color->r = qRed(pixel); 317 | color->g = qGreen(pixel); 318 | color->b = qBlue(pixel); 319 | color_pub_->publish(std::move(color)); 320 | } 321 | 322 | RCLCPP_DEBUG( 323 | nh_->get_logger(), "[%s]: pos_x: %f pos_y: %f theta: %f", 324 | nh_->get_namespace(), pos_.x(), pos_.y(), orient_); 325 | 326 | if (orient_ != old_orient) { 327 | rotateImage(); 328 | modified = true; 329 | } 330 | if (pos_ != old_pos) { 331 | if (pen_on_) { 332 | path_painter.setPen(pen_); 333 | path_painter.drawLine(pos_ * meter_, old_pos * meter_); 334 | } 335 | modified = true; 336 | } 337 | 338 | return modified; 339 | } 340 | 341 | void Turtle::paint(QPainter & painter) 342 | { 343 | QPointF p = pos_ * meter_; 344 | p.rx() -= 0.5 * turtle_rotated_image_.width(); 345 | p.ry() -= 0.5 * turtle_rotated_image_.height(); 346 | painter.drawImage(p, turtle_rotated_image_); 347 | } 348 | 349 | } // namespace turtlesim 350 | -------------------------------------------------------------------------------- /turtlesim/src/turtle_frame.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "turtlesim/turtle_frame.hpp" 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "rcl_interfaces/msg/integer_range.hpp" 39 | #include "rcl_interfaces/msg/parameter_descriptor.hpp" 40 | #include "rcl_interfaces/msg/parameter_event.hpp" 41 | #include "rclcpp/rclcpp.hpp" 42 | #include "std_srvs/srv/empty.hpp" 43 | 44 | #include "turtlesim_msgs/srv/kill.hpp" 45 | #include "turtlesim_msgs/srv/spawn.hpp" 46 | 47 | #define DEFAULT_BG_R 0x45 48 | #define DEFAULT_BG_G 0x56 49 | #define DEFAULT_BG_B 0xff 50 | 51 | namespace turtlesim 52 | { 53 | 54 | TurtleFrame::TurtleFrame(rclcpp::Node::SharedPtr & node_handle, QWidget * parent, Qt::WindowFlags f) 55 | : QFrame(parent, f) 56 | , path_image_(500, 500, QImage::Format_ARGB32) 57 | , path_painter_(&path_image_) 58 | , frame_count_(0) 59 | , id_counter_(0) 60 | { 61 | setFixedSize(500, 500); 62 | setWindowTitle("TurtleSim"); 63 | 64 | srand(time(NULL)); 65 | 66 | update_timer_ = new QTimer(this); 67 | update_timer_->setInterval(16); 68 | update_timer_->start(); 69 | 70 | connect(update_timer_, SIGNAL(timeout()), this, SLOT(onUpdate())); 71 | 72 | nh_ = node_handle; 73 | rcl_interfaces::msg::IntegerRange range; 74 | range.from_value = 0; 75 | range.step = 1; 76 | range.to_value = 255; 77 | rcl_interfaces::msg::ParameterDescriptor background_r_descriptor; 78 | background_r_descriptor.description = "Red channel of the background color"; 79 | background_r_descriptor.integer_range.push_back(range); 80 | rcl_interfaces::msg::ParameterDescriptor background_g_descriptor; 81 | background_g_descriptor.description = "Green channel of the background color"; 82 | background_g_descriptor.integer_range.push_back(range); 83 | rcl_interfaces::msg::ParameterDescriptor background_b_descriptor; 84 | background_b_descriptor.description = "Blue channel of the background color"; 85 | background_b_descriptor.integer_range.push_back(range); 86 | nh_->declare_parameter( 87 | "background_r", rclcpp::ParameterValue( 88 | DEFAULT_BG_R), background_r_descriptor); 89 | nh_->declare_parameter( 90 | "background_g", rclcpp::ParameterValue( 91 | DEFAULT_BG_G), background_g_descriptor); 92 | nh_->declare_parameter( 93 | "background_b", rclcpp::ParameterValue( 94 | DEFAULT_BG_B), background_b_descriptor); 95 | 96 | rcl_interfaces::msg::ParameterDescriptor holonomic_descriptor; 97 | holonomic_descriptor.description = "If true, then turtles will be holonomic"; 98 | nh_->declare_parameter("holonomic", rclcpp::ParameterValue(false), holonomic_descriptor); 99 | 100 | QVector turtles; 101 | turtles.append("ardent.png"); 102 | turtles.append("bouncy.png"); 103 | turtles.append("crystal.png"); 104 | turtles.append("dashing.png"); 105 | turtles.append("eloquent.png"); 106 | turtles.append("foxy.png"); 107 | turtles.append("galactic.png"); 108 | turtles.append("humble.png"); 109 | turtles.append("iron.png"); 110 | turtles.append("jazzy.png"); 111 | turtles.append("kilted.png"); 112 | turtles.append("rolling.png"); 113 | 114 | QString images_path = 115 | (ament_index_cpp::get_package_share_directory("turtlesim") + "/images/").c_str(); 116 | for (int i = 0; i < turtles.size(); ++i) { 117 | QImage img; 118 | img.load(images_path + turtles[i]); 119 | turtle_images_.append(img); 120 | } 121 | 122 | meter_ = turtle_images_[0].height(); 123 | 124 | clear(); 125 | 126 | clear_srv_ = 127 | nh_->create_service( 128 | "clear", 129 | std::bind(&TurtleFrame::clearCallback, this, std::placeholders::_1, std::placeholders::_2)); 130 | reset_srv_ = 131 | nh_->create_service( 132 | "reset", 133 | std::bind(&TurtleFrame::resetCallback, this, std::placeholders::_1, std::placeholders::_2)); 134 | spawn_srv_ = 135 | nh_->create_service( 136 | "spawn", 137 | std::bind(&TurtleFrame::spawnCallback, this, std::placeholders::_1, std::placeholders::_2)); 138 | kill_srv_ = 139 | nh_->create_service( 140 | "kill", 141 | std::bind(&TurtleFrame::killCallback, this, std::placeholders::_1, std::placeholders::_2)); 142 | 143 | rclcpp::QoS qos(rclcpp::KeepLast(100), rmw_qos_profile_sensor_data); 144 | parameter_event_sub_ = nh_->create_subscription( 145 | "/parameter_events", qos, 146 | std::bind(&TurtleFrame::parameterEventCallback, this, std::placeholders::_1)); 147 | 148 | RCLCPP_INFO( 149 | nh_->get_logger(), "Starting turtlesim with node name %s", nh_->get_fully_qualified_name()); 150 | 151 | width_in_meters_ = (width() - 1) / meter_; 152 | height_in_meters_ = (height() - 1) / meter_; 153 | spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); 154 | 155 | // spawn all available turtle types 156 | if (false) { 157 | for (int index = 0; index < turtles.size(); ++index) { 158 | QString name = turtles[index]; 159 | name = name.split(".").first(); 160 | name.replace(QString("-"), QString("")); 161 | spawnTurtle( 162 | name.toStdString(), 1.0f + 1.5f * (index % 7), 1.0f + 1.5f * (index / 7), 163 | static_cast(PI) / 2.0f, index); 164 | } 165 | } 166 | } 167 | 168 | TurtleFrame::~TurtleFrame() 169 | { 170 | delete update_timer_; 171 | } 172 | 173 | bool TurtleFrame::spawnCallback( 174 | const turtlesim_msgs::srv::Spawn::Request::SharedPtr req, 175 | turtlesim_msgs::srv::Spawn::Response::SharedPtr res) 176 | { 177 | std::string name = spawnTurtle(req->name, req->x, req->y, req->theta); 178 | if (name.empty()) { 179 | RCLCPP_ERROR(nh_->get_logger(), "A turtle named [%s] already exists", req->name.c_str()); 180 | return false; 181 | } 182 | 183 | res->name = name; 184 | 185 | return true; 186 | } 187 | 188 | bool TurtleFrame::killCallback( 189 | const turtlesim_msgs::srv::Kill::Request::SharedPtr req, 190 | turtlesim_msgs::srv::Kill::Response::SharedPtr) 191 | { 192 | M_Turtle::iterator it = turtles_.find(req->name); 193 | if (it == turtles_.end()) { 194 | RCLCPP_ERROR( 195 | nh_->get_logger(), "Tried to kill turtle [%s], which does not exist", req->name.c_str()); 196 | return false; 197 | } 198 | 199 | turtles_.erase(it); 200 | update(); 201 | 202 | return true; 203 | } 204 | 205 | void TurtleFrame::parameterEventCallback( 206 | const rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) 207 | { 208 | // only consider events from this node 209 | if (event->node == nh_->get_fully_qualified_name()) { 210 | // since parameter events for this event aren't expected frequently just always call update() 211 | update(); 212 | } 213 | } 214 | 215 | bool TurtleFrame::hasTurtle(const std::string & name) 216 | { 217 | return turtles_.find(name) != turtles_.end(); 218 | } 219 | 220 | std::string TurtleFrame::spawnTurtle(const std::string & name, float x, float y, float angle) 221 | { 222 | return spawnTurtle(name, x, y, angle, rand() % turtle_images_.size()); 223 | } 224 | 225 | std::string TurtleFrame::spawnTurtle( 226 | const std::string & name, float x, float y, float angle, 227 | size_t index) 228 | { 229 | std::string real_name = name; 230 | if (real_name.empty()) { 231 | do{ 232 | std::stringstream ss; 233 | ss << "turtle" << ++id_counter_; 234 | real_name = ss.str(); 235 | } while (hasTurtle(real_name)); 236 | } else { 237 | if (hasTurtle(real_name)) { 238 | return ""; 239 | } 240 | } 241 | 242 | TurtlePtr t = std::make_shared( 243 | nh_, real_name, turtle_images_[static_cast(index)], QPointF( 244 | x, 245 | height_in_meters_ - y), angle); 246 | turtles_[real_name] = t; 247 | update(); 248 | 249 | RCLCPP_INFO( 250 | nh_->get_logger(), "Spawning turtle [%s] at x=[%f], y=[%f], theta=[%f]", 251 | real_name.c_str(), x, y, angle); 252 | 253 | return real_name; 254 | } 255 | 256 | void TurtleFrame::clear() 257 | { 258 | // make all pixels fully transparent 259 | path_image_.fill(qRgba(255, 255, 255, 0)); 260 | update(); 261 | } 262 | 263 | void TurtleFrame::onUpdate() 264 | { 265 | if (!rclcpp::ok()) { 266 | close(); 267 | return; 268 | } 269 | 270 | rclcpp::spin_some(nh_); 271 | 272 | updateTurtles(); 273 | } 274 | 275 | void TurtleFrame::paintEvent(QPaintEvent * event) 276 | { 277 | (void)event; // NO LINT 278 | QPainter painter(this); 279 | 280 | int r = DEFAULT_BG_R; 281 | int g = DEFAULT_BG_G; 282 | int b = DEFAULT_BG_B; 283 | nh_->get_parameter("background_r", r); 284 | nh_->get_parameter("background_g", g); 285 | nh_->get_parameter("background_b", b); 286 | QRgb background_color = qRgb(r, g, b); 287 | painter.fillRect(0, 0, width(), height(), background_color); 288 | 289 | painter.drawImage(QPoint(0, 0), path_image_); 290 | 291 | M_Turtle::iterator it = turtles_.begin(); 292 | M_Turtle::iterator end = turtles_.end(); 293 | for (; it != end; ++it) { 294 | it->second->paint(painter); 295 | } 296 | } 297 | 298 | void TurtleFrame::updateTurtles() 299 | { 300 | if (last_turtle_update_.nanoseconds() == 0) { 301 | last_turtle_update_ = nh_->now(); 302 | return; 303 | } 304 | 305 | bool modified = false; 306 | M_Turtle::iterator it = turtles_.begin(); 307 | M_Turtle::iterator end = turtles_.end(); 308 | for (; it != end; ++it) { 309 | modified |= it->second->update( 310 | 0.001 * update_timer_->interval(), path_painter_, path_image_, width_in_meters_, 311 | height_in_meters_); 312 | } 313 | if (modified) { 314 | update(); 315 | } 316 | 317 | ++frame_count_; 318 | } 319 | 320 | 321 | bool TurtleFrame::clearCallback( 322 | const std_srvs::srv::Empty::Request::SharedPtr, 323 | std_srvs::srv::Empty::Response::SharedPtr) 324 | { 325 | RCLCPP_INFO(nh_->get_logger(), "Clearing turtlesim."); 326 | clear(); 327 | return true; 328 | } 329 | 330 | bool TurtleFrame::resetCallback( 331 | const std_srvs::srv::Empty::Request::SharedPtr, 332 | std_srvs::srv::Empty::Response::SharedPtr) 333 | { 334 | RCLCPP_INFO(nh_->get_logger(), "Resetting turtlesim."); 335 | turtles_.clear(); 336 | id_counter_ = 0; 337 | spawnTurtle("", width_in_meters_ / 2.0, height_in_meters_ / 2.0, 0); 338 | clear(); 339 | return true; 340 | } 341 | 342 | } // namespace turtlesim 343 | -------------------------------------------------------------------------------- /turtlesim/src/turtlesim.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include "turtlesim/turtle_frame.hpp" 34 | 35 | class TurtleApp : public QApplication 36 | { 37 | public: 38 | rclcpp::Node::SharedPtr nh_; 39 | 40 | explicit TurtleApp(int & argc, char ** argv) 41 | : QApplication(argc, argv) 42 | { 43 | rclcpp::init(argc, argv); 44 | nh_ = rclcpp::Node::make_shared("turtlesim"); 45 | } 46 | 47 | ~TurtleApp() 48 | { 49 | rclcpp::shutdown(); 50 | } 51 | 52 | int exec() 53 | { 54 | turtlesim::TurtleFrame frame(nh_); 55 | frame.show(); 56 | 57 | return QApplication::exec(); 58 | } 59 | }; 60 | 61 | int main(int argc, char ** argv) 62 | { 63 | TurtleApp app(argc, argv); 64 | return app.exec(); 65 | } 66 | -------------------------------------------------------------------------------- /turtlesim/src/turtlesim/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/ros_tutorials/8c85a8fd7ff8d9a379735c1336e291b81c8133e2/turtlesim/src/turtlesim/__init__.py -------------------------------------------------------------------------------- /turtlesim/tutorials/draw_square.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "turtlesim/qos.hpp" 40 | 41 | #define PI 3.141592f 42 | 43 | class DrawSquare final : public rclcpp::Node 44 | { 45 | public: 46 | explicit DrawSquare(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 47 | : rclcpp::Node("draw_square", options) 48 | { 49 | const rclcpp::QoS qos = turtlesim::topic_qos(); 50 | twist_pub_ = this->create_publisher("turtle1/cmd_vel", qos); 51 | 52 | pose_sub_ = 53 | this->create_subscription( 54 | "turtle1/pose", qos, std::bind(&DrawSquare::poseCallback, this, std::placeholders::_1)); 55 | 56 | reset_client_ = this->create_client("reset"); 57 | 58 | timer_ = this->create_wall_timer(std::chrono::milliseconds(16), [this]() {timerCallback();}); 59 | 60 | auto empty = std::make_shared(); 61 | reset_result_ = reset_client_->async_send_request(empty).future; 62 | } 63 | 64 | private: 65 | enum State 66 | { 67 | FORWARD, 68 | STOP_FORWARD, 69 | TURN, 70 | STOP_TURN, 71 | }; 72 | 73 | void poseCallback(const turtlesim_msgs::msg::Pose & pose) 74 | { 75 | current_pose_ = pose; 76 | first_pose_set_ = true; 77 | } 78 | 79 | bool hasReachedGoal() 80 | { 81 | return fabsf(current_pose_.x - goal_pose_.x) < 0.1 && 82 | fabsf(current_pose_.y - goal_pose_.y) < 0.1 && 83 | fabsf(current_pose_.theta - goal_pose_.theta) < 0.01; 84 | } 85 | 86 | bool hasStopped() 87 | { 88 | return current_pose_.angular_velocity < 0.0001 && current_pose_.linear_velocity < 0.0001; 89 | } 90 | 91 | void printGoal() 92 | { 93 | RCLCPP_INFO( 94 | this->get_logger(), "New goal [%f %f, %f]", goal_pose_.x, goal_pose_.y, goal_pose_.theta); 95 | } 96 | 97 | void commandTurtle(float linear, float angular) 98 | { 99 | geometry_msgs::msg::Twist twist; 100 | twist.linear.x = linear; 101 | twist.angular.z = angular; 102 | twist_pub_->publish(twist); 103 | } 104 | 105 | void stopForward() 106 | { 107 | if (hasStopped()) { 108 | RCLCPP_INFO(this->get_logger(), "Reached goal"); 109 | state_ = TURN; 110 | goal_pose_.x = current_pose_.x; 111 | goal_pose_.y = current_pose_.y; 112 | goal_pose_.theta = fmod(current_pose_.theta + PI / 2.0f, 2.0f * PI); 113 | // wrap goal_pose_.theta to [-pi, pi) 114 | if (goal_pose_.theta >= PI) { 115 | goal_pose_.theta -= 2.0f * PI; 116 | } 117 | printGoal(); 118 | } else { 119 | commandTurtle(0, 0); 120 | } 121 | } 122 | 123 | void stopTurn() 124 | { 125 | if (hasStopped()) { 126 | RCLCPP_INFO(this->get_logger(), "Reached goal"); 127 | state_ = FORWARD; 128 | goal_pose_.x = cos(current_pose_.theta) * 2 + current_pose_.x; 129 | goal_pose_.y = sin(current_pose_.theta) * 2 + current_pose_.y; 130 | goal_pose_.theta = current_pose_.theta; 131 | printGoal(); 132 | } else { 133 | commandTurtle(0, 0); 134 | } 135 | } 136 | 137 | void forward() 138 | { 139 | if (hasReachedGoal()) { 140 | state_ = STOP_FORWARD; 141 | commandTurtle(0, 0); 142 | } else { 143 | commandTurtle(1.0f, 0); 144 | } 145 | } 146 | 147 | void turn() 148 | { 149 | if (hasReachedGoal()) { 150 | state_ = STOP_TURN; 151 | commandTurtle(0, 0); 152 | } else { 153 | commandTurtle(0, 0.4f); 154 | } 155 | } 156 | 157 | void timerCallback() 158 | { 159 | if (!reset_result_.valid()) { 160 | return; 161 | } 162 | 163 | if (!first_pose_set_) { 164 | return; 165 | } 166 | 167 | if (!first_goal_set_) { 168 | first_goal_set_ = true; 169 | state_ = FORWARD; 170 | goal_pose_.x = cos(current_pose_.theta) * 2 + current_pose_.x; 171 | goal_pose_.y = sin(current_pose_.theta) * 2 + current_pose_.y; 172 | goal_pose_.theta = current_pose_.theta; 173 | printGoal(); 174 | } 175 | 176 | if (state_ == FORWARD) { 177 | forward(); 178 | } else if (state_ == STOP_FORWARD) { 179 | stopForward(); 180 | } else if (state_ == TURN) { 181 | turn(); 182 | } else if (state_ == STOP_TURN) { 183 | stopTurn(); 184 | } 185 | } 186 | 187 | turtlesim_msgs::msg::Pose current_pose_; 188 | turtlesim_msgs::msg::Pose goal_pose_; 189 | bool first_goal_set_ = false; 190 | bool first_pose_set_ = false; 191 | State state_ = FORWARD; 192 | 193 | rclcpp::Publisher::SharedPtr twist_pub_; 194 | rclcpp::Subscription::SharedPtr pose_sub_; 195 | rclcpp::Client::SharedPtr reset_client_; 196 | rclcpp::Client::SharedFuture reset_result_; 197 | rclcpp::TimerBase::SharedPtr timer_; 198 | }; 199 | 200 | int main(int argc, char ** argv) 201 | { 202 | rclcpp::init(argc, argv); 203 | 204 | auto nh = std::make_shared(); 205 | 206 | rclcpp::spin(nh); 207 | 208 | rclcpp::shutdown(); 209 | 210 | return 0; 211 | } 212 | -------------------------------------------------------------------------------- /turtlesim/tutorials/mimic.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "turtlesim/qos.hpp" 34 | 35 | class MimicNode : public rclcpp::Node 36 | { 37 | public: 38 | MimicNode() 39 | : rclcpp::Node("turtle_mimic") 40 | { 41 | const rclcpp::QoS qos = turtlesim::topic_qos(); 42 | twist_pub_ = this->create_publisher("output/cmd_vel", qos); 43 | pose_sub_ = this->create_subscription( 44 | "input/pose", qos, std::bind(&MimicNode::poseCallback, this, std::placeholders::_1)); 45 | } 46 | 47 | private: 48 | void poseCallback(const turtlesim_msgs::msg::Pose::ConstSharedPtr pose) 49 | { 50 | geometry_msgs::msg::Twist twist; 51 | twist.angular.z = pose->angular_velocity; 52 | twist.linear.x = pose->linear_velocity; 53 | twist_pub_->publish(twist); 54 | } 55 | 56 | rclcpp::Publisher::SharedPtr twist_pub_; 57 | rclcpp::Subscription::SharedPtr pose_sub_; 58 | }; 59 | 60 | int main(int argc, char ** argv) 61 | { 62 | rclcpp::init(argc, argv); 63 | rclcpp::spin(std::make_shared()); 64 | } 65 | -------------------------------------------------------------------------------- /turtlesim/tutorials/teleop_turtle_key.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2009, Willow Garage, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Willow Garage nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #ifdef _WIN32 41 | # include // NO LINT 42 | #else 43 | # include // NO LINT 44 | # include // NO LINT 45 | #endif 46 | 47 | #include "turtlesim/qos.hpp" 48 | 49 | static constexpr char KEYCODE_RIGHT = 0x43; 50 | static constexpr char KEYCODE_LEFT = 0x44; 51 | static constexpr char KEYCODE_UP = 0x41; 52 | static constexpr char KEYCODE_DOWN = 0x42; 53 | static constexpr char KEYCODE_B = 0x62; 54 | static constexpr char KEYCODE_C = 0x63; 55 | static constexpr char KEYCODE_D = 0x64; 56 | static constexpr char KEYCODE_E = 0x65; 57 | static constexpr char KEYCODE_F = 0x66; 58 | static constexpr char KEYCODE_G = 0x67; 59 | static constexpr char KEYCODE_Q = 0x71; 60 | static constexpr char KEYCODE_R = 0x72; 61 | static constexpr char KEYCODE_T = 0x74; 62 | static constexpr char KEYCODE_V = 0x76; 63 | 64 | bool running = true; 65 | 66 | class KeyboardReader final 67 | { 68 | public: 69 | KeyboardReader() 70 | { 71 | #ifdef _WIN32 72 | hstdin_ = GetStdHandle(STD_INPUT_HANDLE); 73 | if (hstdin_ == INVALID_HANDLE_VALUE) { 74 | throw std::runtime_error("Failed to get stdin handle"); 75 | } 76 | if (!GetConsoleMode(hstdin_, &old_mode_)) { 77 | throw std::runtime_error("Failed to get old console mode"); 78 | } 79 | DWORD new_mode = ENABLE_PROCESSED_INPUT; // for Ctrl-C processing 80 | if (!SetConsoleMode(hstdin_, new_mode)) { 81 | throw std::runtime_error("Failed to set new console mode"); 82 | } 83 | #else 84 | // get the console in raw mode 85 | if (tcgetattr(0, &cooked_) < 0) { 86 | throw std::runtime_error("Failed to get old console mode"); 87 | } 88 | struct termios raw; 89 | memcpy(&raw, &cooked_, sizeof(struct termios)); 90 | raw.c_lflag &= ~(ICANON | ECHO); 91 | // Setting a new line, then end of file 92 | raw.c_cc[VEOL] = 1; 93 | raw.c_cc[VEOF] = 2; 94 | raw.c_cc[VTIME] = 1; 95 | raw.c_cc[VMIN] = 0; 96 | if (tcsetattr(0, TCSANOW, &raw) < 0) { 97 | throw std::runtime_error("Failed to set new console mode"); 98 | } 99 | #endif 100 | } 101 | 102 | char readOne() 103 | { 104 | char c = 0; 105 | 106 | #ifdef _WIN32 107 | INPUT_RECORD record; 108 | DWORD num_read; 109 | switch (WaitForSingleObject(hstdin_, 100)) { 110 | case WAIT_OBJECT_0: 111 | if (!ReadConsoleInput(hstdin_, &record, 1, &num_read)) { 112 | throw std::runtime_error("Read failed"); 113 | } 114 | 115 | if (record.EventType != KEY_EVENT || !record.Event.KeyEvent.bKeyDown) { 116 | break; 117 | } 118 | 119 | if (record.Event.KeyEvent.wVirtualKeyCode == VK_LEFT) { 120 | c = KEYCODE_LEFT; 121 | } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_UP) { 122 | c = KEYCODE_UP; 123 | } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_RIGHT) { 124 | c = KEYCODE_RIGHT; 125 | } else if (record.Event.KeyEvent.wVirtualKeyCode == VK_DOWN) { 126 | c = KEYCODE_DOWN; 127 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x42) { 128 | c = KEYCODE_B; 129 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x43) { 130 | c = KEYCODE_C; 131 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x44) { 132 | c = KEYCODE_D; 133 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x45) { 134 | c = KEYCODE_E; 135 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x46) { 136 | c = KEYCODE_F; 137 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x47) { 138 | c = KEYCODE_G; 139 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x51) { 140 | c = KEYCODE_Q; 141 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x52) { 142 | c = KEYCODE_R; 143 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x54) { 144 | c = KEYCODE_T; 145 | } else if (record.Event.KeyEvent.wVirtualKeyCode == 0x56) { 146 | c = KEYCODE_V; 147 | } 148 | break; 149 | 150 | case WAIT_TIMEOUT: 151 | break; 152 | } 153 | 154 | #else 155 | int rc = read(0, &c, 1); 156 | if (rc < 0) { 157 | throw std::runtime_error("read failed"); 158 | } 159 | #endif 160 | 161 | return c; 162 | } 163 | 164 | ~KeyboardReader() 165 | { 166 | #ifdef _WIN32 167 | SetConsoleMode(hstdin_, old_mode_); 168 | #else 169 | tcsetattr(0, TCSANOW, &cooked_); 170 | #endif 171 | } 172 | 173 | private: 174 | #ifdef _WIN32 175 | HANDLE hstdin_; 176 | DWORD old_mode_; 177 | #else 178 | struct termios cooked_; 179 | #endif 180 | }; 181 | 182 | class TeleopTurtle final 183 | { 184 | public: 185 | TeleopTurtle() 186 | { 187 | nh_ = rclcpp::Node::make_shared("teleop_turtle"); 188 | nh_->declare_parameter("scale_angular", 2.0); 189 | nh_->declare_parameter("scale_linear", 2.0); 190 | 191 | twist_pub_ = nh_->create_publisher( 192 | "turtle1/cmd_vel", 193 | turtlesim::topic_qos()); 194 | rotate_absolute_client_ = rclcpp_action::create_client( 195 | nh_, 196 | "turtle1/rotate_absolute"); 197 | } 198 | 199 | int keyLoop() 200 | { 201 | char c; 202 | 203 | std::thread{std::bind(&TeleopTurtle::spin, this)}.detach(); 204 | 205 | puts("Reading from keyboard"); 206 | puts("---------------------------"); 207 | puts("Use arrow keys to move the turtle."); 208 | puts("Use g|b|v|c|d|e|r|t keys to rotate to absolute orientations. 'f' to cancel a rotation."); 209 | puts("'q' to quit."); 210 | 211 | while (running) { 212 | // get the next event from the keyboard 213 | try { 214 | c = input_.readOne(); 215 | } catch (const std::runtime_error &) { 216 | perror("read():"); 217 | return -1; 218 | } 219 | 220 | double linear = 0.0; 221 | double angular = 0.0; 222 | 223 | RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); 224 | 225 | switch (c) { 226 | case KEYCODE_LEFT: 227 | RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); 228 | angular = 1.0; 229 | break; 230 | case KEYCODE_RIGHT: 231 | RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); 232 | angular = -1.0; 233 | break; 234 | case KEYCODE_UP: 235 | RCLCPP_DEBUG(nh_->get_logger(), "UP"); 236 | linear = 1.0; 237 | break; 238 | case KEYCODE_DOWN: 239 | RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); 240 | linear = -1.0; 241 | break; 242 | case KEYCODE_G: 243 | RCLCPP_DEBUG(nh_->get_logger(), "G"); 244 | sendGoal(0.0f); 245 | break; 246 | case KEYCODE_T: 247 | RCLCPP_DEBUG(nh_->get_logger(), "T"); 248 | sendGoal(0.7854f); 249 | break; 250 | case KEYCODE_R: 251 | RCLCPP_DEBUG(nh_->get_logger(), "R"); 252 | sendGoal(1.5708f); 253 | break; 254 | case KEYCODE_E: 255 | RCLCPP_DEBUG(nh_->get_logger(), "E"); 256 | sendGoal(2.3562f); 257 | break; 258 | case KEYCODE_D: 259 | RCLCPP_DEBUG(nh_->get_logger(), "D"); 260 | sendGoal(3.1416f); 261 | break; 262 | case KEYCODE_C: 263 | RCLCPP_DEBUG(nh_->get_logger(), "C"); 264 | sendGoal(-2.3562f); 265 | break; 266 | case KEYCODE_V: 267 | RCLCPP_DEBUG(nh_->get_logger(), "V"); 268 | sendGoal(-1.5708f); 269 | break; 270 | case KEYCODE_B: 271 | RCLCPP_DEBUG(nh_->get_logger(), "B"); 272 | sendGoal(-0.7854f); 273 | break; 274 | case KEYCODE_F: 275 | RCLCPP_DEBUG(nh_->get_logger(), "F"); 276 | cancelGoal(); 277 | break; 278 | case KEYCODE_Q: 279 | RCLCPP_DEBUG(nh_->get_logger(), "quit"); 280 | running = false; 281 | break; 282 | default: 283 | // This can happen if the read returned when there was no data, or 284 | // another key was pressed. In these cases, just silently ignore the 285 | // press. 286 | break; 287 | } 288 | 289 | if (running && (linear != 0.0 || angular != 0.0)) { 290 | geometry_msgs::msg::Twist twist; 291 | twist.angular.z = nh_->get_parameter("scale_angular").as_double() * angular; 292 | twist.linear.x = nh_->get_parameter("scale_linear").as_double() * linear; 293 | twist_pub_->publish(twist); 294 | } 295 | } 296 | 297 | return 0; 298 | } 299 | 300 | private: 301 | void spin() 302 | { 303 | rclcpp::spin(nh_); 304 | } 305 | 306 | void sendGoal(float theta) 307 | { 308 | using Rotate = turtlesim_msgs::action::RotateAbsolute; 309 | auto goal = Rotate::Goal(); 310 | goal.theta = theta; 311 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 312 | send_goal_options.goal_response_callback = 313 | [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) 314 | { 315 | RCLCPP_DEBUG(nh_->get_logger(), "Goal response received"); 316 | this->goal_handle_ = goal_handle; 317 | }; 318 | rotate_absolute_client_->async_send_goal(goal, send_goal_options); 319 | } 320 | 321 | void cancelGoal() 322 | { 323 | if (goal_handle_) { 324 | RCLCPP_DEBUG(nh_->get_logger(), "Sending cancel request"); 325 | try { 326 | rotate_absolute_client_->async_cancel_goal(goal_handle_); 327 | } catch (...) { 328 | // This can happen if the goal has already terminated and expired 329 | } 330 | } 331 | } 332 | 333 | rclcpp::Node::SharedPtr nh_; 334 | rclcpp::Publisher::SharedPtr twist_pub_; 335 | rclcpp_action::Client::SharedPtr rotate_absolute_client_; 336 | rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_; 337 | 338 | KeyboardReader input_; 339 | }; 340 | 341 | #ifdef _WIN32 342 | BOOL WINAPI quit(DWORD ctrl_type) 343 | { 344 | (void)ctrl_type; 345 | running = false; 346 | return true; 347 | } 348 | #else 349 | void quit(int sig) 350 | { 351 | (void)sig; 352 | running = false; 353 | } 354 | #endif 355 | 356 | int main(int argc, char ** argv) 357 | { 358 | rclcpp::init(argc, argv); 359 | 360 | #ifdef _WIN32 361 | SetConsoleCtrlHandler(quit, TRUE); 362 | #else 363 | signal(SIGINT, quit); 364 | #endif 365 | 366 | TeleopTurtle teleop_turtle; 367 | 368 | int rc = teleop_turtle.keyLoop(); 369 | 370 | rclcpp::shutdown(); 371 | 372 | return rc; 373 | } 374 | -------------------------------------------------------------------------------- /turtlesim_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtlesim_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.1 (2025-05-26) 6 | ------------------- 7 | 8 | 1.10.0 (2025-04-25) 9 | ------------------- 10 | 11 | 1.9.2 (2024-07-09) 12 | ------------------ 13 | * Create turtlesim_msgs (`#169 `_) 14 | * Contributors: Alejandro Hernández Cordero 15 | -------------------------------------------------------------------------------- /turtlesim_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(turtlesim_msgs) 4 | 5 | # Default to C++17 6 | if(NOT CMAKE_CXX_STANDARD) 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | find_package(ament_cmake REQUIRED) 15 | find_package(builtin_interfaces REQUIRED) 16 | find_package(rosidl_default_generators REQUIRED) 17 | 18 | set(msg_files 19 | action/RotateAbsolute.action 20 | ) 21 | 22 | set(action_files 23 | msg/Color.msg 24 | msg/Pose.msg 25 | ) 26 | 27 | set(srv_files 28 | srv/Kill.srv 29 | srv/SetPen.srv 30 | srv/Spawn.srv 31 | srv/TeleportAbsolute.srv 32 | srv/TeleportRelative.srv 33 | ) 34 | 35 | rosidl_generate_interfaces(${PROJECT_NAME} 36 | ${action_files} 37 | ${msg_files} 38 | ${srv_files} 39 | DEPENDENCIES builtin_interfaces 40 | ADD_LINTER_TESTS 41 | ) 42 | 43 | ament_export_dependencies(rosidl_default_runtime) 44 | 45 | ament_package() 46 | -------------------------------------------------------------------------------- /turtlesim_msgs/action/RotateAbsolute.action: -------------------------------------------------------------------------------- 1 | # The desired heading in radians 2 | float32 theta 3 | --- 4 | # The angular displacement in radians to the starting position 5 | float32 delta 6 | --- 7 | # The remaining rotation in radians 8 | float32 remaining 9 | -------------------------------------------------------------------------------- /turtlesim_msgs/msg/Color.msg: -------------------------------------------------------------------------------- 1 | uint8 r 2 | uint8 g 3 | uint8 b 4 | -------------------------------------------------------------------------------- /turtlesim_msgs/msg/Pose.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | 5 | float32 linear_velocity 6 | float32 angular_velocity -------------------------------------------------------------------------------- /turtlesim_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turtlesim_msgs 5 | 1.10.1 6 | 7 | turtlesim messages. 8 | 9 | 10 | Alejandro Hernandez Cordero 11 | 12 | BSD 13 | 14 | http://www.ros.org/wiki/turtlesim 15 | https://github.com/ros/ros_tutorials/issues 16 | https://github.com/ros/ros_tutorials 17 | 18 | Alejandro Hernandez 19 | 20 | ament_cmake 21 | 22 | rosidl_default_generators 23 | 24 | builtin_interfaces 25 | rosidl_default_runtime 26 | 27 | ament_lint_common 28 | 29 | rosidl_interface_packages 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /turtlesim_msgs/srv/Kill.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- -------------------------------------------------------------------------------- /turtlesim_msgs/srv/SetPen.srv: -------------------------------------------------------------------------------- 1 | uint8 r 2 | uint8 g 3 | uint8 b 4 | uint8 width 5 | uint8 off 6 | --- 7 | -------------------------------------------------------------------------------- /turtlesim_msgs/srv/Spawn.srv: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | string name # Optional. A unique name will be created and returned if this is empty 5 | --- 6 | string name -------------------------------------------------------------------------------- /turtlesim_msgs/srv/TeleportAbsolute.srv: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | --- 5 | -------------------------------------------------------------------------------- /turtlesim_msgs/srv/TeleportRelative.srv: -------------------------------------------------------------------------------- 1 | float32 linear 2 | float32 angular 3 | --- 4 | --------------------------------------------------------------------------------