├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── rviz │ ├── cave.rviz │ ├── cave_seven_robots.rviz │ ├── cave_three_robots.rviz │ ├── lines.rviz │ ├── robot0.rviz │ ├── robot_ns.rviz │ └── robot_ns_depth.rviz └── teleop_twist_joy │ └── f710.config.yaml ├── include └── stage_ros2 │ ├── stage_node.hpp │ ├── static_transform_broadcaster.h │ ├── transform_broadcaster.h │ └── visibility.h ├── launch ├── demo.launch.py ├── f710.launch.py ├── rviz.launch.py ├── rviz_ns.launch.py └── stage.launch.py ├── package.xml ├── res ├── cave.png ├── cave_seven_robots.png ├── cave_three_robots.png ├── cave_three_robots_multiple_tf_trees.jpg ├── cave_three_robots_multiple_tf_trees_depth.jpg ├── cave_three_robots_multiple_tf_trees_depth_info.jpg ├── cave_three_robots_one_tf_tree.jpg ├── cave_three_robots_with_rviz.jpg ├── demos.md ├── install.md ├── lines.png └── multi_robot_setup.md ├── src ├── camera.cpp ├── main.cpp ├── ranger.cpp ├── stage_node.cpp └── vehicle.cpp ├── tmuxinator ├── cave_one_robot.yml └── cave_three_robots.yml └── world ├── bitmaps ├── cave.png ├── hallway.pgm ├── hallway.png ├── line.png ├── straden.png ├── warehouse008.pgm ├── warehouse032.pgm ├── warehouse200.pgm └── willow-full.pgm ├── cave.world ├── cave_multi.world ├── cave_seven_robots.world ├── cave_three_robots.world ├── hallway.world ├── include ├── pioneer2dx.inc └── robots.inc ├── lines.world ├── straden.world └── willow-four-erratics-multisensor.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(stage_ros2) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | 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(rclcpp REQUIRED) 16 | find_package(rclcpp_components REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | find_package(nav_msgs REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(stage REQUIRED) 22 | find_package(tf2_ros REQUIRED) 23 | find_package(tf2_geometry_msgs REQUIRED) 24 | find_package(std_srvs REQUIRED) 25 | 26 | include_directories( 27 | ./include 28 | ${STAGE_INCLUDE_DIRS} 29 | ) 30 | 31 | add_library(stage_node SHARED 32 | src/stage_node.cpp src/vehicle.cpp src/ranger.cpp src/camera.cpp) 33 | target_compile_definitions(stage_node PRIVATE "STAGE_ROS2_PACKAGE_DLL") 34 | ament_target_dependencies(stage_node rclcpp rclcpp_components 35 | sensor_msgs geometry_msgs nav_msgs std_msgs tf2_ros std_srvs tf2_geometry_msgs) 36 | 37 | 38 | # This package installs libraries without exporting them. 39 | # Export the library path to ensure that the installed libraries are available. 40 | if(NOT WIN32) 41 | ament_environment_hooks( 42 | "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") 43 | endif() 44 | 45 | add_executable(stage_ros2 src/main.cpp) 46 | target_include_directories(stage_ros2 PUBLIC 47 | $ 48 | $) 49 | target_compile_features(stage_ros2 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 50 | target_link_libraries(stage_ros2 stage_node ${STAGE_LIBRARIES}) 51 | ament_target_dependencies(stage_ros2 rclcpp) 52 | 53 | install( 54 | DIRECTORY 55 | launch 56 | world 57 | config 58 | DESTINATION 59 | share/${PROJECT_NAME}/) 60 | 61 | install(TARGETS 62 | stage_node 63 | ARCHIVE DESTINATION lib 64 | LIBRARY DESTINATION lib 65 | RUNTIME DESTINATION bin) 66 | 67 | install(TARGETS 68 | stage_ros2 69 | DESTINATION lib/${PROJECT_NAME}) 70 | 71 | if(BUILD_TESTING) 72 | find_package(ament_lint_auto REQUIRED) 73 | # the following line skips the linter which checks for copyrights 74 | # comment the line when a copyright and license is added to all source files 75 | set(ament_cmake_copyright_FOUND TRUE) 76 | # the following line skips cpplint (only works in a git repo) 77 | # comment the line when this package is in a git repo and when 78 | # a copyright and license is added to all source files 79 | set(ament_cmake_cpplint_FOUND TRUE) 80 | ament_lint_auto_find_test_dependencies() 81 | endif() 82 | 83 | ament_package() 84 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # stage_ros2 2 | This is a ROS bridge for the robot simulator [Stage](https://github.com/rtv/Stage). 3 | It supports multiple robots with one or multiple tf-trees. 4 | 5 | * [install](res/install.md) 6 | * [run demos](res/demos.md) 7 | * [handling multiple vehickes](res/multi_robot_setup.md) 8 | 9 | ## Stamped velocity Support (TwistStamped) 10 | This stage ROS bridge can be configured to use stamped velocities on the topic __cmd_vel__. 11 | ``` 12 | ros2 launch stage_ros2 demo.launch.py world:=cave use_stamped_velocity:=false 13 | ``` 14 | 15 | ## Single Robot Support 16 | This stage ROS bridge can be configured to use a namespace even for one robot. 17 | ``` 18 | ## with /tf 19 | ros2 launch stage_ros2 stage.launch.py world:=cave enforce_prefixes:=false one_tf_tree:=true 20 | ## with /robot_0/tf 21 | ros2 launch stage_ros2 stage.launch.py world:=cave enforce_prefixes:=true one_tf_tree:=false 22 | ``` 23 | 24 | ## Multi Robot Support 25 | This stage ROS bridge supports multiple robots with one or multiple tf-Trees. 26 | ### multiple tf-trees 27 |
28 | stage two rviz widowns showing two robot views
29 |
30 | Multiple TF-trees after with two rviz nodes for robot_0 and robot_1:
# ros2 launch stage_ros2 stage.launch.py world:=cave_three_robots one_tf_tree:=false 31 |
# ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns namespace:=robot_0 32 |
# ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns namespace:=robot_1 33 |
on Ubuntu 22.04 with ros2 humble 34 |
35 |
36 | 37 | ### one tf-tree 38 |
39 | stage and rviz with laser, tf, and cameras
40 |
41 | One TF-tree after:
# ros2 launch stage_ros2 demo.launch.py world:=cave_three_robots one_tf_tree:=true 42 |
on Ubuntu 22.04 with ros2 humble 43 |
44 |
45 | 46 | 47 | 48 | 49 | ## Thanks 50 | I like to thank all people behind [Stage](https://github.com/rtv/Stage) and [ros-simulation/stage_ros](https://github.com/ros-simulation) for the simulation and the first ROS bridge as well as the people for the initial ROS2 implementations [ShengliangD/stage_ros2](https://github.com/ShengliangD/stage_ros2) and [n0nzzz/stage_ros2](https://github.com/n0nzzz/stage_ros2). The work was used as base for my implementation. Which differs to the previous ROS interfaces in its modularity. 51 | -------------------------------------------------------------------------------- /config/rviz/cave.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 553 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: LaserScan 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz_default_plugins/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | All Enabled: true 56 | base_link: 57 | Value: true 58 | laser: 59 | Value: true 60 | Marker Scale: 1 61 | Name: TF 62 | Show Arrows: true 63 | Show Axes: true 64 | Show Names: false 65 | Tree: 66 | base_link: 67 | laser: 68 | {} 69 | Update Interval: 0 70 | Value: true 71 | - Alpha: 1 72 | Autocompute Intensity Bounds: true 73 | Autocompute Value Bounds: 74 | Max Value: 10 75 | Min Value: -10 76 | Value: true 77 | Axis: Z 78 | Channel Name: intensity 79 | Class: rviz_default_plugins/LaserScan 80 | Color: 255; 255; 255 81 | Color Transformer: Intensity 82 | Decay Time: 0 83 | Enabled: true 84 | Invert Rainbow: false 85 | Max Color: 255; 255; 255 86 | Max Intensity: 1 87 | Min Color: 0; 0; 0 88 | Min Intensity: 1 89 | Name: LaserScan 90 | Position Transformer: XYZ 91 | Selectable: true 92 | Size (Pixels): 3 93 | Size (m): 0.009999999776482582 94 | Style: Flat Squares 95 | Topic: 96 | Depth: 5 97 | Durability Policy: Volatile 98 | Filter size: 10 99 | History Policy: Keep Last 100 | Reliability Policy: Reliable 101 | Value: base_scan 102 | Use Fixed Frame: true 103 | Use rainbow: true 104 | Value: true 105 | Enabled: true 106 | Global Options: 107 | Background Color: 48; 48; 48 108 | Fixed Frame: base_link 109 | Frame Rate: 30 110 | Name: root 111 | Tools: 112 | - Class: rviz_default_plugins/Interact 113 | Hide Inactive Objects: true 114 | - Class: rviz_default_plugins/MoveCamera 115 | - Class: rviz_default_plugins/Select 116 | - Class: rviz_default_plugins/FocusCamera 117 | - Class: rviz_default_plugins/Measure 118 | Line color: 128; 128; 0 119 | - Class: rviz_default_plugins/SetInitialPose 120 | Covariance x: 0.25 121 | Covariance y: 0.25 122 | Covariance yaw: 0.06853891909122467 123 | Topic: 124 | Depth: 5 125 | Durability Policy: Volatile 126 | History Policy: Keep Last 127 | Reliability Policy: Reliable 128 | Value: /initialpose 129 | - Class: rviz_default_plugins/SetGoal 130 | Topic: 131 | Depth: 5 132 | Durability Policy: Volatile 133 | History Policy: Keep Last 134 | Reliability Policy: Reliable 135 | Value: /goal_pose 136 | - Class: rviz_default_plugins/PublishPoint 137 | Single click: true 138 | Topic: 139 | Depth: 5 140 | Durability Policy: Volatile 141 | History Policy: Keep Last 142 | Reliability Policy: Reliable 143 | Value: /clicked_point 144 | Transformation: 145 | Current: 146 | Class: rviz_default_plugins/TF 147 | Value: true 148 | Views: 149 | Current: 150 | Class: rviz_default_plugins/Orbit 151 | Distance: 7.721435546875 152 | Enable Stereo Rendering: 153 | Stereo Eye Separation: 0.05999999865889549 154 | Stereo Focal Distance: 1 155 | Swap Stereo Eyes: false 156 | Value: false 157 | Focal Point: 158 | X: 0 159 | Y: 0 160 | Z: 0 161 | Focal Shape Fixed Size: true 162 | Focal Shape Size: 0.05000000074505806 163 | Invert Z Axis: false 164 | Name: Current View 165 | Near Clip Distance: 0.009999999776482582 166 | Pitch: 0.785398006439209 167 | Target Frame: 168 | Value: Orbit (rviz) 169 | Yaw: 0.785398006439209 170 | Saved: ~ 171 | Window Geometry: 172 | Displays: 173 | collapsed: false 174 | Height: 846 175 | Hide Left Dock: false 176 | Hide Right Dock: false 177 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 178 | Selection: 179 | collapsed: false 180 | Time: 181 | collapsed: false 182 | Tool Properties: 183 | collapsed: false 184 | Views: 185 | collapsed: false 186 | Width: 1200 187 | X: 60 188 | Y: 60 189 | -------------------------------------------------------------------------------- /config/rviz/cave_seven_robots.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /LaserScan1 11 | Splitter Ratio: 0.5 12 | Tree Height: 532 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz_default_plugins/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.029999999329447746 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Autocompute Intensity Bounds: true 54 | Autocompute Value Bounds: 55 | Max Value: 10 56 | Min Value: -10 57 | Value: true 58 | Axis: Z 59 | Channel Name: intensity 60 | Class: rviz_default_plugins/LaserScan 61 | Color: 255; 255; 255 62 | Color Transformer: Intensity 63 | Decay Time: 0 64 | Enabled: true 65 | Invert Rainbow: false 66 | Max Color: 255; 255; 255 67 | Max Intensity: 1 68 | Min Color: 0; 0; 0 69 | Min Intensity: 1 70 | Name: LaserScan 71 | Position Transformer: XYZ 72 | Selectable: true 73 | Size (Pixels): 3 74 | Size (m): 0.009999999776482582 75 | Style: Flat Squares 76 | Topic: 77 | Depth: 5 78 | Durability Policy: Volatile 79 | Filter size: 10 80 | History Policy: Keep Last 81 | Reliability Policy: Reliable 82 | Value: /robot_0/base_scan 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | - Class: rviz_default_plugins/TF 87 | Enabled: true 88 | Frame Timeout: 15 89 | Frames: 90 | All Enabled: true 91 | robot_0/base_link: 92 | Value: true 93 | robot_0/laser: 94 | Value: true 95 | robot_0/odom: 96 | Value: true 97 | robot_1/base_link: 98 | Value: true 99 | robot_1/laser: 100 | Value: true 101 | robot_1/odom: 102 | Value: true 103 | robot_2/base_link: 104 | Value: true 105 | robot_2/laser: 106 | Value: true 107 | robot_2/odom: 108 | Value: true 109 | robot_3/base_link: 110 | Value: true 111 | robot_3/laser: 112 | Value: true 113 | robot_3/odom: 114 | Value: true 115 | robot_4/base_link: 116 | Value: true 117 | robot_4/laser: 118 | Value: true 119 | robot_4/odom: 120 | Value: true 121 | robot_5/base_link: 122 | Value: true 123 | robot_5/laser: 124 | Value: true 125 | robot_5/odom: 126 | Value: true 127 | robot_6/base_link: 128 | Value: true 129 | robot_6/laser: 130 | Value: true 131 | robot_6/odom: 132 | Value: true 133 | Marker Scale: 1 134 | Name: TF 135 | Show Arrows: true 136 | Show Axes: true 137 | Show Names: false 138 | Tree: 139 | robot_0/odom: 140 | robot_0/base_link: 141 | robot_0/laser: 142 | {} 143 | Update Interval: 0 144 | Value: true 145 | Enabled: true 146 | Global Options: 147 | Background Color: 48; 48; 48 148 | Fixed Frame: robot_0/base_link 149 | Frame Rate: 30 150 | Name: root 151 | Tools: 152 | - Class: rviz_default_plugins/Interact 153 | Hide Inactive Objects: true 154 | - Class: rviz_default_plugins/MoveCamera 155 | - Class: rviz_default_plugins/Select 156 | - Class: rviz_default_plugins/FocusCamera 157 | - Class: rviz_default_plugins/Measure 158 | Line color: 128; 128; 0 159 | - Class: rviz_default_plugins/SetInitialPose 160 | Covariance x: 0.25 161 | Covariance y: 0.25 162 | Covariance yaw: 0.06853891909122467 163 | Topic: 164 | Depth: 5 165 | Durability Policy: Volatile 166 | History Policy: Keep Last 167 | Reliability Policy: Reliable 168 | Value: /initialpose 169 | - Class: rviz_default_plugins/SetGoal 170 | Topic: 171 | Depth: 5 172 | Durability Policy: Volatile 173 | History Policy: Keep Last 174 | Reliability Policy: Reliable 175 | Value: /goal_pose 176 | - Class: rviz_default_plugins/PublishPoint 177 | Single click: true 178 | Topic: 179 | Depth: 5 180 | Durability Policy: Volatile 181 | History Policy: Keep Last 182 | Reliability Policy: Reliable 183 | Value: /clicked_point 184 | Transformation: 185 | Current: 186 | Class: rviz_default_plugins/TF 187 | Value: true 188 | Views: 189 | Current: 190 | Class: rviz_default_plugins/Orbit 191 | Distance: 13.840161323547363 192 | Enable Stereo Rendering: 193 | Stereo Eye Separation: 0.05999999865889549 194 | Stereo Focal Distance: 1 195 | Swap Stereo Eyes: false 196 | Value: false 197 | Focal Point: 198 | X: 0 199 | Y: 0 200 | Z: 0 201 | Focal Shape Fixed Size: true 202 | Focal Shape Size: 0.05000000074505806 203 | Invert Z Axis: false 204 | Name: Current View 205 | Near Clip Distance: 0.009999999776482582 206 | Pitch: 0.785398006439209 207 | Target Frame: 208 | Value: Orbit (rviz) 209 | Yaw: 0.785398006439209 210 | Saved: ~ 211 | Window Geometry: 212 | Displays: 213 | collapsed: false 214 | Height: 846 215 | Hide Left Dock: false 216 | Hide Right Dock: false 217 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002a6000000d000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002a6000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650100000000000004b00000025000fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 218 | Selection: 219 | collapsed: false 220 | Time: 221 | collapsed: false 222 | Tool Properties: 223 | collapsed: false 224 | Views: 225 | collapsed: false 226 | Width: 1200 227 | X: 60 228 | Y: 60 229 | -------------------------------------------------------------------------------- /config/rviz/cave_three_robots.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /r0 Odometry1/Topic1 10 | - /r0 LaserScan1/Topic1 11 | - /TF1/Frames1 12 | - /TF1/Tree1 13 | Splitter Ratio: 0.5 14 | Tree Height: 734 15 | - Class: rviz_common/Selection 16 | Name: Selection 17 | - Class: rviz_common/Tool Properties 18 | Expanded: 19 | - /2D Goal Pose1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz_common/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz_common/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: r0 LaserScan 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz_default_plugins/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Angle Tolerance: 0.10000000149011612 55 | Class: rviz_default_plugins/Odometry 56 | Covariance: 57 | Orientation: 58 | Alpha: 0.5 59 | Color: 255; 255; 127 60 | Color Style: Unique 61 | Frame: Local 62 | Offset: 1 63 | Scale: 1 64 | Value: true 65 | Position: 66 | Alpha: 0.30000001192092896 67 | Color: 204; 51; 204 68 | Scale: 1 69 | Value: true 70 | Value: true 71 | Enabled: true 72 | Keep: 100 73 | Name: r0 Odometry 74 | Position Tolerance: 0.10000000149011612 75 | Shape: 76 | Alpha: 1 77 | Axes Length: 1 78 | Axes Radius: 0.10000000149011612 79 | Color: 255; 25; 0 80 | Head Length: 0.30000001192092896 81 | Head Radius: 0.10000000149011612 82 | Shaft Length: 1 83 | Shaft Radius: 0.05000000074505806 84 | Value: Arrow 85 | Topic: 86 | Depth: 5 87 | Durability Policy: Volatile 88 | Filter size: 10 89 | History Policy: Keep Last 90 | Reliability Policy: Reliable 91 | Value: /robot_0/odom 92 | Value: true 93 | - Angle Tolerance: 0.10000000149011612 94 | Class: rviz_default_plugins/Odometry 95 | Covariance: 96 | Orientation: 97 | Alpha: 0.5 98 | Color: 255; 255; 127 99 | Color Style: Unique 100 | Frame: Local 101 | Offset: 1 102 | Scale: 1 103 | Value: true 104 | Position: 105 | Alpha: 0.30000001192092896 106 | Color: 204; 51; 204 107 | Scale: 1 108 | Value: true 109 | Value: true 110 | Enabled: false 111 | Keep: 100 112 | Name: r1 Odometry 113 | Position Tolerance: 0.10000000149011612 114 | Shape: 115 | Alpha: 1 116 | Axes Length: 1 117 | Axes Radius: 0.10000000149011612 118 | Color: 51; 209; 122 119 | Head Length: 0.30000001192092896 120 | Head Radius: 0.10000000149011612 121 | Shaft Length: 1 122 | Shaft Radius: 0.05000000074505806 123 | Value: Arrow 124 | Topic: 125 | Depth: 5 126 | Durability Policy: Volatile 127 | Filter size: 10 128 | History Policy: Keep Last 129 | Reliability Policy: Reliable 130 | Value: /robot_1/odom 131 | Value: false 132 | - Alpha: 1 133 | Autocompute Intensity Bounds: true 134 | Autocompute Value Bounds: 135 | Max Value: 10 136 | Min Value: -10 137 | Value: true 138 | Axis: Z 139 | Channel Name: intensity 140 | Class: rviz_default_plugins/LaserScan 141 | Color: 255; 255; 255 142 | Color Transformer: Intensity 143 | Decay Time: 0 144 | Enabled: true 145 | Invert Rainbow: false 146 | Max Color: 255; 255; 255 147 | Max Intensity: 1 148 | Min Color: 0; 0; 0 149 | Min Intensity: 1 150 | Name: r0 LaserScan 151 | Position Transformer: XYZ 152 | Selectable: true 153 | Size (Pixels): 3 154 | Size (m): 0.05000000074505806 155 | Style: Flat Squares 156 | Topic: 157 | Depth: 5 158 | Durability Policy: Volatile 159 | Filter size: 10 160 | History Policy: Keep Last 161 | Reliability Policy: Reliable 162 | Value: /robot_0/base_scan 163 | Use Fixed Frame: true 164 | Use rainbow: true 165 | Value: true 166 | - Class: rviz_default_plugins/Camera 167 | Enabled: true 168 | Far Plane Distance: 100 169 | Image Rendering: background and overlay 170 | Name: r0 Camera 171 | Overlay Alpha: 0.5 172 | Topic: 173 | Depth: 5 174 | Durability Policy: Volatile 175 | History Policy: Keep Last 176 | Reliability Policy: Reliable 177 | Value: /robot_0/depth 178 | Value: true 179 | Visibility: 180 | Grid: true 181 | TF: true 182 | Value: true 183 | r0 Image Depth: true 184 | r0 Image RGB: true 185 | r0 LaserScan: true 186 | r0 Odometry: true 187 | r1 Odometry: true 188 | Zoom Factor: 1 189 | - Class: rviz_default_plugins/Image 190 | Enabled: true 191 | Max Value: 1 192 | Median window: 5 193 | Min Value: 0 194 | Name: r0 Image RGB 195 | Normalize Range: true 196 | Topic: 197 | Depth: 5 198 | Durability Policy: Volatile 199 | History Policy: Keep Last 200 | Reliability Policy: Reliable 201 | Value: /robot_0/image 202 | Value: true 203 | - Class: rviz_default_plugins/Image 204 | Enabled: true 205 | Max Value: 5 206 | Median window: 5 207 | Min Value: 0 208 | Name: r0 Image Depth 209 | Normalize Range: false 210 | Topic: 211 | Depth: 5 212 | Durability Policy: Volatile 213 | History Policy: Keep Last 214 | Reliability Policy: Reliable 215 | Value: /robot_0/depth 216 | Value: true 217 | - Class: rviz_default_plugins/TF 218 | Enabled: true 219 | Frame Timeout: 15 220 | Frames: 221 | All Enabled: true 222 | robot_0/base_link: 223 | Value: true 224 | robot_0/camera: 225 | Value: true 226 | robot_0/laser: 227 | Value: true 228 | robot_0/odom: 229 | Value: true 230 | robot_1/base_link: 231 | Value: true 232 | robot_1/camera: 233 | Value: true 234 | robot_1/laser: 235 | Value: true 236 | robot_1/odom: 237 | Value: true 238 | robot_2/base_link: 239 | Value: true 240 | robot_2/laser1: 241 | Value: true 242 | robot_2/laser2: 243 | Value: true 244 | robot_2/odom: 245 | Value: true 246 | Marker Scale: 1 247 | Name: TF 248 | Show Arrows: true 249 | Show Axes: true 250 | Show Names: true 251 | Tree: 252 | robot_0/odom: 253 | robot_0/base_link: 254 | robot_0/camera: 255 | {} 256 | robot_0/laser: 257 | {} 258 | Update Interval: 0 259 | Value: true 260 | Enabled: true 261 | Global Options: 262 | Background Color: 48; 48; 48 263 | Fixed Frame: robot_0/base_link 264 | Frame Rate: 30 265 | Name: root 266 | Tools: 267 | - Class: rviz_default_plugins/Interact 268 | Hide Inactive Objects: true 269 | - Class: rviz_default_plugins/MoveCamera 270 | - Class: rviz_default_plugins/Select 271 | - Class: rviz_default_plugins/FocusCamera 272 | - Class: rviz_default_plugins/Measure 273 | Line color: 128; 128; 0 274 | - Class: rviz_default_plugins/SetInitialPose 275 | Covariance x: 0.25 276 | Covariance y: 0.25 277 | Covariance yaw: 0.06853891909122467 278 | Topic: 279 | Depth: 5 280 | Durability Policy: Volatile 281 | History Policy: Keep Last 282 | Reliability Policy: Reliable 283 | Value: /initialpose 284 | - Class: rviz_default_plugins/SetGoal 285 | Topic: 286 | Depth: 5 287 | Durability Policy: Volatile 288 | History Policy: Keep Last 289 | Reliability Policy: Reliable 290 | Value: /goal_pose 291 | - Class: rviz_default_plugins/PublishPoint 292 | Single click: true 293 | Topic: 294 | Depth: 5 295 | Durability Policy: Volatile 296 | History Policy: Keep Last 297 | Reliability Policy: Reliable 298 | Value: /clicked_point 299 | Transformation: 300 | Current: 301 | Class: rviz_default_plugins/TF 302 | Value: true 303 | Views: 304 | Current: 305 | Class: rviz_default_plugins/Orbit 306 | Distance: 0.453931599855423 307 | Enable Stereo Rendering: 308 | Stereo Eye Separation: 0.05999999865889549 309 | Stereo Focal Distance: 1 310 | Swap Stereo Eyes: false 311 | Value: false 312 | Focal Point: 313 | X: -0.8209503889083862 314 | Y: -4.57040548324585 315 | Z: 2.9276254177093506 316 | Focal Shape Fixed Size: true 317 | Focal Shape Size: 0.05000000074505806 318 | Invert Z Axis: false 319 | Name: Current View 320 | Near Clip Distance: 0.009999999776482582 321 | Pitch: 0.38539785146713257 322 | Target Frame: 323 | Value: Orbit (rviz) 324 | Yaw: 4.175403118133545 325 | Saved: ~ 326 | Window Geometry: 327 | Displays: 328 | collapsed: false 329 | Height: 1031 330 | Hide Left Dock: false 331 | Hide Right Dock: false 332 | QMainWindow State: 000000ff00000000fd00000004000000000000023300000369fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000369000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001ae00000369fc0200000006fb0000001200720030002000430061006d006500720061010000003d000000f30000002800fffffffb000000180072003000200049006d006100670065002000520047004201000001360000010f0000002800fffffffb0000001c0072003000200049006d006100670065002000440065007000740068010000024b0000015b0000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000026600fffffffb0000000800540069006d00650100000000000004500000000000000000000003930000036900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 333 | Selection: 334 | collapsed: false 335 | Time: 336 | collapsed: false 337 | Tool Properties: 338 | collapsed: false 339 | Views: 340 | collapsed: false 341 | Width: 1920 342 | X: 0 343 | Y: 25 344 | r0 Camera: 345 | collapsed: false 346 | r0 Image Depth: 347 | collapsed: false 348 | r0 Image RGB: 349 | collapsed: false 350 | -------------------------------------------------------------------------------- /config/rviz/lines.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 532 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Class: rviz_default_plugins/TF 51 | Enabled: true 52 | Frame Timeout: 15 53 | Frames: 54 | All Enabled: true 55 | base_link: 56 | Value: true 57 | laser: 58 | Value: true 59 | odom: 60 | Value: true 61 | Marker Scale: 1 62 | Name: TF 63 | Show Arrows: true 64 | Show Axes: true 65 | Show Names: false 66 | Tree: 67 | odom: 68 | base_link: 69 | laser: 70 | {} 71 | Update Interval: 0 72 | Value: true 73 | - Alpha: 1 74 | Autocompute Intensity Bounds: true 75 | Autocompute Value Bounds: 76 | Max Value: 10 77 | Min Value: -10 78 | Value: true 79 | Axis: Z 80 | Channel Name: intensity 81 | Class: rviz_default_plugins/LaserScan 82 | Color: 255; 255; 255 83 | Color Transformer: Intensity 84 | Decay Time: 0 85 | Enabled: true 86 | Invert Rainbow: false 87 | Max Color: 255; 255; 255 88 | Max Intensity: 1 89 | Min Color: 0; 0; 0 90 | Min Intensity: 1 91 | Name: LaserScan 92 | Position Transformer: XYZ 93 | Selectable: true 94 | Size (Pixels): 3 95 | Size (m): 0.009999999776482582 96 | Style: Flat Squares 97 | Topic: 98 | Depth: 5 99 | Durability Policy: Volatile 100 | Filter size: 10 101 | History Policy: Keep Last 102 | Reliability Policy: Reliable 103 | Value: /base_scan 104 | Use Fixed Frame: true 105 | Use rainbow: true 106 | Value: true 107 | Enabled: true 108 | Global Options: 109 | Background Color: 48; 48; 48 110 | Fixed Frame: base_link 111 | Frame Rate: 30 112 | Name: root 113 | Tools: 114 | - Class: rviz_default_plugins/Interact 115 | Hide Inactive Objects: true 116 | - Class: rviz_default_plugins/MoveCamera 117 | - Class: rviz_default_plugins/Select 118 | - Class: rviz_default_plugins/FocusCamera 119 | - Class: rviz_default_plugins/Measure 120 | Line color: 128; 128; 0 121 | - Class: rviz_default_plugins/SetInitialPose 122 | Covariance x: 0.25 123 | Covariance y: 0.25 124 | Covariance yaw: 0.06853891909122467 125 | Topic: 126 | Depth: 5 127 | Durability Policy: Volatile 128 | History Policy: Keep Last 129 | Reliability Policy: Reliable 130 | Value: /initialpose 131 | - Class: rviz_default_plugins/SetGoal 132 | Topic: 133 | Depth: 5 134 | Durability Policy: Volatile 135 | History Policy: Keep Last 136 | Reliability Policy: Reliable 137 | Value: /goal_pose 138 | - Class: rviz_default_plugins/PublishPoint 139 | Single click: true 140 | Topic: 141 | Depth: 5 142 | Durability Policy: Volatile 143 | History Policy: Keep Last 144 | Reliability Policy: Reliable 145 | Value: /clicked_point 146 | Transformation: 147 | Current: 148 | Class: rviz_default_plugins/TF 149 | Value: true 150 | Views: 151 | Current: 152 | Class: rviz_default_plugins/Orbit 153 | Distance: 7.721435546875 154 | Enable Stereo Rendering: 155 | Stereo Eye Separation: 0.05999999865889549 156 | Stereo Focal Distance: 1 157 | Swap Stereo Eyes: false 158 | Value: false 159 | Focal Point: 160 | X: 0 161 | Y: 0 162 | Z: 0 163 | Focal Shape Fixed Size: true 164 | Focal Shape Size: 0.05000000074505806 165 | Invert Z Axis: false 166 | Name: Current View 167 | Near Clip Distance: 0.009999999776482582 168 | Pitch: 0.785398006439209 169 | Target Frame: 170 | Value: Orbit (rviz) 171 | Yaw: 0.785398006439209 172 | Saved: ~ 173 | Window Geometry: 174 | Displays: 175 | collapsed: false 176 | Height: 846 177 | Hide Left Dock: false 178 | Hide Right Dock: false 179 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002a6000000d000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002a6000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650100000000000004b00000025000fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 180 | Selection: 181 | collapsed: false 182 | Time: 183 | collapsed: false 184 | Tool Properties: 185 | collapsed: false 186 | Views: 187 | collapsed: false 188 | Width: 1200 189 | X: 60 190 | Y: 60 191 | -------------------------------------------------------------------------------- /config/rviz/robot0.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1/Status1 10 | - /LaserScan1/Topic1 11 | - /Image1 12 | - /Image2 13 | - /Camera1 14 | Splitter Ratio: 0.32846716046333313 15 | Tree Height: 165 16 | - Class: rviz_common/Selection 17 | Name: Selection 18 | - Class: rviz_common/Tool Properties 19 | Expanded: 20 | - /2D Goal Pose1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz_common/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz_common/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: LaserScan 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz_default_plugins/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz_default_plugins/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | base_footprint: 61 | Value: true 62 | base_laser_link: 63 | Value: true 64 | base_link: 65 | Value: true 66 | camera: 67 | Value: true 68 | odom: 69 | Value: true 70 | robot_0/base_footprint: 71 | Value: true 72 | robot_0/base_laser_link_0: 73 | Value: true 74 | robot_0/base_laser_link_1: 75 | Value: true 76 | robot_0/base_link: 77 | Value: true 78 | robot_0/camera_0: 79 | Value: true 80 | robot_0/camera_1: 81 | Value: true 82 | robot_0/odom: 83 | Value: true 84 | robot_1/base_footprint: 85 | Value: true 86 | robot_1/base_laser_link_0: 87 | Value: true 88 | robot_1/base_laser_link_1: 89 | Value: true 90 | robot_1/base_link: 91 | Value: true 92 | robot_1/camera_0: 93 | Value: true 94 | robot_1/camera_1: 95 | Value: true 96 | robot_1/odom: 97 | Value: true 98 | robot_2/base_footprint: 99 | Value: true 100 | robot_2/base_laser_link_0: 101 | Value: true 102 | robot_2/base_laser_link_1: 103 | Value: true 104 | robot_2/base_link: 105 | Value: true 106 | robot_2/camera_0: 107 | Value: true 108 | robot_2/camera_1: 109 | Value: true 110 | robot_2/odom: 111 | Value: true 112 | robot_3/base_footprint: 113 | Value: true 114 | robot_3/base_laser_link_0: 115 | Value: true 116 | robot_3/base_laser_link_1: 117 | Value: true 118 | robot_3/base_link: 119 | Value: true 120 | robot_3/camera_0: 121 | Value: true 122 | robot_3/camera_1: 123 | Value: true 124 | robot_3/odom: 125 | Value: true 126 | Marker Scale: 1 127 | Name: TF 128 | Show Arrows: true 129 | Show Axes: true 130 | Show Names: false 131 | Tree: 132 | odom: 133 | base_footprint: 134 | base_link: 135 | base_laser_link: 136 | {} 137 | camera: 138 | {} 139 | robot_0/base_laser_link_0: 140 | {} 141 | Update Interval: 0 142 | Value: true 143 | - Alpha: 1 144 | Autocompute Intensity Bounds: true 145 | Autocompute Value Bounds: 146 | Max Value: 10 147 | Min Value: -10 148 | Value: true 149 | Axis: Z 150 | Channel Name: intensity 151 | Class: rviz_default_plugins/LaserScan 152 | Color: 255; 255; 255 153 | Color Transformer: Intensity 154 | Decay Time: 0 155 | Enabled: true 156 | Invert Rainbow: false 157 | Max Color: 255; 255; 255 158 | Max Intensity: 1 159 | Min Color: 0; 0; 0 160 | Min Intensity: 1 161 | Name: LaserScan 162 | Position Transformer: XYZ 163 | Selectable: true 164 | Size (Pixels): 3 165 | Size (m): 0.05000000074505806 166 | Style: Flat Squares 167 | Topic: 168 | Depth: 5 169 | Durability Policy: Volatile 170 | Filter size: 10 171 | History Policy: Keep Last 172 | Reliability Policy: Reliable 173 | Value: /robot_0/base_scan_1 174 | Use Fixed Frame: true 175 | Use rainbow: true 176 | Value: true 177 | - Class: rviz_default_plugins/Image 178 | Enabled: false 179 | Max Value: 1 180 | Median window: 5 181 | Min Value: 0 182 | Name: Image 183 | Normalize Range: true 184 | Topic: 185 | Depth: 5 186 | Durability Policy: Volatile 187 | History Policy: Keep Last 188 | Reliability Policy: Reliable 189 | Value: /image 190 | Value: false 191 | - Class: rviz_default_plugins/Image 192 | Enabled: true 193 | Max Value: 5 194 | Median window: 5 195 | Min Value: 0 196 | Name: Image 197 | Normalize Range: false 198 | Topic: 199 | Depth: 5 200 | Durability Policy: Volatile 201 | History Policy: Keep Last 202 | Reliability Policy: Reliable 203 | Value: /depth 204 | Value: true 205 | - Class: rviz_default_plugins/Camera 206 | Enabled: true 207 | Far Plane Distance: 100 208 | Image Rendering: background and overlay 209 | Name: Camera 210 | Overlay Alpha: 0.5 211 | Topic: 212 | Depth: 5 213 | Durability Policy: Volatile 214 | History Policy: Keep Last 215 | Reliability Policy: Reliable 216 | Value: /depth 217 | Value: true 218 | Visibility: 219 | Grid: true 220 | Image: true 221 | LaserScan: true 222 | TF: true 223 | Value: true 224 | Zoom Factor: 1 225 | Enabled: true 226 | Global Options: 227 | Background Color: 48; 48; 48 228 | Fixed Frame: base_laser_link 229 | Frame Rate: 30 230 | Name: root 231 | Tools: 232 | - Class: rviz_default_plugins/Interact 233 | Hide Inactive Objects: true 234 | - Class: rviz_default_plugins/MoveCamera 235 | - Class: rviz_default_plugins/Select 236 | - Class: rviz_default_plugins/FocusCamera 237 | - Class: rviz_default_plugins/Measure 238 | Line color: 128; 128; 0 239 | - Class: rviz_default_plugins/SetInitialPose 240 | Covariance x: 0.25 241 | Covariance y: 0.25 242 | Covariance yaw: 0.06853891909122467 243 | Topic: 244 | Depth: 5 245 | Durability Policy: Volatile 246 | History Policy: Keep Last 247 | Reliability Policy: Reliable 248 | Value: /initialpose 249 | - Class: rviz_default_plugins/SetGoal 250 | Topic: 251 | Depth: 5 252 | Durability Policy: Volatile 253 | History Policy: Keep Last 254 | Reliability Policy: Reliable 255 | Value: /goal_pose 256 | - Class: rviz_default_plugins/PublishPoint 257 | Single click: true 258 | Topic: 259 | Depth: 5 260 | Durability Policy: Volatile 261 | History Policy: Keep Last 262 | Reliability Policy: Reliable 263 | Value: /clicked_point 264 | Transformation: 265 | Current: 266 | Class: rviz_default_plugins/TF 267 | Value: true 268 | Views: 269 | Current: 270 | Class: rviz_default_plugins/Orbit 271 | Distance: 9.526798248291016 272 | Enable Stereo Rendering: 273 | Stereo Eye Separation: 0.05999999865889549 274 | Stereo Focal Distance: 1 275 | Swap Stereo Eyes: false 276 | Value: false 277 | Focal Point: 278 | X: 0.009052327834069729 279 | Y: -0.14499789476394653 280 | Z: 0.0961274728178978 281 | Focal Shape Fixed Size: true 282 | Focal Shape Size: 0.05000000074505806 283 | Invert Z Axis: false 284 | Name: Current View 285 | Near Clip Distance: 0.009999999776482582 286 | Pitch: 0.8003981113433838 287 | Target Frame: 288 | Value: Orbit (rviz) 289 | Yaw: 5.638582706451416 290 | Saved: ~ 291 | Window Geometry: 292 | Camera: 293 | collapsed: false 294 | Displays: 295 | collapsed: false 296 | Height: 846 297 | Hide Left Dock: false 298 | Hide Right Dock: false 299 | Image: 300 | collapsed: false 301 | QMainWindow State: 000000ff00000000fd000000040000000000000226000002b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000128000000c900fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005900fffffffb000000100044006900730070006c0061007900730100000000000002260000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000200000000ed0000000000000000fb0000000a0049006d006100670065010000016b000000bc0000002800fffffffb0000000c00430061006d006500720061010000022d000000c00000002800ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026600fffffffb0000000800540069006d0065010000000000000450000000000000000000000284000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 302 | Selection: 303 | collapsed: false 304 | Time: 305 | collapsed: false 306 | Tool Properties: 307 | collapsed: false 308 | Views: 309 | collapsed: false 310 | Width: 1200 311 | X: 60 312 | Y: 60 313 | -------------------------------------------------------------------------------- /config/rviz/robot_ns.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /Camera1 11 | - /LaserScan1 12 | Splitter Ratio: 0.5 13 | Tree Height: 470 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 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz_default_plugins/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | base_link: 59 | Value: true 60 | camera: 61 | Value: true 62 | laser: 63 | Value: true 64 | odom: 65 | Value: true 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: false 71 | Tree: 72 | odom: 73 | base_link: 74 | camera: 75 | {} 76 | laser: 77 | {} 78 | Update Interval: 0 79 | Value: true 80 | - Class: rviz_default_plugins/Image 81 | Enabled: false 82 | Max Value: 1 83 | Median window: 5 84 | Min Value: 0 85 | Name: Image 86 | Normalize Range: true 87 | Topic: 88 | Depth: 5 89 | Durability Policy: Volatile 90 | History Policy: Keep Last 91 | Reliability Policy: Reliable 92 | Value: /robot_1/depth 93 | Value: false 94 | - Class: rviz_default_plugins/Camera 95 | Enabled: true 96 | Far Plane Distance: 100 97 | Image Rendering: background and overlay 98 | Name: Camera 99 | Overlay Alpha: 0.5 100 | Topic: 101 | Depth: 5 102 | Durability Policy: Volatile 103 | History Policy: Keep Last 104 | Reliability Policy: Reliable 105 | Value: image 106 | Value: true 107 | Visibility: 108 | Grid: true 109 | Image: true 110 | LaserScan: true 111 | TF: true 112 | Value: true 113 | Zoom Factor: 1 114 | - Alpha: 1 115 | Autocompute Intensity Bounds: true 116 | Autocompute Value Bounds: 117 | Max Value: 10 118 | Min Value: -10 119 | Value: true 120 | Axis: Z 121 | Channel Name: intensity 122 | Class: rviz_default_plugins/LaserScan 123 | Color: 255; 255; 255 124 | Color Transformer: Intensity 125 | Decay Time: 0 126 | Enabled: true 127 | Invert Rainbow: false 128 | Max Color: 255; 255; 255 129 | Max Intensity: 1 130 | Min Color: 0; 0; 0 131 | Min Intensity: 1 132 | Name: LaserScan 133 | Position Transformer: XYZ 134 | Selectable: true 135 | Size (Pixels): 3 136 | Size (m): 0.019999999552965164 137 | Style: Flat Squares 138 | Topic: 139 | Depth: 5 140 | Durability Policy: Volatile 141 | Filter size: 10 142 | History Policy: Keep Last 143 | Reliability Policy: Reliable 144 | Value: base_scan 145 | Use Fixed Frame: true 146 | Use rainbow: true 147 | Value: true 148 | Enabled: true 149 | Global Options: 150 | Background Color: 48; 48; 48 151 | Fixed Frame: base_link 152 | Frame Rate: 30 153 | Name: root 154 | Tools: 155 | - Class: rviz_default_plugins/Interact 156 | Hide Inactive Objects: true 157 | - Class: rviz_default_plugins/MoveCamera 158 | - Class: rviz_default_plugins/Select 159 | - Class: rviz_default_plugins/FocusCamera 160 | - Class: rviz_default_plugins/Measure 161 | Line color: 128; 128; 0 162 | - Class: rviz_default_plugins/SetInitialPose 163 | Covariance x: 0.25 164 | Covariance y: 0.25 165 | Covariance yaw: 0.06853891909122467 166 | Topic: 167 | Depth: 5 168 | Durability Policy: Volatile 169 | History Policy: Keep Last 170 | Reliability Policy: Reliable 171 | Value: /initialpose 172 | - Class: rviz_default_plugins/SetGoal 173 | Topic: 174 | Depth: 5 175 | Durability Policy: Volatile 176 | History Policy: Keep Last 177 | Reliability Policy: Reliable 178 | Value: /goal_pose 179 | - Class: rviz_default_plugins/PublishPoint 180 | Single click: true 181 | Topic: 182 | Depth: 5 183 | Durability Policy: Volatile 184 | History Policy: Keep Last 185 | Reliability Policy: Reliable 186 | Value: /clicked_point 187 | Transformation: 188 | Current: 189 | Class: rviz_default_plugins/TF 190 | Value: true 191 | Views: 192 | Current: 193 | Class: rviz_default_plugins/Orbit 194 | Distance: 12.790231704711914 195 | Enable Stereo Rendering: 196 | Stereo Eye Separation: 0.05999999865889549 197 | Stereo Focal Distance: 1 198 | Swap Stereo Eyes: false 199 | Value: false 200 | Focal Point: 201 | X: 0 202 | Y: 0 203 | Z: 0 204 | Focal Shape Fixed Size: true 205 | Focal Shape Size: 0.05000000074505806 206 | Invert Z Axis: false 207 | Name: Current View 208 | Near Clip Distance: 0.009999999776482582 209 | Pitch: 0.785398006439209 210 | Target Frame: 211 | Value: Orbit (rviz) 212 | Yaw: 0.785398006439209 213 | Saved: ~ 214 | Window Geometry: 215 | Camera: 216 | collapsed: false 217 | Displays: 218 | collapsed: false 219 | Height: 1031 220 | Hide Left Dock: false 221 | Hide Right Dock: false 222 | Image: 223 | collapsed: false 224 | QMainWindow State: 000000ff00000000fd0000000400000000000001b80000035ffc020000000bfb0000001200530065006c0065006300740069006f006e00000000410000007d0000005f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000268000000d000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000022a000000bd0000002b00fffffffb0000000a0049006d006100670065010000022a000000bd0000000000000000fb0000000c00430061006d00650072006101000002af000000f10000002b00ffffff000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002a6000000af00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e900000040fc0100000002fb0000000800540069006d00650100000000000004e90000025000fffffffb0000000800540069006d006501000000000000045000000000000000000000032b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 225 | Selection: 226 | collapsed: false 227 | Time: 228 | collapsed: false 229 | Tool Properties: 230 | collapsed: false 231 | Views: 232 | collapsed: false 233 | Width: 1257 234 | X: 135 235 | Y: 46 236 | -------------------------------------------------------------------------------- /config/rviz/robot_ns_depth.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /DepthCloud1 11 | - /DepthCloud1/Auto Size1 12 | Splitter Ratio: 0.5 13 | Tree Height: 481 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 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz_default_plugins/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | base_link: 59 | Value: true 60 | camera: 61 | Value: true 62 | laser: 63 | Value: true 64 | odom: 65 | Value: true 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: false 71 | Tree: 72 | base_link: 73 | camera: 74 | {} 75 | laser: 76 | {} 77 | odom: 78 | {} 79 | Update Interval: 0 80 | Value: true 81 | - Class: rviz_default_plugins/Image 82 | Enabled: false 83 | Max Value: 1 84 | Median window: 5 85 | Min Value: 0 86 | Name: Image 87 | Normalize Range: true 88 | Topic: 89 | Depth: 5 90 | Durability Policy: Volatile 91 | History Policy: Keep Last 92 | Reliability Policy: Reliable 93 | Value: /robot_1/depth 94 | Value: false 95 | - Class: rviz_default_plugins/Camera 96 | Enabled: true 97 | Far Plane Distance: 100 98 | Image Rendering: background and overlay 99 | Name: Camera 100 | Overlay Alpha: 0.5 101 | Topic: 102 | Depth: 5 103 | Durability Policy: Volatile 104 | History Policy: Keep Last 105 | Reliability Policy: Reliable 106 | Value: image 107 | Value: true 108 | Visibility: 109 | DepthCloud: true 110 | Grid: true 111 | Image: true 112 | LaserScan: true 113 | TF: true 114 | Value: true 115 | Zoom Factor: 1 116 | - Alpha: 1 117 | Autocompute Intensity Bounds: true 118 | Autocompute Value Bounds: 119 | Max Value: 10 120 | Min Value: -10 121 | Value: true 122 | Axis: Z 123 | Channel Name: intensity 124 | Class: rviz_default_plugins/LaserScan 125 | Color: 255; 255; 255 126 | Color Transformer: Intensity 127 | Decay Time: 0 128 | Enabled: true 129 | Invert Rainbow: false 130 | Max Color: 255; 255; 255 131 | Max Intensity: 1 132 | Min Color: 0; 0; 0 133 | Min Intensity: 1 134 | Name: LaserScan 135 | Position Transformer: XYZ 136 | Selectable: true 137 | Size (Pixels): 3 138 | Size (m): 0.019999999552965164 139 | Style: Flat Squares 140 | Topic: 141 | Depth: 5 142 | Durability Policy: Volatile 143 | Filter size: 10 144 | History Policy: Keep Last 145 | Reliability Policy: Reliable 146 | Value: base_scan 147 | Use Fixed Frame: true 148 | Use rainbow: true 149 | Value: true 150 | - Alpha: 1 151 | Auto Size: 152 | Auto Size Factor: 1 153 | Value: true 154 | Autocompute Intensity Bounds: true 155 | Autocompute Value Bounds: 156 | Max Value: 10 157 | Min Value: -10 158 | Value: true 159 | Axis: Z 160 | Channel Name: intensity 161 | Class: rviz_default_plugins/DepthCloud 162 | Color: 255; 255; 255 163 | Color Image Topic: image 164 | Color Transformer: RGB8 165 | Color Transport Hint: raw 166 | Decay Time: 0 167 | Depth Map Topic: depth 168 | Depth Map Transport Hint: raw 169 | Enabled: false 170 | Invert Rainbow: false 171 | Max Color: 255; 255; 255 172 | Max Intensity: 4096 173 | Min Color: 0; 0; 0 174 | Min Intensity: 0 175 | Name: DepthCloud 176 | Occlusion Compensation: 177 | Occlusion Time-Out: 30 178 | Value: false 179 | Position Transformer: XYZ 180 | Queue Size: 5 181 | Selectable: true 182 | Size (Pixels): 3 183 | Style: Flat Squares 184 | Topic Filter: true 185 | Use Fixed Frame: true 186 | Use rainbow: true 187 | Value: false 188 | Enabled: true 189 | Global Options: 190 | Background Color: 48; 48; 48 191 | Fixed Frame: base_link 192 | Frame Rate: 30 193 | Name: root 194 | Tools: 195 | - Class: rviz_default_plugins/Interact 196 | Hide Inactive Objects: true 197 | - Class: rviz_default_plugins/MoveCamera 198 | - Class: rviz_default_plugins/Select 199 | - Class: rviz_default_plugins/FocusCamera 200 | - Class: rviz_default_plugins/Measure 201 | Line color: 128; 128; 0 202 | - Class: rviz_default_plugins/SetInitialPose 203 | Covariance x: 0.25 204 | Covariance y: 0.25 205 | Covariance yaw: 0.06853891909122467 206 | Topic: 207 | Depth: 5 208 | Durability Policy: Volatile 209 | History Policy: Keep Last 210 | Reliability Policy: Reliable 211 | Value: /initialpose 212 | - Class: rviz_default_plugins/SetGoal 213 | Topic: 214 | Depth: 5 215 | Durability Policy: Volatile 216 | History Policy: Keep Last 217 | Reliability Policy: Reliable 218 | Value: /goal_pose 219 | - Class: rviz_default_plugins/PublishPoint 220 | Single click: true 221 | Topic: 222 | Depth: 5 223 | Durability Policy: Volatile 224 | History Policy: Keep Last 225 | Reliability Policy: Reliable 226 | Value: /clicked_point 227 | Transformation: 228 | Current: 229 | Class: rviz_default_plugins/TF 230 | Value: true 231 | Views: 232 | Current: 233 | Class: rviz_default_plugins/Orbit 234 | Distance: 12.790231704711914 235 | Enable Stereo Rendering: 236 | Stereo Eye Separation: 0.05999999865889549 237 | Stereo Focal Distance: 1 238 | Swap Stereo Eyes: false 239 | Value: false 240 | Focal Point: 241 | X: 0 242 | Y: 0 243 | Z: 0 244 | Focal Shape Fixed Size: true 245 | Focal Shape Size: 0.05000000074505806 246 | Invert Z Axis: false 247 | Name: Current View 248 | Near Clip Distance: 0.009999999776482582 249 | Pitch: 0.785398006439209 250 | Target Frame: 251 | Value: Orbit (rviz) 252 | Yaw: 0.785398006439209 253 | Saved: ~ 254 | Window Geometry: 255 | Camera: 256 | collapsed: false 257 | Displays: 258 | collapsed: false 259 | Height: 1021 260 | Hide Left Dock: false 261 | Hide Right Dock: false 262 | Image: 263 | collapsed: false 264 | QMainWindow State: 000000ff00000000fd0000000400000000000001b800000361fc020000000bfb0000001200530065006c0065006300740069006f006e00000000410000007d0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000026a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000022a000000bd0000002800fffffffb0000000a0049006d006100670065010000022a000000bd0000000000000000fb0000000c00430061006d00650072006101000002ab000000f10000002800ffffff000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002a6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e900000040fc0100000002fb0000000800540069006d00650100000000000004e90000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000032b0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 265 | Selection: 266 | collapsed: false 267 | Time: 268 | collapsed: false 269 | Tool Properties: 270 | collapsed: false 271 | Views: 272 | collapsed: false 273 | Width: 1257 274 | X: 135 275 | Y: 25 276 | -------------------------------------------------------------------------------- /config/teleop_twist_joy/f710.config.yaml: -------------------------------------------------------------------------------- 1 | # /robot_0/teleop_twist_joy_node: 2 | /**: # over all namespaces 3 | ros__parameters: 4 | axis_linear: 5 | x: 1 6 | scale_linear: 7 | x: 0.7 8 | scale_linear_turbo: 9 | x: 1.5 10 | 11 | axis_angular: 12 | yaw: 3 13 | scale_angular: 14 | yaw: 0.4 15 | 16 | enable_button: 5 # RB 17 | enable_turbo_button: 4 # LB -------------------------------------------------------------------------------- /include/stage_ros2/stage_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef STAGE_ROS2_PKG__STAGE_ROS_HPP_ 2 | #define STAGE_ROS2_PKG__STAGE_ROS_HPP_ 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // roscpp 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // libstage 29 | #include 30 | 31 | #include "stage_ros2/visibility.h" 32 | 33 | 34 | 35 | // Our node 36 | class StageNode : public rclcpp::Node 37 | { 38 | public: 39 | STAGE_ROS2_PACKAGE_PUBLIC StageNode(rclcpp::NodeOptions options); 40 | 41 | private: 42 | // A mutex to lock access to fields that are used in message callbacks 43 | std::mutex msg_lock; 44 | 45 | // a structure representing a robot inthe simulator 46 | class Vehicle 47 | { 48 | public: 49 | class Ranger 50 | { 51 | bool initialized_; 52 | size_t id_; 53 | Stg::ModelRanger * model; 54 | std::shared_ptr vehicle; 55 | std::string topic_name; 56 | std::string frame_base; 57 | std::string frame_id; 58 | geometry_msgs::msg::TransformStamped::SharedPtr transform; 59 | rclcpp::Publisher::SharedPtr pub; 60 | sensor_msgs::msg::LaserScan::SharedPtr msg; 61 | bool prepare_msg(); 62 | bool prepare_tf(); 63 | 64 | public: 65 | Ranger( 66 | unsigned int id, Stg::ModelRanger * m, std::shared_ptr & vehicle); 67 | void init(bool add_id_to_topic); 68 | unsigned int id() const; 69 | void publish_msg(); 70 | void publish_tf(); 71 | }; 72 | class Camera 73 | { 74 | bool initialized_; 75 | size_t id_; 76 | Stg::ModelCamera * model; 77 | std::shared_ptr vehicle; 78 | geometry_msgs::msg::TransformStamped::SharedPtr transform; 79 | rclcpp::Publisher::SharedPtr pub_image; // multiple images 80 | sensor_msgs::msg::Image::SharedPtr msg_image; 81 | rclcpp::Publisher::SharedPtr pub_depth; // multiple depths 82 | sensor_msgs::msg::Image::SharedPtr msg_depth; 83 | rclcpp::Publisher::SharedPtr pub_camera; // multiple cameras 84 | sensor_msgs::msg::CameraInfo::SharedPtr msg_camera; 85 | bool prepare_msg(); 86 | bool prepare_msg_image(); 87 | bool prepare_msg_depth(); 88 | bool prepare_msg_camera(); 89 | bool prepare_tf(); 90 | 91 | public: 92 | Camera( 93 | unsigned int id, Stg::ModelCamera * m, std::shared_ptr & vehicle); 94 | void init(bool add_id_to_topic); 95 | unsigned int id() const; 96 | void publish_msg(); 97 | void publish_tf(); 98 | std::string topic_name_image; 99 | std::string topic_name_depth; 100 | std::string topic_name_camera_info; 101 | std::string frame_id; 102 | }; 103 | 104 | private: 105 | bool initialized_; 106 | size_t id_; 107 | Stg::Pose initial_pose_; 108 | std::string name_; /// used for the ros publisher 109 | StageNode * node_; 110 | Stg::World * world_; 111 | rclcpp::Time time_last_cmd_received_; 112 | rclcpp::Time timeout_cmd_; /// if no command is received befor the vehicle is stopped 113 | // Last time we saved global position (for velocity calculation). 114 | rclcpp::Time time_last_pose_update_; 115 | 116 | std::string topic_name_space_; 117 | std::string frame_name_space_; 118 | std::string topic_name_cmd_; 119 | 120 | std::string topic_name_tf_; 121 | std::string topic_name_tf_static_; 122 | std::string topic_name_odom_; 123 | std::string topic_name_ground_truth_; 124 | std::string frame_id_odom_; 125 | std::string frame_id_world_; 126 | std::string frame_id_base_link_; 127 | nav_msgs::msg::Odometry msg_odom_; 128 | std::shared_ptr global_pose_; 129 | 130 | public: 131 | Vehicle(size_t id, const Stg::Pose & pose, const std::string & name, StageNode * node); 132 | 133 | void soft_reset(); 134 | size_t id() const; 135 | const std::string & name() const; 136 | const std::string & name_space() const; 137 | void init(bool use_topic_prefixes, bool use_one_tf_tree); 138 | void callback_cmd(const geometry_msgs::msg::Twist::SharedPtr msg); 139 | void callback_cmd_stamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg); 140 | void publish_msg(); 141 | void publish_tf(); 142 | void check_watchdog_timeout(); 143 | StageNode *node(){ 144 | return node_; 145 | } 146 | 147 | // stage related models 148 | Stg::ModelPosition * positionmodel; // one position 149 | std::vector> rangers_; // multiple rangers per position 150 | std::vector> cameras_; // multiple cameras per position 151 | 152 | // ros publishers 153 | rclcpp::Publisher::SharedPtr pub_odom_; // one odom 154 | rclcpp::Publisher::SharedPtr pub_ground_truth_; // one ground truth 155 | rclcpp::Subscription::SharedPtr sub_cmd_; // one cmd_vel subscriber 156 | rclcpp::Subscription::SharedPtr sub_cmd_stamped_; // one sub_cmd_stamped_ subscriber 157 | 158 | std::shared_ptr tf_static_broadcaster_; 159 | std::shared_ptr tf_broadcaster_; 160 | }; 161 | 162 | /// vector to hold the simulated vehicles with ros interfaces 163 | std::vector> vehicles_; 164 | 165 | 166 | bool isDepthCanonical_; /// ROS parameter 167 | bool enforce_prefixes_; /// ROS parameter 168 | bool one_tf_tree_; /// ROS parameter 169 | bool enable_gui_; /// ROS parameter 170 | bool use_stamped_velocity_; /// ROS parameter 171 | bool publish_ground_truth_; /// ROS parameter 172 | bool use_static_transformations_; /// ROS parameter 173 | std::string world_file_; /// ROS parameter 174 | std::string frame_id_odom_name_; /// ROS parameter 175 | std::string frame_id_world_name_; /// ROS parameter 176 | std::string frame_id_base_link_name_; /// ROS parameter 177 | 178 | // TF broadcaster to publish the robot odom 179 | std::shared_ptr tf_broadcaster_stage_; 180 | 181 | // Service to listening on soft reset signals 182 | rclcpp::Service::SharedPtr srv_reset_; 183 | 184 | // publisher for the simulated clock 185 | rclcpp::Publisher::SharedPtr clock_pub_; 186 | 187 | /// called only ones to init the models and to crate for each model a link to ROS 188 | static int callback_init_stage_model(Stg::Model * mod, StageNode * node); 189 | 190 | /// called on every simulation interation 191 | static int callback_update_stage_world(Stg::World * world, StageNode * node); 192 | 193 | public: 194 | ~StageNode(); 195 | // Constructor 196 | void init(int argc, char ** argv); 197 | 198 | // declares ros parameters 199 | void declare_parameters(); 200 | 201 | // int ros parameters for the startup 202 | void update_parameters(); 203 | 204 | // callback to check changes on the parameters 205 | void callback_update_parameters(); 206 | 207 | // timer to check regulary for parameter changes 208 | rclcpp::TimerBase::SharedPtr timer_update_parameter_; 209 | 210 | // Subscribe to models of interest. Currently, we find and subscribe 211 | // to the first 'laser' model and the first 'position' model. Returns 212 | // 0 on success (both models subscribed), -1 otherwise. 213 | int SubscribeModels(); 214 | 215 | // Do one update of the world. May pause if the next update time 216 | // has not yet arrived. 217 | bool UpdateWorld(); 218 | 219 | // Service callback for soft reset 220 | bool cb_reset_srv(const std_srvs::srv::Empty::Request::SharedPtr, 221 | std_srvs::srv::Empty::Response::SharedPtr); 222 | 223 | // The main simulator object 224 | Stg::World * world; 225 | 226 | rclcpp::Duration base_watchdog_timeout_; 227 | 228 | // Current simulation time 229 | rclcpp::Time sim_time_; 230 | 231 | private: 232 | static geometry_msgs::msg::TransformStamped create_transform_stamped( 233 | const tf2::Transform & in, 234 | const rclcpp::Time & timestamp, const std::string & frame_id, 235 | const std::string & child_frame_id); 236 | static geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw); 237 | 238 | }; 239 | 240 | #endif // STAGE_ROS2_PKG__STAGE_ROS_HPP_ 241 | -------------------------------------------------------------------------------- /include/stage_ros2/static_transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. 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 OWNER 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 | 30 | 31 | /** \author Tully Foote */ 32 | /** \author Markus Bader (2023) */ 33 | 34 | #ifndef STAGE_ROS2_PKG__CUSTOM_TF_BROADCASTER_HPP_ 35 | #define STAGE_ROS2_PKG__CUSTOM_TF_BROADCASTER_HPP_ 36 | 37 | #include 38 | 39 | namespace stage_ros2 40 | { 41 | 42 | class StaticTransformBroadcaster 43 | { 44 | public: 45 | /** \brief Node interface constructor */ 46 | template > 47 | StaticTransformBroadcaster( 48 | NodeT &&node, 49 | const char *topic = "/tf_static", 50 | const rclcpp::QoS &qos = tf2_ros::StaticBroadcasterQoS(), 51 | const rclcpp::PublisherOptionsWithAllocator &options = []() 52 | { 53 | rclcpp::PublisherOptionsWithAllocator options; 54 | options.qos_overriding_options = rclcpp::QosOverridingOptions{ 55 | rclcpp::QosPolicyKind::Depth, 56 | rclcpp::QosPolicyKind::History, 57 | rclcpp::QosPolicyKind::Reliability}; 58 | /* 59 | This flag disables intra-process communication while publishing to 60 | /tf_static topic, when the StaticTransformBroadcaster is constructed 61 | using an existing node handle which happens to be a component 62 | (in rclcpp terminology). 63 | Required until rclcpp intra-process communication supports 64 | transient_local QoS durability. 65 | */ 66 | options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; 67 | return options; }()) 68 | { 69 | publisher_ = rclcpp::create_publisher( 70 | node, topic, qos, options); 71 | } 72 | 73 | /** \brief Send a TransformStamped message 74 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 75 | TF2_ROS_PUBLIC 76 | void sendTransform(const geometry_msgs::msg::TransformStamped &msgtf) 77 | { 78 | std::vector v1; 79 | v1.push_back(msgtf); 80 | sendTransform(v1); 81 | } 82 | 83 | /** \brief Send a vector of TransformStamped messages 84 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 85 | TF2_ROS_PUBLIC 86 | void sendTransform(const std::vector &msgtf) 87 | { 88 | for (auto it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) 89 | { 90 | bool match_found = false; 91 | for (auto it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); 92 | ++it_msg) 93 | { 94 | if (it_in->child_frame_id == it_msg->child_frame_id) 95 | { 96 | *it_msg = *it_in; 97 | match_found = true; 98 | break; 99 | } 100 | } 101 | if (!match_found) 102 | { 103 | net_message_.transforms.push_back(*it_in); 104 | } 105 | } 106 | 107 | publisher_->publish(net_message_); 108 | } 109 | 110 | private: 111 | /// Internal reference to ros::Node 112 | rclcpp::Publisher::SharedPtr publisher_; 113 | tf2_msgs::msg::TFMessage net_message_; 114 | }; 115 | 116 | } // namespace stage_ros2 117 | #endif // STAGE_ROS2_PKG__CUSTOM_TF_BROADCASTER_HPP_ -------------------------------------------------------------------------------- /include/stage_ros2/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. 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 OWNER 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 | 30 | 31 | /** \author Tully Foote */ 32 | /** \author Markus Bader (2023) */ 33 | 34 | #ifndef STAGE_ROS2__TRANSFORM_BROADCASTER_H_ 35 | #define STAGE_ROS2__TRANSFORM_BROADCASTER_H_ 36 | 37 | #include 38 | 39 | namespace stage_ros2 40 | { 41 | 42 | /** \brief This class provides an easy way to publish coordinate frame transform information. 43 | * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the 44 | * necessary data needed for each message. */ 45 | 46 | class TransformBroadcaster 47 | { 48 | public: 49 | /** \brief Node constructor */ 50 | template> 51 | TransformBroadcaster( 52 | NodeT && node, 53 | const char *topic = "/tf", 54 | const rclcpp::QoS & qos = tf2_ros::DynamicBroadcasterQoS(), 55 | const rclcpp::PublisherOptionsWithAllocator & options = [] () { 56 | rclcpp::PublisherOptionsWithAllocator options; 57 | options.qos_overriding_options = rclcpp::QosOverridingOptions{ 58 | rclcpp::QosPolicyKind::Depth, 59 | rclcpp::QosPolicyKind::Durability, 60 | rclcpp::QosPolicyKind::History, 61 | rclcpp::QosPolicyKind::Reliability}; 62 | return options; 63 | } ()) 64 | : TransformBroadcaster( 65 | rclcpp::node_interfaces::get_node_parameters_interface(node), 66 | rclcpp::node_interfaces::get_node_topics_interface(node), 67 | qos, 68 | options, 69 | topic) 70 | {} 71 | 72 | /** \brief Node interfaces constructor */ 73 | template> 74 | TransformBroadcaster( 75 | rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, 76 | rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, 77 | const rclcpp::QoS & qos = tf2_ros::DynamicBroadcasterQoS(), 78 | const rclcpp::PublisherOptionsWithAllocator & options = [] () { 79 | rclcpp::PublisherOptionsWithAllocator options; 80 | options.qos_overriding_options = rclcpp::QosOverridingOptions{ 81 | rclcpp::QosPolicyKind::Depth, 82 | rclcpp::QosPolicyKind::Durability, 83 | rclcpp::QosPolicyKind::History, 84 | rclcpp::QosPolicyKind::Reliability}; 85 | return options; 86 | } (), 87 | const char *topic = "/tf") 88 | { 89 | publisher_ = rclcpp::create_publisher( 90 | node_parameters, node_topics, topic, qos, options); 91 | } 92 | 93 | /** \brief Send a TransformStamped message 94 | * 95 | * The transform ʰTₐ added is from `child_frame_id`, `a` to `header.frame_id`, 96 | * `h`. That is, position in `child_frame_id` ᵃp can be transformed to 97 | * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . 98 | * 99 | */ 100 | TF2_ROS_PUBLIC 101 | void sendTransform(const geometry_msgs::msg::TransformStamped & msgtf) 102 | { 103 | std::vector v1; 104 | v1.push_back(msgtf); 105 | sendTransform(v1); 106 | } 107 | 108 | /** \brief Send a vector of TransformStamped messages 109 | * 110 | * The transforms ʰTₐ added are from `child_frame_id`, `a` to `header.frame_id`, 111 | * `h`. That is, position in `child_frame_id` ᵃp can be transformed to 112 | * position in `header.frame_id` ʰp such that ʰp = ʰTₐ ᵃp . 113 | */ 114 | TF2_ROS_PUBLIC 115 | void sendTransform(const std::vector & msgtf) 116 | { 117 | tf2_msgs::msg::TFMessage message; 118 | for (std::vector::const_iterator it = msgtf.begin(); 119 | it != msgtf.end(); ++it) 120 | { 121 | message.transforms.push_back(*it); 122 | } 123 | publisher_->publish(message); 124 | } 125 | 126 | 127 | private: 128 | rclcpp::Publisher::SharedPtr publisher_; 129 | }; 130 | 131 | } // namespace stage_ros2 132 | 133 | #endif // STAGE_ROS2__TRANSFORM_BROADCASTER_H_ -------------------------------------------------------------------------------- /include/stage_ros2/visibility.h: -------------------------------------------------------------------------------- 1 | #ifndef STAGE_ROS2_PKG__VISIBILITY_H_ 2 | #define STAGE_ROS2_PKG__VISIBILITY_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 10 | // https://gcc.gnu.org/wiki/Visibility 11 | 12 | #if defined _WIN32 || defined __CYGWIN__ 13 | 14 | #ifdef __GNUC__ 15 | #define STAGE_ROS2_PACKAGE_EXPORT __attribute__ ((dllexport)) 16 | #define STAGE_ROS2_PACKAGE_IMPORT __attribute__ ((dllimport)) 17 | #else 18 | #define STAGE_ROS2_PACKAGE_EXPORT __declspec(dllexport) 19 | #define STAGE_ROS2_PACKAGE_IMPORT __declspec(dllimport) 20 | #endif 21 | 22 | #ifdef STAGE_ROS2_PACKAGE_DLL 23 | #define STAGE_ROS2_PACKAGE_PUBLIC STAGE_ROS2_PACKAGE_EXPORT 24 | #else 25 | #define STAGE_ROS2_PACKAGE_PUBLIC STAGE_ROS2_PACKAGE_IMPORT 26 | #endif 27 | 28 | #define STAGE_ROS2_PACKAGE_PUBLIC_TYPE STAGE_ROS2_PACKAGE_PUBLIC 29 | 30 | #define STAGE_ROS2_PACKAGE_LOCAL 31 | 32 | #else 33 | 34 | #define STAGE_ROS2_PACKAGE_EXPORT __attribute__ ((visibility("default"))) 35 | #define STAGE_ROS2_PACKAGE_IMPORT 36 | 37 | #if __GNUC__ >= 4 38 | #define STAGE_ROS2_PACKAGE_PUBLIC __attribute__ ((visibility("default"))) 39 | #define STAGE_ROS2_PACKAGE_LOCAL __attribute__ ((visibility("hidden"))) 40 | #else 41 | #define STAGE_ROS2_PACKAGE_PUBLIC 42 | #define STAGE_ROS2_PACKAGE_LOCAL 43 | #endif 44 | 45 | #define STAGE_ROS2_PACKAGE_PUBLIC_TYPE 46 | #endif 47 | 48 | #ifdef __cplusplus 49 | } 50 | #endif 51 | 52 | #endif // STAGE_ROS2_PKG__VISIBILITY_H_ 53 | -------------------------------------------------------------------------------- /launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.substitutions import LaunchConfiguration 8 | from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument 9 | from launch.conditions import IfCondition 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | 12 | 13 | def generate_launch_description(): 14 | 15 | use_sim_time = LaunchConfiguration('use_sim_time', default='true') 16 | this_directory = get_package_share_directory('stage_ros2') 17 | launch_dir = os.path.join(this_directory, 'launch') 18 | 19 | enforce_prefixes = LaunchConfiguration('enforce_prefixes') 20 | enforce_prefixes_cmd = DeclareLaunchArgument( 21 | 'enforce_prefixes', 22 | default_value='false', 23 | description='on true a prefixes are used for a single robot environment') 24 | 25 | use_stamped_velocity = LaunchConfiguration('use_stamped_velocity') 26 | use_stamped_velocity_cmd = DeclareLaunchArgument( 27 | 'use_stamped_velocity', 28 | default_value='false', 29 | description='on true stage will accept TwistStamped command messages') 30 | 31 | one_tf_tree = LaunchConfiguration('one_tf_tree') 32 | one_tf_tree_cmd = DeclareLaunchArgument( 33 | 'one_tf_tree', 34 | default_value='false', 35 | description='on true all tfs are published with a namespace on /tf and /tf_static') 36 | 37 | namespace = LaunchConfiguration('namespace') 38 | declare_namespace_cmd = DeclareLaunchArgument( 39 | 'namespace', 40 | default_value='', 41 | description='Top-level namespace') 42 | 43 | stage = LaunchConfiguration('stage') 44 | declare_stage_cmd = DeclareLaunchArgument( 45 | 'stage', 46 | default_value='True', 47 | description='Whether run a stage') 48 | 49 | rviz = LaunchConfiguration('rviz') 50 | declare_rviz_cmd = DeclareLaunchArgument( 51 | 'rviz', 52 | default_value='True', 53 | description='Whether run a rviz') 54 | 55 | world = LaunchConfiguration('world') 56 | declare_world = DeclareLaunchArgument( 57 | 'world', default_value='cave_three_robots', 58 | description='world to load in stage and rviz config [cave, example]') 59 | 60 | return LaunchDescription([ 61 | declare_namespace_cmd, 62 | use_stamped_velocity_cmd, 63 | declare_rviz_cmd, 64 | declare_stage_cmd, 65 | enforce_prefixes_cmd, 66 | one_tf_tree_cmd, 67 | declare_world, 68 | IncludeLaunchDescription( 69 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz.launch.py')), 70 | condition=IfCondition(rviz), 71 | launch_arguments={'namespace': namespace, 72 | 'use_sim_time': use_sim_time, 73 | 'config': world}.items()), 74 | IncludeLaunchDescription( 75 | PythonLaunchDescriptionSource(os.path.join(launch_dir, 'stage.launch.py')), 76 | condition=IfCondition(stage), 77 | launch_arguments={'one_tf_tree':one_tf_tree, 78 | 'enforce_prefixes':enforce_prefixes, 79 | 'use_stamped_velocity': use_stamped_velocity, 80 | 'world': world}.items()), 81 | ]) 82 | -------------------------------------------------------------------------------- /launch/f710.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.substitutions import LaunchConfiguration, TextSubstitution 6 | from launch.actions import DeclareLaunchArgument 7 | from launch_ros.actions import Node 8 | 9 | 10 | def generate_launch_description(): 11 | this_directory = get_package_share_directory('stage_ros2') 12 | joy_config = LaunchConfiguration('joy_config') 13 | joy_dev = LaunchConfiguration('joy_dev') 14 | config_filepath = LaunchConfiguration('config_filepath') 15 | namespace_arg = DeclareLaunchArgument( 16 | 'namespace', 17 | default_value=TextSubstitution(text='robot_0')) 18 | 19 | return LaunchDescription([ 20 | namespace_arg, 21 | DeclareLaunchArgument('joy_config', default_value='f710'), 22 | DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0'), 23 | DeclareLaunchArgument('config_filepath', default_value=[ 24 | TextSubstitution(text=os.path.join(this_directory, 'config', 'teleop_twist_joy', '')), 25 | joy_config, TextSubstitution(text='.config.yaml')]), 26 | 27 | Node( 28 | namespace=LaunchConfiguration('namespace'), 29 | package='joy', executable='joy_node', name='joy_node', 30 | parameters=[{ 31 | 'dev': joy_dev, 32 | 'deadzone': 0.3, 33 | 'autorepeat_rate': 20.0, 34 | }]), 35 | Node( 36 | namespace=LaunchConfiguration('namespace'), 37 | package='teleop_twist_joy', executable='teleop_node', 38 | name='teleop_twist_joy_node', parameters=[config_filepath]), 39 | ]) 40 | -------------------------------------------------------------------------------- /launch/rviz.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.substitutions import LaunchConfiguration, TextSubstitution 8 | from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | use_sim_time = LaunchConfiguration('use_sim_time', default='true') 15 | this_directory = get_package_share_directory('stage_ros2') 16 | 17 | namespace = LaunchConfiguration('namespace') 18 | namespace_arg = DeclareLaunchArgument('namespace', default_value=TextSubstitution(text='')) 19 | 20 | rviz_config = LaunchConfiguration('config') 21 | rviz_config_arg = DeclareLaunchArgument( 22 | 'config', 23 | default_value=TextSubstitution(text='empty'), 24 | description='Use empty, cave or roblab to load a TUW enviroment') 25 | 26 | def rviz_launch_configuration(context): 27 | file = os.path.join( 28 | this_directory, 29 | 'config/rviz', 30 | context.launch_configurations['config'] + '.rviz') 31 | return [SetLaunchConfiguration('config', file)] 32 | 33 | rviz_launch_configuration_arg = OpaqueFunction(function=rviz_launch_configuration) 34 | 35 | return LaunchDescription([ 36 | namespace_arg, 37 | rviz_config_arg, 38 | rviz_launch_configuration_arg, 39 | Node( 40 | package='rviz2', 41 | namespace=namespace, 42 | executable='rviz2', 43 | name='rviz2', 44 | arguments=['-d', [rviz_config]], 45 | parameters=[{ 46 | "use_sim_time": use_sim_time}], 47 | ) 48 | ]) 49 | -------------------------------------------------------------------------------- /launch/rviz_ns.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2018 Intel Corporation 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 DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration 21 | from launch.conditions import IfCondition, UnlessCondition 22 | from launch.substitutions import LaunchConfiguration, TextSubstitution 23 | from launch_ros.actions import Node 24 | 25 | 26 | def generate_launch_description(): 27 | # Get the launch directory 28 | this_directory = get_package_share_directory('stage_ros2') 29 | 30 | # Create the launch configuration variables 31 | 32 | # Declare the launch arguments 33 | namespace = LaunchConfiguration('namespace') 34 | declare_namespace_cmd = DeclareLaunchArgument( 35 | 'namespace', 36 | default_value='robot_0', 37 | description=('Top-level namespace. The value will be used to replace the ' 38 | ' keyword on the rviz config file.')) 39 | 40 | one_tf_tree = LaunchConfiguration('one_tf_tree') 41 | one_tf_tree_cmd = DeclareLaunchArgument( 42 | 'one_tf_tree', 43 | default_value='false', 44 | description='on true all tfs are published with a namespace on /tf and /tf_static') 45 | 46 | use_sim_time = LaunchConfiguration('use_sim_time') 47 | declare_use_sim_time_cmd = DeclareLaunchArgument( 48 | 'use_sim_time', 49 | default_value='true', 50 | description='Use simulation (Gazebo/Stage) clock if true') 51 | 52 | rviz_config = LaunchConfiguration('config') 53 | rviz_config_arg = DeclareLaunchArgument( 54 | 'config', 55 | default_value=TextSubstitution(text='empty'), 56 | description='Use empty, cave or roblab to load a TUW enviroment') 57 | 58 | def rviz_launch_configuration(context): 59 | file = os.path.join( 60 | this_directory, 61 | 'config/rviz', 62 | context.launch_configurations['config'] + '.rviz') 63 | return [SetLaunchConfiguration('config', file)] 64 | 65 | rviz_launch_configuration_arg = OpaqueFunction(function=rviz_launch_configuration) 66 | 67 | # Launch rviz 68 | start_rviz_one_tf_tree_cmd = Node( 69 | condition=IfCondition(one_tf_tree), 70 | package='rviz2', 71 | executable='rviz2', 72 | namespace=namespace, 73 | arguments=['-d', rviz_config], 74 | output='screen', 75 | parameters=[{ 76 | "use_sim_time": use_sim_time}]) 77 | 78 | start_rviz_multiple_tf_trees_cmd = Node( 79 | condition=UnlessCondition(one_tf_tree), 80 | package='rviz2', 81 | executable='rviz2', 82 | namespace=namespace, 83 | arguments=['-d', rviz_config, '-t', '{NAMESPACE} - {CONFIG_PATH}/{CONFIG_FILENAME} - RViz2'], 84 | output='screen', 85 | remappings=[('/tf', 'tf'), 86 | ('/tf_static', 'tf_static'), 87 | ('/camera_info', 'camera_info'), 88 | ('/goal_pose', 'goal_pose'), 89 | ('/clicked_point', 'clicked_point'), 90 | ('/initialpose', 'initialpose')], 91 | parameters=[{ 92 | "use_sim_time": use_sim_time}]) 93 | 94 | # Create the launch description and populate 95 | ld = LaunchDescription() 96 | 97 | # Declare the launch options 98 | ld.add_action(declare_use_sim_time_cmd) 99 | ld.add_action(declare_namespace_cmd) 100 | ld.add_action(one_tf_tree_cmd) 101 | ld.add_action(rviz_config_arg) 102 | ld.add_action(rviz_launch_configuration_arg) 103 | 104 | # Add any conditioned actions 105 | ld.add_action(start_rviz_one_tf_tree_cmd) 106 | ld.add_action(start_rviz_multiple_tf_trees_cmd) 107 | 108 | return ld 109 | -------------------------------------------------------------------------------- /launch/stage.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.substitutions import LaunchConfiguration, TextSubstitution 8 | from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | this_directory = get_package_share_directory('stage_ros2') 15 | 16 | use_stamped_velocity = LaunchConfiguration('use_stamped_velocity') 17 | use_stamped_velocity_arg = DeclareLaunchArgument( 18 | 'use_stamped_velocity', 19 | default_value='false', 20 | description='on true stage will accept TwistStamped command messages') 21 | 22 | stage_world_arg = DeclareLaunchArgument( 23 | 'world', 24 | default_value=TextSubstitution(text='cave'), 25 | description='World file relative to the project world file, without .world') 26 | 27 | 28 | enforce_prefixes = LaunchConfiguration('enforce_prefixes') 29 | enforce_prefixes_arg = DeclareLaunchArgument( 30 | 'enforce_prefixes', 31 | default_value='false', 32 | description='on true a prefixes are used for a single robot environment') 33 | 34 | use_static_transformations = LaunchConfiguration('use_static_transformations') 35 | use_static_transformations_arg = DeclareLaunchArgument( 36 | 'use_static_transformations', 37 | default_value='true', 38 | description='Use static transformations for sensor frames!') 39 | 40 | one_tf_tree = LaunchConfiguration('one_tf_tree') 41 | one_tf_tree_arg = DeclareLaunchArgument( 42 | 'one_tf_tree', 43 | default_value='false', 44 | description='on true all tfs are published with a namespace on /tf and /tf_static') 45 | 46 | def stage_world_configuration(context): 47 | file = os.path.join( 48 | this_directory, 49 | 'world', 50 | context.launch_configurations['world'] + '.world') 51 | return [SetLaunchConfiguration('world_file', file)] 52 | 53 | stage_world_configuration_arg = OpaqueFunction(function=stage_world_configuration) 54 | 55 | return LaunchDescription([ 56 | use_stamped_velocity_arg, 57 | stage_world_arg, 58 | one_tf_tree_arg, 59 | enforce_prefixes_arg, 60 | use_static_transformations_arg, 61 | stage_world_configuration_arg, 62 | Node( 63 | package='stage_ros2', 64 | executable='stage_ros2', 65 | name='stage', 66 | parameters=[{'one_tf_tree': one_tf_tree, 67 | 'enforce_prefixes': enforce_prefixes, 68 | 'use_stamped_velocity': use_stamped_velocity, 69 | 'use_static_transformations': use_static_transformations, 70 | "world_file": [LaunchConfiguration('world_file')]}], 71 | ) 72 | ]) 73 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stage_ros2 5 | 0.0.1 6 | ROS2 wrapper for Stage 7 | Markus Bader 8 | BSD 9 | 10 | ament_cmake 11 | 12 | 13 | rclcpp 14 | rclcpp_components 15 | sensor_msgs 16 | geometry_msgs 17 | nav_msgs 18 | std_msgs 19 | stage 20 | tf2_ros 21 | tf2_geometry_msgs 22 | std_srvs 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | ament_cmake_clang_format 27 | ament_cmake_cppcheck 28 | ament_cmake_pycodestyle 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /res/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave.png -------------------------------------------------------------------------------- /res/cave_seven_robots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_seven_robots.png -------------------------------------------------------------------------------- /res/cave_three_robots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots.png -------------------------------------------------------------------------------- /res/cave_three_robots_multiple_tf_trees.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots_multiple_tf_trees.jpg -------------------------------------------------------------------------------- /res/cave_three_robots_multiple_tf_trees_depth.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots_multiple_tf_trees_depth.jpg -------------------------------------------------------------------------------- /res/cave_three_robots_multiple_tf_trees_depth_info.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots_multiple_tf_trees_depth_info.jpg -------------------------------------------------------------------------------- /res/cave_three_robots_one_tf_tree.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots_one_tf_tree.jpg -------------------------------------------------------------------------------- /res/cave_three_robots_with_rviz.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/cave_three_robots_with_rviz.jpg -------------------------------------------------------------------------------- /res/demos.md: -------------------------------------------------------------------------------- 1 | # stage_ros2 Demos 2 | 3 | ## launch files 4 | 5 | ### stage 6 | ``` 7 | ## with /tf 8 | ros2 launch stage_ros2 stage.launch.py world:=cave enforce_prefixes:=false one_tf_tree:=true 9 | ## with /robot_0/tf 10 | ros2 launch stage_ros2 stage.launch.py world:=cave enforce_prefixes:=true one_tf_tree:=false 11 | ``` 12 | ### stage and rviz 13 | demo worlds with different robot configurations and rviz configs with __one tf tree__ 14 | ``` 15 | ros2 launch stage_ros2 demo.launch.py world:=cave 16 | ros2 launch stage_ros2 demo.launch.py world:=lines 17 | ros2 launch stage_ros2 demo.launch.py world:=cave_three_robots 18 | ros2 launch stage_ros2 demo.launch.py world:=cave_seven_robots 19 | ``` 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 |

cave
lines
cave_three_robots
cave_seven_robots
28 | 29 | ### teleop joy 30 | 31 | ``` 32 | sudo apt install ros-jazzy-teleop-twist-joy # install dependencies 33 | ros2 launch stage_ros2 f710.launch.py namespace:='/' # for a single vehicle world like cave.world 34 | ros2 launch stage_ros2 f710.launch.py namespace:='robot_0' # for a world like cave_multi.world or cave_three_robots.world 35 | ``` 36 | 37 | 38 | ## parameters on stage_ros2 39 | ### without parameters 40 | it will start the cave.world 41 | ``` 42 | ros2 run stage_ros2 stage_ros2 43 | ``` 44 | ### with parameters 45 | ``` 46 | cd stage_ros2/world 47 | ros2 run stage_ros2 stage_ros2 --ros-args --ros-args \ 48 | -p world_file:=lines.world \ 49 | -p use_static_transformations:=true \ 50 | -p enforce_prefixes:=true \ 51 | -p one_tf_tree:=false \ 52 | -p use_stamped_velocity:=false 53 | ``` 54 | 55 | ## SoftReset 56 | 57 | ``` 58 | ros2 service call /reset_positions std_srvs/srv/Empty 59 | ``` 60 | 61 | 62 | ## tmuxinator 63 | you can use the tmuxinator configurations to start some demos like 64 | ``` 65 | tmuxinator start -p ./ws01/src/stage_ros2/tmuxinator/cave_three_robots.yml 66 | tmuxinator start -p ./ws01/src/stage_ros2/tmuxinator/cave_one_robot.yml 67 | ``` -------------------------------------------------------------------------------- /res/install.md: -------------------------------------------------------------------------------- 1 | # stage_ros2 Install 2 | ## Install 3 | ``` 4 | cd YOUR_ROS2_WORKSPACE 5 | mkdir src 6 | cd src 7 | git clone git@github.com:tuw-robotics/Stage.git 8 | git clone git@github.com:tuw-robotics/stage_ros2.git 9 | rosdep update 10 | rosdep install --from-paths ./Stage --ignore-src -r -y # install dependencies for Stage 11 | rosdep install --from-paths ./stage_ros2 --ignore-src -r -y # install dependencies for stage_ros2 12 | cd YOUR_ROS2_WORKSPACE 13 | colcon build --symlink-install 14 | ``` -------------------------------------------------------------------------------- /res/lines.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/res/lines.png -------------------------------------------------------------------------------- /res/multi_robot_setup.md: -------------------------------------------------------------------------------- 1 | 2 | # Multi Robot Setup 3 | 4 | Working with multiple vehicles or robots can create a knot in your brain. In order to at least solve my brain knot, I have tried here to document the problem and also possible solutions. 5 | 6 | ## The Problem 7 | 8 | ### One Vehicle 9 | Each vehicle needs to be identified in your system. This leads to some sort of naming conventions. On the other site, one like to use existing software or ROS nodes, but many nodes were designed to work for a single robot systems. For example, you like to use multiple laser scanners, each with a laser filter to cut out scan points. Therefore, you are using a namespace for the published scan like */laser0/scan* and *laser1/scan* and a laser filter within each name space but the TF-tree used for both lasers filters listens to */tf* and not to a namespaces TF-tree. This indicates that a common TF-tree for a vehicle is somehow vital. 10 | 11 | ### Multiple Vehicles 12 | Now let's take a look at multiple vehicles. 13 | #### 1. One TF-tree on /tf 14 | In this case, we are using one TF-tree for multiple vehicles, therefore each described frame in the TF-tree needs to be unique in the multi robot system. This leads to fame names with prefixes.
15 | 16 | __For example:__
17 | We have a *robot_0/scan* topic publishing a LaserScan message with its data associated to the frame_id  *robot_0/laser* and a second robot with a *robot_1/scan* topic publishing a LaserScan message with its data associated to the frame_id __robot_1/laser_link__. 18 | The associated transforms to the frame_ids __robot_0/laser_link__ and __robot_1/laser_link__ are published on */tf*.  19 | In this configuration, we can view all localized robots on a common map with their sensor data in RViz.  20 | On the other site, each robot is listening to all transformations on all robots in the system. Configurations cannot easily be transferred between robots because the used frame_ids with the nodes need to be parametrized with a prefix. Adding a namespace on a node does not add a prefix on the frame_id.  21 | 22 | #### 2. Multiple TF-trees topics (robot_0/tf robot_1/tf) 23 | In this case, we are using for each vehicle a separate TF-tree topic to publish.  Therefore, each described frame in the TF-tree needs to be unique only on this vehicle. This simplifies the parametrization of reused nodes on multiple vehicles.
24 | 25 | __For example:__
26 | We still have a namespace such as robot_0 for our laser scan. But the message LaserScan published on *robot_0/scan* topic is associated to the frame_id __laser_link__. A second robot with a *robot_1/scan* topic publishing a LaserScan message with its data also associated to the frame_id __laser_link__. This is possible because the TF-tree used is different for these vehicles, the first one uses the TF-tree published on */robot_0/tf* and the second on */robot_1/tf*.  27 | In this configuration, we can view only one robot with its sensor data in RViz.  28 | On the other site, it simplifies the configuration overhead because one just needs to use a namespace on nodes and to remap */tf* to a custom topic with a namespace (*/robot_XXX/tf*). 29 | 30 | 31 | ## Demo 32 | ### Demo: one TF-tree on /tf 33 |
34 | stage and rviz with laser, tf, and cameras
35 |
36 | One TF-tree after:
# ros2 launch stage_ros2 demo.launch.py world:=cave_three_robots one_tf_tree:=true 37 |
on Ubuntu 22.04 with ros2 humble 38 |
39 |
40 | 41 | 42 | ### Demo: multiple TF-trees on /robot_0/tf, /robot_1/tf, /robot_2/tf 43 |
44 | stage and rviz with laser, tf, and cameras
45 |
46 | Multiple TF-trees after: 47 |
# ros2 launch stage_ros2 stage.launch.py world:=cave_three_robots one_tf_tree:=false 48 |
# ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns namespace:=robot_0 49 |
# ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns namespace:=robot_1 50 |
# ros2 run mouse_teleop mouse_teleop -r __ns:=/robot_1 --ros-args --remap mouse_vel:=cmd_vel 51 |
on Ubuntu 22.04 with ros2 humble 52 |
53 |
54 | 55 | #### no root slash 56 | It is important to say that there are no root slashes on the subscribed topics in rviz2. Because the rviz config is using a namespace. 57 |
58 | stage and rviz with laser, tf, and cameras
59 |
60 | No root slashes: 61 |
62 |
-------------------------------------------------------------------------------- /src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_IMAGE "image" 8 | #define TOPIC_DEPTH "depth" 9 | #define TOPIC_CAMERA_INFO "camera_info" 10 | #define FRAME_CAMERA "camera" 11 | 12 | using std::placeholders::_1; 13 | 14 | StageNode::Vehicle::Camera::Camera( 15 | unsigned int id, Stg::ModelCamera * m, 16 | std::shared_ptr & v) 17 | : initialized_(false), id_(id), model(m), vehicle(v) {} 18 | 19 | unsigned int StageNode::Vehicle::Camera::id() const 20 | { 21 | return id_; 22 | } 23 | 24 | void StageNode::Vehicle::Camera::init(bool add_id_to_topic) 25 | { 26 | if(initialized_) return; 27 | model->Subscribe(); 28 | topic_name_image = vehicle->topic_name_space_ + TOPIC_IMAGE; 29 | topic_name_camera_info = vehicle->topic_name_space_ + TOPIC_CAMERA_INFO; 30 | topic_name_depth = vehicle->topic_name_space_ + TOPIC_DEPTH; 31 | frame_id = vehicle->frame_name_space_ + FRAME_CAMERA; 32 | 33 | if (add_id_to_topic) { 34 | topic_name_image += std::to_string(id()); 35 | topic_name_camera_info += std::to_string(id()); 36 | topic_name_depth += std::to_string(id()); 37 | frame_id += std::to_string(id()); 38 | } 39 | 40 | pub_image = vehicle->node()->create_publisher(topic_name_image, 10); 41 | pub_camera = vehicle->node()->create_publisher(topic_name_camera_info, 10); 42 | pub_depth = vehicle->node()->create_publisher(topic_name_depth, 10); 43 | initialized_ = true; 44 | } 45 | bool StageNode::Vehicle::Camera::prepare_msg_image() 46 | { 47 | if (msg_image) { 48 | return true; 49 | } 50 | if (!model->FrameColor()) { 51 | return false; 52 | } 53 | msg_image = std::make_shared(); 54 | 55 | msg_image->height = model->getHeight(); 56 | msg_image->width = model->getWidth(); 57 | msg_image->encoding = "rgba8"; 58 | // node->imageMsgs[r].is_bigendian=""; 59 | msg_image->step = msg_image->width * 4; 60 | msg_image->data.resize(msg_image->width * msg_image->height * 4); 61 | msg_image->header.frame_id = frame_id; 62 | 63 | return true; 64 | } 65 | bool StageNode::Vehicle::Camera::prepare_msg_depth() 66 | { 67 | if (msg_depth) { 68 | return true; 69 | } 70 | if (!model->FrameDepth()) { 71 | return false; 72 | } 73 | msg_depth = std::make_shared(); 74 | msg_depth->height = model->getHeight(); 75 | msg_depth->width = model->getWidth(); 76 | msg_depth->encoding = 77 | vehicle->node()->isDepthCanonical_ ? sensor_msgs::image_encodings::TYPE_32FC1 : sensor_msgs:: 78 | image_encodings::TYPE_16UC1; 79 | // node->depthMsgs[r].is_bigendian=""; 80 | int sz = vehicle->node()->isDepthCanonical_ ? sizeof(float) : sizeof(uint16_t); 81 | size_t len = msg_depth->width * msg_depth->height; 82 | msg_depth->step = msg_depth->width * sz; 83 | msg_depth->data.resize(len * sz); 84 | return true; 85 | } 86 | bool StageNode::Vehicle::Camera::prepare_msg_camera() 87 | { 88 | 89 | if (msg_camera) { 90 | return true; 91 | } 92 | if (!(model->FrameColor() || model->FrameDepth())) { 93 | return false; 94 | } 95 | msg_camera = std::make_shared(); 96 | msg_camera->header.frame_id = frame_id; 97 | msg_camera->header.stamp = vehicle->node()->sim_time_; 98 | msg_camera->height = model->getHeight(); 99 | msg_camera->width = model->getWidth(); 100 | 101 | double fx, fy, cx, cy; 102 | cx = msg_camera->width / 2.0; 103 | cy = msg_camera->height / 2.0; 104 | double fovh = model->getCamera().horizFov() * M_PI / 180.0; 105 | double fovv = model->getCamera().vertFov() * M_PI / 180.0; 106 | // double fx_ = 1.43266615300557*node->models[r]->getWidth()/tan(fovh); 107 | // double fy_ = 1.43266615300557*node->models[r]->getHeight()/tan(fovv); 108 | fx = model->getWidth() / (2 * tan(fovh / 2)); 109 | fy = model->getHeight() / (2 * tan(fovv / 2)); 110 | 111 | // ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_); 112 | 113 | msg_camera->d.resize(4, 0.0); 114 | 115 | msg_camera->k[0] = fx; 116 | msg_camera->k[2] = cx; 117 | msg_camera->k[4] = fy; 118 | msg_camera->k[5] = cy; 119 | msg_camera->k[8] = 1.0; 120 | 121 | msg_camera->r[0] = 1.0; 122 | msg_camera->r[4] = 1.0; 123 | msg_camera->r[8] = 1.0; 124 | 125 | msg_camera->p[0] = fx; 126 | msg_camera->p[2] = cx; 127 | msg_camera->p[5] = fy; 128 | msg_camera->p[6] = cy; 129 | msg_camera->p[10] = 1.0; 130 | return true; 131 | } 132 | 133 | bool StageNode::Vehicle::Camera::prepare_msg() 134 | { 135 | if (!prepare_msg_image()) {return false;} 136 | if (!prepare_msg_depth()) {return false;} 137 | return true; 138 | } 139 | 140 | void StageNode::Vehicle::Camera::publish_msg() 141 | { 142 | // Guard 143 | if(!initialized_) return; 144 | 145 | // Translate into ROS message format and publish 146 | if (prepare_msg_image()) { 147 | 148 | memcpy(&(msg_image->data[0]), model->FrameColor(), msg_image->width * msg_image->height * 4); 149 | 150 | // invert the opengl weirdness 151 | char * temp = new char[msg_image->step]; 152 | for (unsigned int y = 0; y < (msg_image->height + 1) / 2; y++) { 153 | memcpy(temp, &msg_image->data[y * msg_image->step], msg_image->step); 154 | memcpy( 155 | &(msg_image->data[y * msg_image->step]), 156 | &(msg_image->data[(msg_image->height - y - 1) * msg_image->step]), msg_image->step); 157 | memcpy( 158 | &(msg_image->data[(msg_image->height - y - 1) * msg_image->step]), temp, 159 | msg_image->step); 160 | } 161 | msg_image->header.stamp = vehicle->node()->sim_time_; 162 | pub_image->publish(*msg_image); 163 | } 164 | 165 | // Translate into ROS message format and publish 166 | if (prepare_msg_depth()) { 167 | // processing data according to REP118 168 | if (vehicle->node()->isDepthCanonical_) { 169 | float nearClip = model->getCamera().nearClip(); 170 | float farClip = model->getCamera().farClip(); 171 | memcpy(&(msg_depth->data[0]), model->FrameDepth(), msg_depth->data.size()); 172 | float * data = (float *)&(msg_depth->data[0]); 173 | size_t len = msg_depth->width * msg_depth->height; 174 | for (size_t i = 0; i < len; ++i) { 175 | if (data[i] <= nearClip) { 176 | data[i] = -INFINITY; 177 | } else if (data[i] >= farClip) { 178 | data[i] = INFINITY; 179 | } 180 | } 181 | } else { 182 | int nearClip = (int)(model->getCamera().nearClip() * 1000); 183 | int farClip = (int)(model->getCamera().farClip() * 1000); 184 | size_t len = msg_depth->width * msg_depth->height; 185 | for (size_t i = 0; i < len; ++i) { 186 | int v = (int)(model->FrameDepth()[i] * 1000); 187 | if (v <= nearClip || v >= farClip) { 188 | v = 0; 189 | } 190 | ((uint16_t *)&(msg_depth->data[0]))[i] = 191 | (uint16_t)((v <= nearClip || v >= farClip) ? 0 : v); 192 | } 193 | } 194 | 195 | // invert the opengl weirdness 196 | char * temp = new char[msg_depth->step]; 197 | for (unsigned int y = 0; y < msg_depth->height / 2; y++) { 198 | memcpy(temp, &msg_depth->data[y * msg_depth->step], msg_depth->step); 199 | memcpy( 200 | &(msg_depth->data[y * msg_depth->step]), 201 | &(msg_depth->data[(msg_depth->height - 1 - y) * msg_depth->step]), msg_depth->step); 202 | memcpy( 203 | &(msg_depth->data[(msg_depth->height - 1 - y) * msg_depth->step]), temp, 204 | msg_depth->step); 205 | } 206 | 207 | msg_depth->header.frame_id = frame_id; 208 | msg_depth->header.stamp = vehicle->node()->sim_time_; 209 | pub_depth->publish(*msg_depth); 210 | } 211 | 212 | // Translate into ROS message format and publish 213 | if (prepare_msg_camera()) { 214 | msg_camera->header.stamp = vehicle->node()->sim_time_; 215 | pub_camera->publish(*msg_camera); 216 | } 217 | } 218 | bool StageNode::Vehicle::Camera::prepare_tf() 219 | { 220 | if (transform) {return true;} 221 | 222 | if (!(model->FrameColor() || model->FrameDepth())) {return false;} 223 | transform = std::make_shared(); 224 | 225 | Stg::Pose pose = model->GetPose(); 226 | tf2::Quaternion quternion; 227 | quternion.setRPY( 228 | (model->getCamera().pitch() * M_PI / 180.0) - M_PI, 229 | 0.0, 230 | pose.a + (model->getCamera().yaw() * M_PI / 180.0) - vehicle->positionmodel->GetPose().a); 231 | tf2::Transform tr = 232 | tf2::Transform( 233 | quternion, 234 | tf2::Vector3(pose.x, pose.y, vehicle->positionmodel->GetGeom().size.z + pose.z)); 235 | *transform = 236 | create_transform_stamped(tr, vehicle->node()->sim_time_, vehicle->frame_id_base_link_, frame_id); 237 | if (vehicle->node()->use_static_transformations_) { 238 | vehicle->tf_static_broadcaster_->sendTransform(*transform); 239 | } 240 | return true; 241 | } 242 | void StageNode::Vehicle::Camera::publish_tf() 243 | { 244 | if (prepare_tf()) { 245 | 246 | if (vehicle->node()->use_static_transformations_) {return;} 247 | 248 | // use tf publsiher only if use_static_transformations_ is false 249 | transform->header.stamp = vehicle->node()->sim_time_; 250 | vehicle->tf_broadcaster_->sendTransform(*transform); 251 | } 252 | } 253 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "stage_ros2/stage_node.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | 5 | int main(int argc, char ** argv) 6 | { 7 | rclcpp::init(argc, argv); 8 | auto node = std::make_shared(rclcpp::NodeOptions()); 9 | node->init(argc - 1, argv); 10 | if (node->SubscribeModels() != 0) {exit(-1);} 11 | std::thread t = std::thread([&node]() {rclcpp::spin(node);}); 12 | node->world->Start(); 13 | Stg::World::Run(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /src/ranger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_LASER "base_scan" 8 | #define FRAME_LASER "laser" 9 | 10 | using std::placeholders::_1; 11 | 12 | StageNode::Vehicle::Ranger::Ranger( 13 | unsigned int id, Stg::ModelRanger * m, 14 | std::shared_ptr & v) 15 | : initialized_(false), id_(id), model(m), vehicle(v) {} 16 | 17 | unsigned int StageNode::Vehicle::Ranger::id() const 18 | { 19 | return id_; 20 | } 21 | void StageNode::Vehicle::Ranger::init(bool add_id_to_topic) 22 | { 23 | if(initialized_) return; 24 | model->Subscribe(); 25 | topic_name = vehicle->topic_name_space_ + TOPIC_LASER; 26 | frame_id = vehicle->frame_name_space_ + FRAME_LASER; 27 | if (add_id_to_topic) { 28 | topic_name += std::to_string(id()); 29 | frame_id += std::to_string(id()); 30 | } 31 | 32 | pub = vehicle->node()->create_publisher(topic_name, 10); 33 | initialized_ = true; 34 | } 35 | 36 | bool StageNode::Vehicle::Ranger::prepare_msg() 37 | { 38 | if (msg) {return true;} 39 | if (model->GetSensors().size() > 1) { 40 | RCLCPP_WARN(vehicle->node()->get_logger(), "ROS Stage currently supports rangers with 1 sensor only."); 41 | } 42 | const Stg::ModelRanger::Sensor & sensor = model->GetSensors()[0]; // we use the first sensor data 43 | if (sensor.ranges.size() == 0) {return false;} 44 | 45 | msg = std::make_shared(); 46 | msg->angle_min = -sensor.fov / 2.0; 47 | msg->angle_max = +sensor.fov / 2.0; 48 | msg->angle_increment = sensor.fov / (double)(sensor.sample_count - 1); 49 | msg->range_min = sensor.range.min; 50 | msg->range_max = sensor.range.max; 51 | msg->ranges.resize(sensor.ranges.size()); 52 | msg->intensities.resize(sensor.intensities.size()); 53 | msg->header.frame_id = frame_id; 54 | 55 | return true; 56 | } 57 | 58 | bool StageNode::Vehicle::Ranger::prepare_tf() 59 | { 60 | 61 | transform = std::make_shared(); 62 | 63 | Stg::Pose pose = model->GetPose(); 64 | tf2::Quaternion quternion; 65 | quternion.setRPY(0.0, 0.0, pose.a); 66 | tf2::Transform txLaser = 67 | tf2::Transform( 68 | quternion, 69 | tf2::Vector3(pose.x, pose.y, vehicle->positionmodel->GetGeom().size.z + pose.z)); 70 | *transform = create_transform_stamped( 71 | txLaser, vehicle->node()->sim_time_, vehicle->frame_id_base_link_, 72 | frame_id); 73 | if (vehicle->node()->use_static_transformations_) { 74 | vehicle->tf_static_broadcaster_->sendTransform(*transform); 75 | } 76 | return true; 77 | } 78 | 79 | void StageNode::Vehicle::Ranger::publish_msg() 80 | { 81 | // Guard 82 | if(!initialized_) return; 83 | 84 | if (model->GetSensors().size() > 1) { 85 | RCLCPP_WARN(vehicle->node()->get_logger(), "ROS Stage currently supports rangers with 1 sensor only."); 86 | } 87 | 88 | // for now we access only the zeroth sensor of the ranger - good 89 | // enough for most laser models that have a single beam origin 90 | const Stg::ModelRanger::Sensor & sensor = model->GetSensors()[0]; // we use the first sensor data 91 | 92 | if (prepare_msg()) { 93 | msg->header.stamp = vehicle->node()->sim_time_; 94 | for (unsigned int i = 0; i < sensor.ranges.size(); i++) { 95 | msg->ranges[i] = sensor.ranges[i]; 96 | msg->intensities[i] = sensor.intensities[i]; 97 | } 98 | pub->publish(*msg); 99 | } 100 | } 101 | 102 | void StageNode::Vehicle::Ranger::publish_tf() 103 | { 104 | if (prepare_tf()) { 105 | 106 | if (vehicle->node()->use_static_transformations_) {return;} 107 | 108 | // use tf publsiher only if use_static_transformations_ is false 109 | transform->header.stamp = vehicle->node()->sim_time_; 110 | vehicle->tf_broadcaster_->sendTransform(*transform); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /src/stage_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | StageNode::StageNode(rclcpp::NodeOptions options) 8 | : Node("stage_ros2", options), base_watchdog_timeout_(0, 0) 9 | { 10 | tf_broadcaster_stage_ = std::make_shared(this); 11 | declare_parameters(); 12 | } 13 | 14 | StageNode::~StageNode() 15 | { 16 | } 17 | 18 | void StageNode::declare_parameters() 19 | { 20 | this->set_parameter(rclcpp::Parameter("use_sim_time", true)); 21 | 22 | auto param_desc_use_stamped_velocity = rcl_interfaces::msg::ParameterDescriptor{}; 23 | param_desc_use_stamped_velocity.description = "on true it uses stamped command velocities, TwistStamped on false Twist msgs!"; 24 | this->declare_parameter("use_stamped_velocity", false, param_desc_use_stamped_velocity); 25 | 26 | auto param_desc_enable_gui = rcl_interfaces::msg::ParameterDescriptor{}; 27 | param_desc_enable_gui.description = "Enable GUI!"; 28 | this->declare_parameter("enable_gui", true, param_desc_enable_gui); 29 | 30 | auto param_desc_enforce_prefixes = rcl_interfaces::msg::ParameterDescriptor{}; 31 | param_desc_enforce_prefixes.description = 32 | "on true it enforces prefixes on topic even with one robot"; 33 | this->declare_parameter("enforce_prefixes", false, param_desc_enforce_prefixes); 34 | 35 | 36 | auto param_desc_one_tf_tree = rcl_interfaces::msg::ParameterDescriptor{}; 37 | param_desc_one_tf_tree.description = 38 | "On true: all tfs are publishe on /tf and /tf_static!"; 39 | this->declare_parameter("one_tf_tree", false, param_desc_one_tf_tree); 40 | 41 | auto param_desc_use_static_transformations_ = rcl_interfaces::msg::ParameterDescriptor{}; 42 | param_desc_use_static_transformations_.description = 43 | "use static transformations for sensor frames!"; 44 | this->declare_parameter( 45 | "use_static_transformations", true, 46 | param_desc_use_static_transformations_); 47 | 48 | auto param_desc_watchdog_timeout = rcl_interfaces::msg::ParameterDescriptor{}; 49 | param_desc_watchdog_timeout.description = 50 | "timeout after which a vehicle stopps if no command is received!"; 51 | this->declare_parameter("base_watchdog_timeout", 5, param_desc_watchdog_timeout); 52 | 53 | auto param_desc_is_depth_canonical = rcl_interfaces::msg::ParameterDescriptor{}; 54 | param_desc_is_depth_canonical.description = "USE depth canonical!"; 55 | this->declare_parameter("is_depth_canonical", true, param_desc_is_depth_canonical); 56 | 57 | auto param_desc_publish_ground_truth = rcl_interfaces::msg::ParameterDescriptor{}; 58 | param_desc_publish_ground_truth.description = "publishes on true a ground truth tf!"; 59 | this->declare_parameter("publish_ground_truth", true, param_desc_publish_ground_truth); 60 | 61 | auto param_desc_world_file = rcl_interfaces::msg::ParameterDescriptor{}; 62 | param_desc_world_file.description = "USE model names!"; 63 | this->declare_parameter("world_file", "cave.world", param_desc_world_file); 64 | 65 | auto param_desc_frame_id_odom_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 66 | param_desc_frame_id_odom_name_.description = 67 | "odom frame name or postfix in case of multiple robots"; 68 | this->declare_parameter("frame_id_odom", "odom", param_desc_frame_id_odom_name_); 69 | 70 | auto param_desc_frame_id_world_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 71 | param_desc_frame_id_world_name_.description = "world frame name for ground truth odom data"; 72 | this->declare_parameter("frame_id_world", "world", param_desc_frame_id_world_name_); 73 | 74 | auto param_desc_frame_id_base_link_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 75 | param_desc_frame_id_base_link_name_.description = 76 | "base link frame name or postfix in case of multiple robots"; 77 | this->declare_parameter( 78 | "frame_id_base_link", "base_link", 79 | param_desc_frame_id_base_link_name_); 80 | } 81 | 82 | void StageNode::update_parameters() 83 | { 84 | double base_watchdog_timeout_sec{5.0}; 85 | this->get_parameter("use_stamped_velocity", this->use_stamped_velocity_); 86 | this->get_parameter("enable_gui", this->enable_gui_); 87 | this->get_parameter("enforce_prefixes", this->enforce_prefixes_); 88 | this->get_parameter("one_tf_tree", this->one_tf_tree_); 89 | this->get_parameter("base_watchdog_timeout", base_watchdog_timeout_sec); 90 | this->base_watchdog_timeout_ = rclcpp::Duration::from_seconds(base_watchdog_timeout_sec); 91 | this->get_parameter("is_depth_canonical", this->isDepthCanonical_); 92 | this->get_parameter("publish_ground_truth", this->publish_ground_truth_); 93 | this->get_parameter("frame_id_odom", this->frame_id_odom_name_); 94 | this->get_parameter("frame_id_world", this->frame_id_world_name_); 95 | this->get_parameter("frame_id_base_link", this->frame_id_base_link_name_); 96 | 97 | this->get_parameter("world_file", this->world_file_); 98 | if (!std::filesystem::exists(this->world_file_)) { 99 | RCLCPP_FATAL( 100 | this->get_logger(), "The world file %s does not exist.", 101 | this->world_file_.c_str()); 102 | exit(0); 103 | } 104 | 105 | if (this->one_tf_tree_){ 106 | RCLCPP_WARN( 107 | this->get_logger(), "The parameter one_tf_tree is set but deprecated and will be removed in later versions"); 108 | } 109 | 110 | callback_update_parameters(); 111 | 112 | using namespace std::chrono_literals; 113 | timer_update_parameter_ = 114 | this->create_wall_timer(1000ms, std::bind(&StageNode::callback_update_parameters, this)); 115 | } 116 | 117 | void StageNode::callback_update_parameters() 118 | { 119 | double base_watchdog_timeout_sec; 120 | this->get_parameter("base_watchdog_timeout", base_watchdog_timeout_sec); 121 | this->base_watchdog_timeout_ = rclcpp::Duration::from_seconds(base_watchdog_timeout_sec); 122 | 123 | this->get_parameter("use_static_transformations", use_static_transformations_); 124 | 125 | this->get_parameter("publish_ground_truth", this->publish_ground_truth_); 126 | // RCLCPP_INFO(this->get_logger(), "callback_update_parameter"); 127 | } 128 | 129 | /** 130 | * Is called only ones after the simulation starts with each model 131 | * The function fills the vehicle vector with pointers to the stage models 132 | * @param mod stage model 133 | * @param node pointer to this class 134 | */ 135 | int StageNode::callback_init_stage_model(Stg::Model * mod, StageNode * node) 136 | { 137 | if (dynamic_cast(mod)) { 138 | Stg::ModelPosition * position = dynamic_cast(mod); 139 | RCLCPP_INFO(node->get_logger(), "New Vehicle \"%s\"", mod->TokenStr().c_str()); 140 | auto vehicle = std::make_shared( 141 | node->vehicles_.size(), 142 | position->GetGlobalPose(), mod->TokenStr(), node); 143 | node->vehicles_.push_back(vehicle); 144 | vehicle->positionmodel = position; 145 | } 146 | 147 | if (dynamic_cast(mod)) { 148 | Stg::ModelPosition * parent = dynamic_cast(mod->Parent()); 149 | for (std::shared_ptr vehcile: node->vehicles_) { 150 | if (parent == vehcile->positionmodel) { 151 | auto ranger = 152 | std::make_shared( 153 | vehcile->rangers_.size() + 1, 154 | dynamic_cast(mod), vehcile); 155 | vehcile->rangers_.push_back(ranger); 156 | } 157 | } 158 | } 159 | if (dynamic_cast(mod)) { 160 | Stg::ModelPosition * parent = dynamic_cast(mod->Parent()); 161 | for (std::shared_ptr vehcile: node->vehicles_) { 162 | if (parent == vehcile->positionmodel) { 163 | auto camera = 164 | std::make_shared( 165 | vehcile->cameras_.size() + 1, 166 | dynamic_cast(mod), vehcile); 167 | vehcile->cameras_.push_back(camera); 168 | } 169 | } 170 | } 171 | return 0; 172 | } 173 | 174 | int StageNode::callback_update_stage_world(Stg::World * world, StageNode * node) 175 | { 176 | // We return false to indicate that we want to be called again (an 177 | // odd convention, but that's the way that Stage works). 178 | if (!rclcpp::ok()) { 179 | RCLCPP_INFO(node->get_logger(), "rclcpp::ok() is false. Quitting."); 180 | node->world->QuitAll(); 181 | return 1; 182 | } 183 | 184 | std::scoped_lock lock(node->msg_lock); 185 | 186 | 187 | node->sim_time_ = rclcpp::Time(world->SimTimeNow() * 1e3); 188 | // We're not allowed to publish clock==0, because it used as a special 189 | // value in parts of ROS, #4027. 190 | if (int(node->sim_time_.nanoseconds()) == 0) { 191 | RCLCPP_DEBUG( 192 | node->get_logger(), "Skipping initial simulation step, to avoid publishing clock==0"); 193 | return 0; 194 | } 195 | // loop on the robot models 196 | for (size_t r = 0; r < node->vehicles_.size(); ++r) { 197 | auto vehicle = node->vehicles_[r]; 198 | vehicle->check_watchdog_timeout(); 199 | vehicle->publish_msg(); 200 | vehicle->publish_tf(); 201 | 202 | // loop on the ranger devices for the current robot 203 | for (auto ranger: vehicle->rangers_) { 204 | ranger->publish_msg(); 205 | ranger->publish_tf(); 206 | } 207 | 208 | 209 | // loop on the camera devices for the current robot 210 | for (auto camera: vehicle->cameras_) { 211 | camera->publish_msg(); 212 | camera->publish_tf(); 213 | } 214 | } 215 | rosgraph_msgs::msg::Clock clock_msg; 216 | clock_msg.clock = node->sim_time_; 217 | node->clock_pub_->publish(clock_msg); 218 | return 0; 219 | } 220 | 221 | bool StageNode::cb_reset_srv( 222 | const std_srvs::srv::Empty::Request::SharedPtr, 223 | std_srvs::srv::Empty::Response::SharedPtr) 224 | { 225 | RCLCPP_INFO(this->get_logger(), "Resetting stage!"); 226 | for (auto vehicle: this->vehicles_) { 227 | vehicle->soft_reset(); 228 | } 229 | return true; 230 | } 231 | 232 | void StageNode::init(int argc, char ** argv) 233 | { 234 | 235 | this->sim_time_ = rclcpp::Time(0, 0); 236 | update_parameters(); 237 | 238 | 239 | // initialize the libstage 240 | Stg::Init(&argc, &argv); 241 | 242 | if (this->enable_gui_) { 243 | this->world = new Stg::WorldGui(600, 400, "Stage (ROS)"); 244 | } else { 245 | this->world = new Stg::World(); 246 | } 247 | 248 | this->world->Load(world_file_.c_str()); 249 | this->world->AddUpdateCallback((Stg::world_callback_t)callback_update_stage_world, this); 250 | this->world->ForEachDescendant((Stg::model_callback_t)callback_init_stage_model, this); 251 | } 252 | 253 | // Subscribe to models of interest. Currently, we find and subscribe 254 | // to the first 'laser' model and the first 'position' model. Returns 255 | // 0 on success (both models subscribed), -1 otherwise. 256 | // 257 | // Eventually, we should provide a general way to map stage models onto ROS 258 | // topics, similar to Player .cfg files. 259 | int StageNode::SubscribeModels() 260 | { 261 | for (std::shared_ptr vehicle: this->vehicles_) { 262 | // init topics and use the stage models names if there are more than one vehicle in the world 263 | bool use_topic_prefix = this->enforce_prefixes_ || (vehicles_.size() > 1); // a prefixes are enforced 264 | vehicle->init(use_topic_prefix, this->one_tf_tree_); 265 | } 266 | 267 | // create the clock publisher 268 | clock_pub_ = this->create_publisher("/clock", 10); 269 | 270 | // advertising reset service 271 | srv_reset_ = this->create_service( 272 | "reset_positions", 273 | [this](const std_srvs::srv::Empty::Request::SharedPtr request, 274 | std_srvs::srv::Empty::Response::SharedPtr response) 275 | {this->cb_reset_srv(request, response);}); 276 | 277 | return 0; 278 | } 279 | 280 | bool StageNode::UpdateWorld() 281 | { 282 | return this->world->UpdateAll(); 283 | } 284 | 285 | // helper functions 286 | geometry_msgs::msg::TransformStamped StageNode::create_transform_stamped( 287 | const tf2::Transform & in, 288 | const rclcpp::Time & timestamp, const std::string & frame_id, const std::string & child_frame_id) 289 | { 290 | geometry_msgs::msg::TransformStamped out; 291 | out.header.stamp = timestamp; 292 | out.header.frame_id = frame_id; 293 | out.child_frame_id = child_frame_id; 294 | out.transform.translation.x = in.getOrigin().getX(); 295 | out.transform.translation.y = in.getOrigin().getY(); 296 | out.transform.translation.z = in.getOrigin().getZ(); 297 | out.transform.rotation.w = in.getRotation().getW(); 298 | out.transform.rotation.x = in.getRotation().getX(); 299 | out.transform.rotation.y = in.getRotation().getY(); 300 | out.transform.rotation.z = in.getRotation().getZ(); 301 | return out; 302 | } 303 | 304 | geometry_msgs::msg::Quaternion StageNode::createQuaternionMsgFromYaw(double yaw) 305 | { 306 | tf2::Quaternion q; 307 | q.setRPY(0, 0, yaw); 308 | return tf2::toMsg(q); 309 | } 310 | -------------------------------------------------------------------------------- /src/vehicle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_TF "tf" 8 | #define TOPIC_TF_STATIC "tf_static" 9 | #define TOPIC_ODOM "odom" 10 | #define TOPIC_GROUND_TRUTH "ground_truth" 11 | #define TOPIC_CMD_VEL "cmd_vel" 12 | 13 | using std::placeholders::_1; 14 | 15 | StageNode::Vehicle::Vehicle( 16 | size_t id, const Stg::Pose &pose, const std::string &name, 17 | StageNode *node) 18 | : initialized_(false), id_(id), initial_pose_(pose), name_(name), node_(node) 19 | { 20 | } 21 | 22 | size_t StageNode::Vehicle::id() const 23 | { 24 | return id_; 25 | } 26 | void StageNode::Vehicle::soft_reset() 27 | { 28 | positionmodel->SetPose(this->initial_pose_); 29 | positionmodel->SetStall(false); 30 | } 31 | 32 | const std::string &StageNode::Vehicle::name() const 33 | { 34 | return name_; 35 | } 36 | 37 | void StageNode::Vehicle::init(bool use_topic_prefixes, bool use_one_tf_tree) 38 | { 39 | if (initialized_) 40 | return; 41 | 42 | time_last_pose_update_ = rclcpp::Time(0, 0); 43 | time_last_cmd_received_ = rclcpp::Time(0, 0); 44 | timeout_cmd_ = rclcpp::Time(0, 0); 45 | 46 | topic_name_space_ = std::string(); 47 | frame_name_space_ = std::string(); 48 | if (use_topic_prefixes == true) 49 | { 50 | topic_name_space_ = name() + "/"; 51 | } 52 | if (use_one_tf_tree) 53 | { 54 | frame_name_space_ = name() + "/"; 55 | topic_name_tf_ = std::string("/") + TOPIC_TF; 56 | topic_name_tf_static_ = std::string("/") + TOPIC_TF_STATIC; 57 | } else { 58 | topic_name_tf_ = topic_name_space_ + TOPIC_TF; 59 | topic_name_tf_static_ = topic_name_space_ + TOPIC_TF_STATIC; 60 | } 61 | 62 | frame_id_base_link_ = frame_name_space_ + node_->frame_id_base_link_name_; 63 | frame_id_odom_ = frame_name_space_ + node_->frame_id_odom_name_; 64 | frame_id_world_ = frame_name_space_ + node_->frame_id_world_name_; 65 | 66 | topic_name_odom_ = topic_name_space_ + TOPIC_ODOM; 67 | topic_name_ground_truth_ = topic_name_space_ + TOPIC_GROUND_TRUTH; 68 | topic_name_cmd_ = topic_name_space_ + TOPIC_CMD_VEL; 69 | 70 | tf_static_broadcaster_ = std::make_shared(node_, topic_name_tf_static_.c_str()); 71 | tf_broadcaster_ = std::make_shared(node_, topic_name_tf_.c_str()); 72 | 73 | pub_odom_ = node_->create_publisher(topic_name_odom_, 10); 74 | pub_ground_truth_ = 75 | node_->create_publisher(topic_name_ground_truth_, 10); 76 | 77 | if(node_->use_stamped_velocity_){ 78 | sub_cmd_stamped_ = 79 | node_->create_subscription( 80 | topic_name_cmd_, 10, 81 | std::bind(&StageNode::Vehicle::callback_cmd_stamped, this, _1)); 82 | RCLCPP_INFO(node_->get_logger(), "%s is using stamped velocity commands.", name().c_str()); 83 | } else { 84 | sub_cmd_ = 85 | node_->create_subscription( 86 | topic_name_cmd_, 10, 87 | std::bind(&StageNode::Vehicle::callback_cmd, this, _1)); 88 | RCLCPP_INFO(node_->get_logger(), "%s is useing unstamped velocity commands.", name().c_str()); 89 | } 90 | positionmodel->Subscribe(); 91 | 92 | for (std::shared_ptr ranger : rangers_) 93 | { 94 | ranger->init(rangers_.size() > 1); 95 | } 96 | 97 | for (std::shared_ptr camera : cameras_) 98 | { 99 | camera->init(rangers_.size() > 1); 100 | } 101 | initialized_ = true; 102 | } 103 | 104 | void StageNode::Vehicle::publish_msg() 105 | { 106 | // Guard 107 | if (!initialized_) 108 | return; 109 | 110 | // Get latest odometry data 111 | // Translate into ROS message format and publish 112 | msg_odom_.pose.pose.position.x = positionmodel->est_pose.x; 113 | msg_odom_.pose.pose.position.y = positionmodel->est_pose.y; 114 | msg_odom_.pose.pose.orientation = createQuaternionMsgFromYaw(positionmodel->est_pose.a); 115 | Stg::Velocity v = positionmodel->GetVelocity(); 116 | msg_odom_.twist.twist.linear.x = v.x; 117 | msg_odom_.twist.twist.linear.y = v.y; 118 | msg_odom_.twist.twist.angular.z = v.a; 119 | msg_odom_.header.frame_id = frame_id_odom_; 120 | msg_odom_.header.stamp = node_->sim_time_; 121 | msg_odom_.child_frame_id = frame_id_base_link_; 122 | 123 | pub_odom_->publish(msg_odom_); 124 | 125 | // Also publish the ground truth pose and velocity 126 | Stg::Pose gpose = positionmodel->GetGlobalPose(); 127 | tf2::Quaternion q_gpose; 128 | q_gpose.setRPY(0.0, 0.0, gpose.a); 129 | tf2::Transform gt(q_gpose, tf2::Vector3(gpose.x, gpose.y, 0.0)); 130 | // Velocity is 0 by default and will be set only if there is previous pose and time delta>0 131 | // @ToDo uising the positionmodel->GetVelocity() a self computed delta 132 | Stg::Velocity gvel(0, 0, 0, 0); 133 | if (global_pose_) 134 | { 135 | double dT = (node_->sim_time_ - time_last_pose_update_).seconds(); 136 | if (dT > 0) 137 | { 138 | gvel = Stg::Velocity( 139 | (gpose.x - global_pose_->x) / dT, 140 | (gpose.y - global_pose_->y) / dT, 141 | (gpose.z - global_pose_->z) / dT, 142 | Stg::normalize(gpose.a - global_pose_->a) / dT); 143 | } 144 | *global_pose_ = gpose; 145 | } 146 | else 147 | { 148 | // There are no previous readings, adding current pose... 149 | global_pose_ = std::make_shared(gpose); 150 | } 151 | nav_msgs::msg::Odometry ground_truth_msg; 152 | ground_truth_msg.pose.pose.position.x = gt.getOrigin().x(); 153 | ground_truth_msg.pose.pose.position.y = gt.getOrigin().y(); 154 | ground_truth_msg.pose.pose.position.z = gt.getOrigin().z(); 155 | ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x(); 156 | ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y(); 157 | ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z(); 158 | ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w(); 159 | ground_truth_msg.twist.twist.linear.x = gvel.x; 160 | ground_truth_msg.twist.twist.linear.y = gvel.y; 161 | ground_truth_msg.twist.twist.linear.z = gvel.z; 162 | ground_truth_msg.twist.twist.angular.z = gvel.a; 163 | 164 | ground_truth_msg.header.frame_id = frame_id_world_; 165 | ground_truth_msg.header.stamp = node_->sim_time_; 166 | 167 | pub_ground_truth_->publish(ground_truth_msg); 168 | time_last_pose_update_ = node_->sim_time_; 169 | } 170 | void StageNode::Vehicle::publish_tf() 171 | { 172 | 173 | // broadcast odometry transform 174 | tf2::Quaternion quaternion = tf2::Quaternion( 175 | msg_odom_.pose.pose.orientation.x, 176 | msg_odom_.pose.pose.orientation.y, 177 | msg_odom_.pose.pose.orientation.z, 178 | msg_odom_.pose.pose.orientation.w); 179 | tf2::Transform transform(quaternion, 180 | tf2::Vector3(msg_odom_.pose.pose.position.x, msg_odom_.pose.pose.position.y, 0.0)); 181 | tf_broadcaster_->sendTransform( 182 | create_transform_stamped( 183 | transform, node_->sim_time_, 184 | frame_id_odom_, 185 | frame_id_base_link_)); 186 | } 187 | 188 | void StageNode::Vehicle::check_watchdog_timeout() 189 | { 190 | 191 | if ((timeout_cmd_ != rclcpp::Time(0, 0)) && (node_->sim_time_ > timeout_cmd_)) 192 | { 193 | Stg::Velocity v = positionmodel->GetVelocity(); 194 | // stopping makes only sense if the vehicle drives 195 | if (!positionmodel->GetVelocity().IsZero()) 196 | { 197 | this->positionmodel->SetSpeed(0.0, 0.0, 0.0); 198 | RCLCPP_INFO(node_->get_logger(), "watchdog timeout on %s", name().c_str()); 199 | } 200 | } 201 | } 202 | void StageNode::Vehicle::callback_cmd(const geometry_msgs::msg::Twist::SharedPtr msg) 203 | { 204 | std::scoped_lock lock(node_->msg_lock); 205 | this->positionmodel->SetSpeed( 206 | msg->linear.x, 207 | msg->linear.y, 208 | msg->angular.z); 209 | time_last_cmd_received_ = node_->sim_time_; 210 | timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_; 211 | } 212 | 213 | void StageNode::Vehicle::callback_cmd_stamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg) 214 | { 215 | std::scoped_lock lock(node_->msg_lock); 216 | this->positionmodel->SetSpeed( 217 | msg->twist.linear.x, 218 | msg->twist.linear.y, 219 | msg->twist.angular.z); 220 | time_last_cmd_received_ = node_->sim_time_; 221 | timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_; 222 | } 223 | -------------------------------------------------------------------------------- /tmuxinator/cave_one_robot.yml: -------------------------------------------------------------------------------- 1 | ##enable named panes in tmux.conf with: 2 | #set -g pane-border-format "#{pane_index} #{pane_title}" 3 | #set -g pane-border-status bottom 4 | 5 | name: cave_one_robot 6 | #root: /home/markus/projects/tuw-ros 7 | 8 | windows: 9 | - simulation: 10 | layout: even-vertical 11 | panes: 12 | - stage: 13 | - printf '\033]2;%s\033\\' 'stage' 14 | - ros2 launch stage_ros2 stage.launch.py world:=cave enforce_prefixes:=true one_tf_tree:=false 15 | - rviz robot_0: 16 | - printf '\033]2;%s\033\\' 'rviz robot_0' 17 | - ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns namespace:=robot_0 18 | - mouse_teleop robot_0: 19 | - printf '\033]2;%s\033\\' 'mouse_teleop robot_0' 20 | - ros2 run mouse_teleop mouse_teleop -r __ns:=/robot_0 --ros-args --remap mouse_vel:=cmd_vel -------------------------------------------------------------------------------- /tmuxinator/cave_three_robots.yml: -------------------------------------------------------------------------------- 1 | ##enable named panes in tmux.conf with: 2 | #set -g pane-border-format "#{pane_index} #{pane_title}" 3 | #set -g pane-border-status bottom 4 | 5 | 6 | name: cave_three_robots 7 | #root: /home/markus/projects/tuw-ros 8 | 9 | windows: 10 | - simulation: 11 | layout: even-vertical 12 | panes: 13 | - stage: 14 | - printf '\033]2;%s\033\\' 'stage' 15 | - ros2 launch stage_ros2 stage.launch.py world:=cave_three_robots one_tf_tree:=false 16 | - rviz robot_0: 17 | - printf '\033]2;%s\033\\' 'rviz robot_0' 18 | - ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns_depth namespace:=robot_0 19 | - rviz robot_1: 20 | - printf '\033]2;%s\033\\' 'rviz robot_1' 21 | - ros2 launch stage_ros2 rviz_ns.launch.py config:=robot_ns_depth namespace:=robot_1 22 | - mouse_teleop robot_0: 23 | - printf '\033]2;%s\033\\' 'mouse_teleop robot_0' 24 | - ros2 run mouse_teleop mouse_teleop -r __ns:=/robot_0 --ros-args --remap mouse_vel:=cmd_vel -------------------------------------------------------------------------------- /world/bitmaps/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/cave.png -------------------------------------------------------------------------------- /world/bitmaps/hallway.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/hallway.pgm -------------------------------------------------------------------------------- /world/bitmaps/hallway.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/hallway.png -------------------------------------------------------------------------------- /world/bitmaps/line.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/line.png -------------------------------------------------------------------------------- /world/bitmaps/straden.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/straden.png -------------------------------------------------------------------------------- /world/bitmaps/warehouse008.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/warehouse008.pgm -------------------------------------------------------------------------------- /world/bitmaps/warehouse032.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/warehouse032.pgm -------------------------------------------------------------------------------- /world/bitmaps/warehouse200.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/warehouse200.pgm -------------------------------------------------------------------------------- /world/bitmaps/willow-full.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tuw-robotics/stage_ros2/c56d82f1a0082ac33c4724982c64f70d7035bef2/world/bitmaps/willow-full.pgm -------------------------------------------------------------------------------- /world/cave.world: -------------------------------------------------------------------------------- 1 | # cave.world 2 | # simple cave environment with a pioneer robot based on the basic world file examples of 3 | # Richard Vaughan Richard Vaughan, Andrew Howard, Luis Riazuelo 4 | # Authors: Markus Bader 5 | 6 | include "include/robots.inc" 7 | 8 | # set the resolution of the underlying raytrace model in meters 9 | resolution 0.02 10 | 11 | # simulation timestep in milliseconds 12 | interval_sim 100 13 | 14 | define floorplan model 15 | ( 16 | # sombre, sensible, artistic 17 | color "gray30" 18 | 19 | # most maps will need a bounding box 20 | boundary 1 21 | 22 | gui_nose 0 23 | gui_grid 0 24 | 25 | gui_outline 0 26 | gripper_return 0 27 | fiducial_return 0 28 | laser_return 1 29 | ) 30 | 31 | # configure the GUI window 32 | window 33 | ( 34 | size [ 635.000 666.000 ] # in pixels 35 | scale 36.995 # pixels per meter 36 | center [ -0.040 -0.274 ] 37 | rotate [ 0 0 ] 38 | 39 | show_data 1 # 1=on 0=off 40 | ) 41 | 42 | # load an environment bitmap 43 | floorplan 44 | ( 45 | name "cave" 46 | size [16.000 16.000 0.800] 47 | pose [0 0 0 0] 48 | bitmap "bitmaps/cave.png" 49 | gui_move 0 50 | ) 51 | 52 | # define a block 53 | define my_block model 54 | ( 55 | size [0.5 0.5 0.5] 56 | gui_nose 0 57 | ) 58 | 59 | # define a block 60 | define mount model 61 | ( 62 | size [0.02 0.02 0.5] 63 | gui_nose 0 64 | ) 65 | 66 | # throw in a block 67 | my_block( pose [ 5 4 0 180.000 ] color "green") 68 | 69 | # throw in a robot 70 | pioneer2dx_with_laser 71 | ( 72 | # can refer to the robot by this name 73 | name "robot_0" 74 | color "red" 75 | pose [ -7 -7 0 45 ] 76 | ) 77 | -------------------------------------------------------------------------------- /world/cave_multi.world: -------------------------------------------------------------------------------- 1 | # cave_multi.world 2 | # simple cave environment with multiple pioneer robots based on the basic world file examples of 3 | # Richard Vaughan Richard Vaughan, Andrew Howard, Luis Riazuelo 4 | # Authors: Markus Bader 5 | 6 | include "include/robots.inc" 7 | 8 | # set the resolution of the underlying raytrace model in meters 9 | resolution 0.02 10 | 11 | # simulation timestep in milliseconds 12 | interval_sim 100 13 | 14 | define floorplan model 15 | ( 16 | # sombre, sensible, artistic 17 | color "gray30" 18 | 19 | # most maps will need a bounding box 20 | boundary 1 21 | 22 | gui_nose 0 23 | gui_grid 0 24 | 25 | gui_outline 0 26 | gripper_return 0 27 | fiducial_return 0 28 | laser_return 1 29 | ) 30 | 31 | # configure the GUI window 32 | window 33 | ( 34 | size [ 635.000 666.000 ] # in pixels 35 | scale 36.995 # pixels per meter 36 | center [ -0.040 -0.274 ] 37 | rotate [ 0 0 ] 38 | 39 | show_data 1 # 1=on 0=off 40 | ) 41 | 42 | # load an environment bitmap 43 | floorplan 44 | ( 45 | name "cave" 46 | size [16.000 16.000 0.800] 47 | pose [0 0 0 0] 48 | bitmap "bitmaps/cave.png" 49 | ) 50 | 51 | # define a block 52 | define my_block model 53 | ( 54 | size [0.5 0.5 0.5] 55 | gui_nose 0 56 | ) 57 | 58 | # throw in a block 59 | my_block( pose [ 5 4 0 180.000 ] color "green") 60 | 61 | # throw in some robots 62 | pioneer2dx_with_laser 63 | ( 64 | # can refer to the robot by this name 65 | name "robot_0" 66 | color "red" 67 | pose [ -7 -7 0 45 ] 68 | ) 69 | 70 | pioneer2dx_with_laser 71 | ( 72 | # can refer to the robot by this name 73 | name "robot_1" 74 | color "GreenYellow" 75 | pose [ 7 7 0 225 ] 76 | ) 77 | pioneer2dx_with_laser 78 | ( 79 | # can refer to the robot by this name 80 | name "robot_2" 81 | color "DarkOliveGreen" 82 | pose [ -7 7 0 -45 ] 83 | ) 84 | pioneer2dx_with_laser 85 | ( 86 | # can refer to the robot by this name 87 | name "robot_3" 88 | color "magenta" 89 | pose [ 7 -3 0 180 ] 90 | ) 91 | pioneer2dx_with_laser 92 | ( 93 | # can refer to the robot by this name 94 | name "robot_4" 95 | color "cyan" 96 | pose [ 0 7 0 -90 ] 97 | ) 98 | 99 | pioneer2dx_with_laser 100 | ( 101 | # can refer to the robot by this name 102 | name "robot_5" 103 | color "yellow" 104 | pose [ 0 -7 0 90 ] 105 | ) 106 | pioneer2dx_with_laser 107 | ( 108 | # can refer to the robot by this name 109 | name "robot_6" 110 | color "DarkSeaGreen" 111 | pose [ -7 0 0 0 ] 112 | ) -------------------------------------------------------------------------------- /world/cave_seven_robots.world: -------------------------------------------------------------------------------- 1 | # cave_multi.world 2 | # simple cave environment with multiple pioneer robots based on the basic world file examples of 3 | # Richard Vaughan Richard Vaughan, Andrew Howard, Luis Riazuelo 4 | # Authors: Markus Bader 5 | 6 | 7 | include "include/robots.inc" 8 | 9 | # set the resolution of the underlying raytrace model in meters 10 | resolution 0.02 11 | 12 | # simulation timestep in milliseconds 13 | interval_sim 100 14 | 15 | define floorplan model 16 | ( 17 | # sombre, sensible, artistic 18 | color "gray30" 19 | 20 | # most maps will need a bounding box 21 | boundary 1 22 | 23 | gui_nose 0 24 | gui_grid 0 25 | 26 | gui_outline 0 27 | gripper_return 0 28 | fiducial_return 0 29 | laser_return 1 30 | ) 31 | 32 | # configure the GUI window 33 | window 34 | ( 35 | size [ 635.000 666.000 ] # in pixels 36 | scale 36.995 # pixels per meter 37 | center [ -0.040 -0.274 ] 38 | rotate [ 0 0 ] 39 | 40 | show_data 1 # 1=on 0=off 41 | ) 42 | 43 | # load an environment bitmap 44 | floorplan 45 | ( 46 | name "cave" 47 | size [16.000 16.000 0.800] 48 | pose [0 0 0 0] 49 | bitmap "bitmaps/cave.png" 50 | gui_move 0 51 | ) 52 | 53 | # define a block 54 | define my_block model 55 | ( 56 | size [0.5 0.5 0.5] 57 | gui_nose 0 58 | ) 59 | 60 | # throw in a block 61 | my_block( pose [ 5 4 0 180.000 ] color "green") 62 | 63 | # throw in some robots 64 | pioneer2dx_with_laser 65 | ( 66 | # can refer to the robot by this name 67 | name "robot_0" 68 | color "red" 69 | pose [ -7 -7 0 45 ] 70 | ) 71 | 72 | pioneer2dx_with_laser 73 | ( 74 | # can refer to the robot by this name 75 | name "robot_1" 76 | color "GreenYellow" 77 | pose [ 7 7 0 225 ] 78 | ) 79 | pioneer2dx_with_laser 80 | ( 81 | # can refer to the robot by this name 82 | name "robot_2" 83 | color "DarkOliveGreen" 84 | pose [ -7 7 0 -45 ] 85 | ) 86 | pioneer2dx_with_laser 87 | ( 88 | # can refer to the robot by this name 89 | name "robot_3" 90 | color "magenta" 91 | pose [ 7 -3 0 180 ] 92 | ) 93 | pioneer2dx_with_laser 94 | ( 95 | # can refer to the robot by this name 96 | name "robot_4" 97 | color "cyan" 98 | pose [ 0 7 0 -90 ] 99 | ) 100 | 101 | pioneer2dx_with_laser 102 | ( 103 | # can refer to the robot by this name 104 | name "robot_5" 105 | color "yellow" 106 | pose [ 0 -7 0 90 ] 107 | ) 108 | pioneer2dx_with_laser 109 | ( 110 | # can refer to the robot by this name 111 | name "robot_6" 112 | color "DarkSeaGreen" 113 | pose [ -7 0 0 0 ] 114 | ) 115 | -------------------------------------------------------------------------------- /world/cave_three_robots.world: -------------------------------------------------------------------------------- 1 | # cave.world 2 | # simple cave environment with a pioneer robot based on the basic world file examples of 3 | # Richard Vaughan Richard Vaughan, Andrew Howard, Luis Riazuelo 4 | # Authors: Markus Bader 5 | 6 | include "include/robots.inc" 7 | 8 | # set the resolution of the underlying raytrace model in meters 9 | resolution 0.02 10 | 11 | # simulation timestep in milliseconds 12 | interval_sim 100 13 | 14 | define floorplan model 15 | ( 16 | # sombre, sensible, artistic 17 | color "gray30" 18 | 19 | # most maps will need a bounding box 20 | boundary 1 21 | 22 | gui_nose 0 23 | gui_grid 0 24 | 25 | gui_outline 0 26 | gripper_return 0 27 | fiducial_return 0 28 | laser_return 1 29 | ) 30 | 31 | # configure the GUI window 32 | window 33 | ( 34 | size [ 635.000 666.000 ] # in pixels 35 | scale 36.995 # pixels per meter 36 | center [ -0.040 -0.274 ] 37 | rotate [ 0 0 ] 38 | 39 | show_data 1 # 1=on 0=off 40 | ) 41 | 42 | # load an environment bitmap 43 | floorplan 44 | ( 45 | name "cave" 46 | size [16.000 16.000 0.800] 47 | pose [0 0 0 0] 48 | bitmap "bitmaps/cave.png" 49 | gui_move 0 50 | ) 51 | 52 | # define a block 53 | define my_block model 54 | ( 55 | size [0.5 0.5 0.5] 56 | gui_nose 0 57 | ) 58 | 59 | # define a block 60 | define mount model 61 | ( 62 | size [0.02 0.02 0.5] 63 | gui_nose 0 64 | ) 65 | 66 | # throw in a block 67 | my_block( pose [ 5 4 0 180.000 ] color "green") 68 | 69 | # throw in a robot 70 | pioneer2dx_with_laser_and_camera 71 | ( 72 | # can refer to the robot by this name 73 | name "robot_0" 74 | color "red" 75 | pose [ -7 -7 0 45 ] 76 | ) 77 | 78 | 79 | # throw in a robot 80 | pioneer2dx_with_laser_and_camera 81 | ( 82 | # can refer to the robot by this name 83 | name "robot_1" 84 | color "GreenYellow" 85 | pose [ -5 -4 0 225 ] 86 | ) 87 | 88 | # throw in a robot 89 | pioneer2dx_with_two_laser 90 | ( 91 | # can refer to the robot by this name 92 | name "robot_2" 93 | color "GreenBlue" 94 | pose [ -5 -6 0 225 ] 95 | ) -------------------------------------------------------------------------------- /world/hallway.world: -------------------------------------------------------------------------------- 1 | # cave.world 2 | # simple cave environment with a pioneer robot based on the basic world file examples of 3 | # Richard Vaughan Richard Vaughan, Andrew Howard, Luis Riazuelo 4 | # Authors: Markus Bader 5 | 6 | include "include/robots.inc" 7 | 8 | # set the resolution of the underlying raytrace model in meters 9 | resolution 0.02 10 | 11 | # simulation timestep in milliseconds 12 | interval_sim 100 13 | 14 | define floorplan model 15 | ( 16 | # sombre, sensible, artistic 17 | color "gray30" 18 | 19 | # most maps will need a bounding box 20 | boundary 1 21 | 22 | gui_nose 0 23 | gui_grid 0 24 | 25 | gui_outline 0 26 | gripper_return 0 27 | fiducial_return 0 28 | laser_return 1 29 | ) 30 | 31 | # configure the GUI window 32 | window 33 | ( 34 | size [ 635.000 666.000 ] # in pixels 35 | scale 20.0 # pixels per meter 36 | center [ -0.0 -0.0 ] 37 | rotate [ 0 0 ] 38 | 39 | show_data 1 # 1=on 0=off 40 | ) 41 | 42 | # load an environment bitmap 43 | floorplan 44 | ( 45 | name "hallway" 46 | size [25.0 25.00 0.800] 47 | pose [0 0 0 0] 48 | bitmap "bitmaps/hallway.png" 49 | gui_move 0 50 | ) 51 | 52 | # define a block 53 | define my_block model 54 | ( 55 | size [0.5 0.5 0.5] 56 | gui_nose 0 57 | ) 58 | 59 | # define a block 60 | define mount model 61 | ( 62 | size [0.02 0.02 0.5] 63 | gui_nose 0 64 | ) 65 | 66 | # throw in a block 67 | my_block( pose [ 0 -1.5 0 0.000 ] color "green") 68 | 69 | 70 | # throw in a robot 71 | pioneer2dx_with_laser 72 | ( 73 | # can refer to the robot by this name 74 | name "robot_0" 75 | color "red" 76 | pose [ -3 0 0 0 ] 77 | ) 78 | 79 | 80 | # throw in a robot 81 | pioneer2dx_with_laser 82 | ( 83 | # can refer to the robot by this name 84 | name "robot_1" 85 | color "GreenYellow" 86 | pose [ 3 0 0 180 ] 87 | ) 88 | 89 | # throw in a robot 90 | pioneer2dx_with_laser 91 | ( 92 | # can refer to the robot by this name 93 | name "robot_2" 94 | color "GreenBlue" 95 | pose [ -0 4 0 270 ] 96 | ) -------------------------------------------------------------------------------- /world/include/pioneer2dx.inc: -------------------------------------------------------------------------------- 1 | # A pioneer robot optionally equipped with a laser range sensor and/or a QR-code reader 2 | 3 | define pioneer_base position 4 | ( 5 | color "red" # Default color. 6 | drive "diff" # Differential steering model. 7 | gui_nose 1 # Draw a nose on the robot so we can see which way it points 8 | obstacle_return 1 # Can hit things. 9 | ranger_return 0.5 # reflects sonar beams 10 | blob_return 1 # Seen by blobfinders 11 | fiducial_return 1 # Seen as "1" fiducial finders 12 | 13 | localization "odom" # Change to "gps" to have impossibly perfect, global odometry 14 | odom_error [ 0.05 0.05 0.0 0.1 ] # Odometry error or slip in X, Y and Theta (Uniform random distribution) 15 | # localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). By default, this is copied from the model's initial pose 16 | 17 | # four DOF kinematics limits 18 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 19 | velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ] 20 | acceleration_bounds [-0.5 0.5 0 0 0 0 -45 45.0 ] 21 | ) 22 | 23 | define pioneer2dx_base_no_sonar pioneer_base 24 | ( 25 | # actual size 26 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site 27 | 28 | # the pioneer's center of rotation is offset from its center of area 29 | origin [-0.04 0 0 0] 30 | 31 | # draw a nose on the robot so we can see which way it points 32 | gui_nose 0 33 | 34 | # estimated mass in KG 35 | mass 23.0 36 | ) 37 | 38 | # as above, but with front sonar only 39 | define pioneer2dx_no_sonar pioneer2dx_base_no_sonar 40 | ( 41 | # simplified Body shape: 42 | block( 43 | points 8 44 | point[0] [-0.2 0.12] 45 | point[1] [-0.2 -0.12] 46 | point[2] [-0.12 -0.2555] 47 | point[3] [0.12 -0.2555] 48 | point[4] [0.2 -0.12] 49 | point[5] [0.2 0.12] 50 | point[6] [0.12 0.2555] 51 | point[7] [-0.12 0.2555] 52 | z [0 0.22] 53 | ) 54 | ) 55 | -------------------------------------------------------------------------------- /world/include/robots.inc: -------------------------------------------------------------------------------- 1 | include "include/pioneer2dx.inc" 2 | 3 | define hokuyolaser ranger 4 | ( 5 | sensor( 6 | # laser-specific properties 7 | # factory settings for LMS200 8 | pose [ 0 0 0 0 ] 9 | size [ 0.07 0.07 0.05 ] 10 | range [ 0.0 5.0 ] 11 | fov 270.0 12 | samples 270 13 | color_rgba [ 0 0 1 0.15 ] 14 | ) 15 | model 16 | ( 17 | # generic model properties 18 | pose [ 0.0 0.0 -0.2 0.0 ] 19 | size [ 0.07 0.07 0.05 ] 20 | color "blue" 21 | ) 22 | ) 23 | 24 | define camera_realsens_d435 camera 25 | ( 26 | range [ 0.3 3.0 ] 27 | resolution [ 160 90 ] # 1280 × 720 / 8 28 | fov [ 87 58 ] 29 | pantilt [ 0 0 ] 30 | alwayson 1 31 | size [ 0.025 0.09 0.025 ] 32 | color "gray" 33 | ) 34 | 35 | define qrcodereader fiducial 36 | ( 37 | fiducial_key 1 # fiducial_key values of model and fiducial detector must match 38 | 39 | # fiducial properties 40 | range_min 0.0 41 | range_max 8.0 42 | range_max_id 5.0 43 | fov 270.0 44 | ignore_zloc 1 # When set to 1, the fiducial finder ignores the z component when checking a fiducial 45 | 46 | model 47 | ( 48 | # model properties 49 | size [ 0.1 0.1 0.1 ] 50 | color "green" 51 | ) 52 | ) 53 | 54 | define pioneer2dx_with_laser pioneer2dx_no_sonar 55 | ( 56 | # can refer to the robot by this name 57 | hokuyolaser(pose [ 0.15 0 0 0 ]) 58 | model 59 | ( 60 | # generic model properties 61 | pose [ -0.1 0.0 0 0.0 ] 62 | size [ 0.2 0.2 0.05 ] 63 | color "DimGray" 64 | ) 65 | ) 66 | 67 | define pioneer2dx_with_laser_and_camera pioneer2dx_with_laser 68 | ( 69 | # can refer to the robot by this name 70 | camera_realsens_d435(pose [ 0 0 0.5 0 ]) 71 | mount( pose [ 0 -0.15 0 0.000 ] color "gray") 72 | mount( pose [ 0 0.15 0 0.000 ] color "gray") 73 | ) 74 | 75 | 76 | define pioneer2dx_with_two_laser pioneer2dx_no_sonar 77 | ( 78 | # can refer to the robot by this name 79 | hokuyolaser(pose [ 0.15 0.15 0 45 ]) 80 | model 81 | ( 82 | # generic model properties 83 | pose [ -0.1 0.0 0 0 ] 84 | size [ 0.2 0.2 0.05 ] 85 | color "DimGray" 86 | ) 87 | hokuyolaser(pose [ -0.20 -0.15 0 -135 ]) 88 | model 89 | ( 90 | # generic model properties 91 | pose [ -0.1 0.0 0 0.0 ] 92 | size [ 0.2 0.2 0.05 ] 93 | color "DimGray" 94 | ) 95 | ) 96 | -------------------------------------------------------------------------------- /world/lines.world: -------------------------------------------------------------------------------- 1 | # line.world 2 | # Authors: Markus Bader 3 | 4 | include "include/robots.inc" 5 | 6 | # set the resolution of the underlying raytrace model in meters 7 | resolution 0.02 8 | 9 | # simulation timestep in milliseconds 10 | interval_sim 100 11 | 12 | 13 | define floorplan model 14 | ( 15 | # sombre, sensible, artistic 16 | color "gray30" 17 | 18 | # most maps will need a bounding box 19 | boundary 1 20 | 21 | gui_nose 0 22 | gui_grid 0 23 | 24 | gui_outline 0 25 | gripper_return 0 26 | fiducial_return 0 27 | laser_return 1 28 | ) 29 | 30 | # configure the GUI window 31 | window 32 | ( 33 | size [ 635.000 666.000 ] # in pixels 34 | scale 36.995 # pixels per meter 35 | center [ -0.040 -0.274 ] 36 | rotate [ 0 0 ] 37 | 38 | show_data 1 # 1=on 0=off 39 | ) 40 | 41 | # load an environment bitmap 42 | floorplan 43 | ( 44 | name "line" 45 | size [16.000 16.000 0.800] 46 | pose [0 0 0 0] 47 | bitmap "bitmaps/line.png" 48 | gui_move 0 49 | ) 50 | 51 | # define a block 52 | define my_block model 53 | ( 54 | size [1 1 1] 55 | gui_nose 0 56 | ) 57 | 58 | # throw in a block 59 | my_block( pose [ 5 4 0 180.000 ] color "green") 60 | 61 | # throw in a block 62 | my_block( pose [ 5 7 0 180.000 ] color "blue") 63 | 64 | # throw in a robot 65 | pioneer2dx_with_laser 66 | ( 67 | # can refer to the robot by this name 68 | name "robot_0" 69 | color "red" 70 | pose [ 0 0 0 45 ] 71 | ) 72 | -------------------------------------------------------------------------------- /world/straden.world: -------------------------------------------------------------------------------- 1 | # line.world 2 | # Authors: Markus Bader 3 | 4 | include "include/robots.inc" 5 | 6 | # set the resolution of the underlying raytrace model in meters 7 | resolution 0.02 8 | 9 | # simulation timestep in milliseconds 10 | interval_sim 100 11 | 12 | 13 | define floorplan model 14 | ( 15 | # sombre, sensible, artistic 16 | color "gray30" 17 | 18 | # most maps will need a bounding box 19 | boundary 1 20 | 21 | gui_nose 0 22 | gui_grid 0 23 | 24 | gui_outline 0 25 | gripper_return 0 26 | fiducial_return 0 27 | laser_return 1 28 | ) 29 | 30 | # configure the GUI window 31 | window 32 | ( 33 | size [ 1200 700 ] # in pixels 34 | scale 10.0 # pixels per meter 35 | center [ -0.079 0.522 ] 36 | rotate [ 0.000 0.000 ] 37 | 38 | show_data 1 # 1=on 0=off 39 | ) 40 | 41 | # load an environment bitmap 42 | floorplan 43 | ( 44 | name "line" 45 | size [190.000 90.000 0.800] 46 | pose [0 0 0 0] 47 | bitmap "bitmaps/straden.png" 48 | gui_move 0 49 | ) 50 | 51 | # define a block 52 | define my_block model 53 | ( 54 | size [1 1 1] 55 | gui_nose 0 56 | ) 57 | 58 | # throw in a block 59 | my_block( pose [ 5 4 0 180.000 ] color "green") 60 | 61 | # throw in a block 62 | my_block( pose [ 5 7 0 180.000 ] color "blue") 63 | 64 | # throw in a robot 65 | pioneer2dx_with_laser 66 | ( 67 | # can refer to the robot by this name 68 | name "robot_0" 69 | color "red" 70 | pose [ 0 -32 0 180 ] 71 | ) 72 | -------------------------------------------------------------------------------- /world/willow-four-erratics-multisensor.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.5 0.5 0.5] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range [ 0.0 30.0 ] 11 | fov 270.25 12 | samples 1081 13 | ) 14 | 15 | # generic model properties 16 | color "black" 17 | size [ 0.05 0.05 0.1 ] 18 | ) 19 | 20 | define mycamera camera 21 | ( 22 | range [ 0.2 8.0 ] 23 | resolution [ 100 100 ] 24 | fov [ 70 40 ] 25 | pantilt [ 0 0 ] 26 | alwayson 1 27 | ) 28 | 29 | define erratic position 30 | ( 31 | #size [0.415 0.392 0.25] 32 | size [0.35 0.35 0.25] 33 | origin [-0.05 0 0 0] 34 | gui_nose 1 35 | drive "diff" 36 | topurg(pose [ 0.050 0.000 0 0.000 ]) 37 | topurg(pose [ -0.050 0.000 0 180.000 ]) 38 | mycamera(pose [ 0 0 0 90.0 ]) 39 | mycamera(pose [ 0 0 0 -90.0 ]) 40 | ) 41 | 42 | define floorplan model 43 | ( 44 | # sombre, sensible, artistic 45 | color "gray30" 46 | 47 | # most maps will need a bounding box 48 | boundary 1 49 | 50 | gui_nose 0 51 | gui_grid 0 52 | 53 | gui_outline 0 54 | gripper_return 0 55 | fiducial_return 0 56 | laser_return 1 57 | ) 58 | 59 | # set the resolution of the underlying raytrace model in meters 60 | resolution 0.02 61 | 62 | interval_sim 100 # simulation timestep in milliseconds 63 | 64 | 65 | window 66 | ( 67 | size [ 745.000 448.000 ] 68 | 69 | rotate [ 0.000 -1.560 ] 70 | scale 28.806 71 | ) 72 | 73 | # load an environment bitmap 74 | floorplan 75 | ( 76 | name "willow" 77 | bitmap "bitmaps/willow-full.pgm" 78 | size [54.0 58.7 0.5] 79 | pose [ -29.350 27.000 0 90.000 ] 80 | ) 81 | 82 | # throw in two robots 83 | erratic( pose [ -11.277 23.266 0 180.000 ] name "era" color "blue") 84 | erratic( pose [ -13.277 22.266 0 180.000 ] name "era2" color "blue") 85 | erratic( pose [ -13.277 23.266 0 180.000 ] name "era3" color "blue") 86 | erratic( pose [ -11.277 22.266 0 180.000 ] name "era4" color "blue") 87 | block( pose [ -13.924 25.020 0 180.000 ] color "red") 88 | --------------------------------------------------------------------------------