├── .gitignore ├── README.md ├── ros2webots_rosbot ├── CMakeLists.txt ├── LICENSE ├── cpp_models │ ├── laser_env_mapper.cpp │ └── laser_env_mapper.hpp ├── data_scripts │ ├── bag_data_extractor_bagpy.ipynb │ ├── bag_data_extractor_rosbags.py │ └── baglog_process.m ├── include │ └── ros2webots_rosbot │ │ ├── rosbot_cpp_node_test.hpp │ │ └── rosbot_localization_node.hpp ├── launch │ ├── rosbot_control_launch.py │ └── rosbot_world_launch.py ├── package.xml ├── protos │ ├── Rosbot.proto │ └── icons │ │ └── Rosbot.png ├── resource │ ├── frames_2024-01-19_18.13.03.gv │ ├── frames_2024-01-19_18.13.03.pdf │ ├── rosBotComponent.urdf │ ├── rosbot.urdf │ ├── rosbot.xacro │ ├── rosbot_robot_localization.yaml │ └── sample.urdf ├── ros2webots_rosbot │ ├── __init__.py │ ├── module_to_import.py │ └── rosbot_robot_driver.py ├── simulink_models │ ├── ros2webots_go_to_point_controller_model_cg.sldd │ ├── ros2webots_go_to_point_controller_model_cg.slx │ ├── ros2webots_go_to_point_controller_model_cg_ert_rtw │ │ ├── ros2webots_go_to_point_controller_model_cg.cpp │ │ ├── ros2webots_go_to_point_controller_model_cg.h │ │ ├── ros2webots_go_to_point_controller_model_cg_private.h │ │ ├── ros2webots_go_to_point_controller_model_cg_types.h │ │ ├── rtGetInf.cpp │ │ ├── rtGetInf.h │ │ ├── rtGetNaN.cpp │ │ ├── rtGetNaN.h │ │ ├── rt_defines.h │ │ ├── rt_nonfinite.cpp │ │ ├── rt_nonfinite.h │ │ ├── rtmodel.h │ │ └── rtwtypes.h │ ├── ros2webots_localization_model.sldd │ ├── ros2webots_localization_model.slx │ ├── ros2webots_localization_model_cg.slx │ └── ros2webots_localization_model_cg_ert_rtw │ │ ├── ros2webots_localization_model_cg.cpp │ │ ├── ros2webots_localization_model_cg.h │ │ ├── ros2webots_localization_model_cg_private.h │ │ ├── ros2webots_localization_model_cg_types.h │ │ ├── rtGetInf.cpp │ │ ├── rtGetInf.h │ │ ├── rtGetNaN.cpp │ │ ├── rtGetNaN.h │ │ ├── rt_defines.h │ │ ├── rt_nonfinite.cpp │ │ ├── rt_nonfinite.h │ │ ├── rtmodel.h │ │ └── rtwtypes.h ├── src │ ├── rosbot_cpp_node_test.cpp │ ├── rosbot_env_mapper_node.cpp │ ├── rosbot_get_keyboard_command.cpp │ ├── rosbot_localization_node.cpp │ ├── rosbot_tf_publisher.cpp │ └── rosbot_way_point_tracker_node.cpp └── worlds │ ├── .world_roboticknowledge.jpg │ ├── .world_roboticknowledge.wbproj │ └── world_roboticknowledge.wbt └── ros2webots_turtlebot ├── LICENSE ├── launch ├── test_launch.py └── turtlebot_world_launch.py ├── package.xml ├── protos ├── TurtleBot3Burger.proto └── icons │ └── TurtleBot3Burger.png ├── resource ├── TurtleBot3Burger.urdf ├── ros2webots_turtlebot └── turtleBotComponent.urdf ├── ros2webots_turtlebot ├── __init__.py └── turtlebot_robot_driver.py ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── worlds ├── .world_roboticknowledge.jpg ├── .world_roboticknowledge.wbproj └── world_roboticknowledge.wbt /.gitignore: -------------------------------------------------------------------------------- 1 | install/ 2 | log/ 3 | build/ 4 | baglog/ 5 | .vscode/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # ros2webots 3 | 4 | Simulation on Webots, Development with ROS2! 5 | 6 | In ```ros2webots``` repo, we are creating simulated robots and environments in Webots, that are controlled by ROS2. The whole simulation (Webots world setup + ROS2-based software of the robot) is done in one ROS2 package. 7 | 8 | Currently the repo hosts two ROS2 packages: 9 | 10 | - ```ros2webots_rosbot```: A rosbot is used in this world. The future development and simulation feature improvement will be mainly done in this package, as rosbot has many sensors like Camera, Lidar, and IMU; Also, the ```ros2webots_rosbot``` package building is done by ```ament_cmake```, that makes it easier for C++ based development. 11 | ![alt world of rosbot](https://github.com/MJavadZallaghi/ros2webots/blob/main/ros2webots_rosbot/worlds/.world_roboticknowledge.jpg) 12 | - ```ros2webots_turtlebot```: The world has a turtlebot, and package has been build using ```ament_python``` for initial ROS2+Webots interface setup tests. 13 | 14 | 15 | 16 | ## Demo 17 | - ROSBot with a way point tracker 18 | 19 | ![alt ROSBot with a way point tracker](https://github.com/MJavadZallaghi/media_ros2webots/blob/main/media/Zallaghi_ros2webots_wayPointTracker_edit_0_edit_0.gif) 20 | 21 | - ROSbot localization node 22 | 23 | [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/x2v0zY_rH44/0.jpg)](https://www.youtube.com/watch?v=x2v0zY_rH44) 24 | 25 | - Controlling the rosbot by keyboard 26 | 27 | ![alt demo of robot controlled by keyboard](https://github.com/MJavadZallaghi/media_ros2webots/blob/main/media/ros2webots_rosbot_demo_keyboard_control.gif) 28 | 29 | 30 | ## Installation 31 | ### Install directly 32 | I assume you have installed Ubuntu 22.04 as OS or installed on Windows WSL2. 33 | 34 | 1. Install [ROS2 iron](https://docs.ros.org/en/iron/Installation.html) 35 | 2. Install [Webots](https://cyberbotics.com/doc/guide/installing-webots). If you are working on WSL2, then you can install Webots on windows as ```webots_ros2``` package can find your installation by a ```WEBOTS_HOME``` variable. Don't worry, eveything is explain the the reference of step 3. 36 | 3. Install [```webots_ros2```](https://docs.ros.org/en/iron/Tutorials/Advanced/Simulators/Webots/Simulation-Webots.html) package. 37 | 4. Clone the repo whereever you want: 38 | ``` 39 | git clone https://github.com/MJavadZallaghi/ros2webots.git 40 | ``` 41 | 5. change directory to the workspace of the cloned package: 42 | ``` 43 | cd ros2webots 44 | ``` 45 | 6. Source your ros2 installation: 46 | ``` 47 | source /opt/ros/iron/setup.bash 48 | ``` 49 | 7. Build the package. 50 | ``` 51 | colcon build 52 | ``` 53 | 8. Source ```ros2webots``` installed packages: 54 | ``` 55 | source install/setup.bash 56 | ``` 57 | 8. Check the installation: 58 | ``` 59 | ros2 pkg list |grep ros2webots 60 | ``` 61 | You must see this package: ```ros2webots_rosbot``` 62 | ### Install using Docker 63 | Docker image of the ros2webots package is under construction! 64 | 65 | ## Run the simulations 66 | - sim 1: rosbot controlled by a waypoint tracker 67 | 1. Run the rosbot wprld in terminal and see how the Football ball are being passed by the ROSBot: 68 | ``` 69 | ros2 launch ros2webots_rosbot rosbot_world_launch.py 70 | ``` 71 | 72 | 73 | ## Roadmap 74 | - [x] Visualized the rosbot using rviz2 of foxglove with it's frames, sensor data, etc. 75 | - [x] Implment IMU + Wheel based locallization fir the rosbot and visualize its path. 76 | - [ ] Implement KF for the IMU acceleration fussion with odometry data and increase the localization accuracy. 77 | - [ ] Develop a mapping node for the rosbot for environment mapping. 78 | - [x] Implement a go-to-point controller for the ROSbot 79 | 80 | 81 | ## Support and Feedback 82 | For any support and feedback, email MohammadJavadZallaghi@gmail.com or use [issues](https://github.com/MJavadZallaghi/ros2webots/issues) section of this repo. 83 | 84 | 85 | ## License 86 | [MIT](https://choosealicense.com/licenses/mit/) 87 | 88 | 89 | ## Authors 90 | - [@MJavadZallaghi](https://www.github.com/MJavadZallaghi) 91 | 92 | 93 | ## Acknowledgements 94 | - ```ros2webots``` is created based on the Webots and ROS2 interface, that is possible by using ```webots_ros2``` package.```webots_ros2``` is a package that provides the necessary interfaces to simulate a robot in the Webots open-source 3D robots simulator. (https://github.com/cyberbotics/webots_ros2) 95 | 96 | -------------------------------------------------------------------------------- /ros2webots_rosbot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ros2webots_rosbot) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(rclpy REQUIRED) 13 | find_package(geometry_msgs REQUIRED) 14 | find_package(nav_msgs REQUIRED) 15 | find_package(std_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(tf2_ros REQUIRED) 18 | find_package(urdf REQUIRED) 19 | find_package(ament_index_cpp REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | # Include Cpp "include" directory 25 | 26 | include_directories(include simulink_models/ros2webots_localization_model_cg_ert_rtw 27 | simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw) 28 | 29 | install(DIRECTORY 30 | launch 31 | DESTINATION share/${PROJECT_NAME}/ 32 | ) 33 | install(DIRECTORY 34 | worlds 35 | DESTINATION share/${PROJECT_NAME}/ 36 | ) 37 | install(DIRECTORY 38 | resource 39 | DESTINATION share/${PROJECT_NAME}/ 40 | ) 41 | 42 | # Create Cpp executable 43 | add_executable(rosbot_cpp_node_test src/rosbot_cpp_node_test.cpp) 44 | ament_target_dependencies(rosbot_cpp_node_test rclcpp std_msgs) 45 | 46 | add_executable(rosbot_get_keyboard_command src/rosbot_get_keyboard_command.cpp) 47 | ament_target_dependencies(rosbot_get_keyboard_command rclcpp geometry_msgs) 48 | 49 | add_executable(rosbot_tf_publisher src/rosbot_tf_publisher.cpp) 50 | ament_target_dependencies(rosbot_tf_publisher rclcpp tf2_ros urdf std_msgs geometry_msgs tf2_ros nav_msgs) 51 | 52 | add_executable(rosbot_localization_node 53 | src/rosbot_localization_node.cpp 54 | simulink_models/ros2webots_localization_model_cg_ert_rtw/ros2webots_localization_model_cg.cpp 55 | simulink_models/ros2webots_localization_model_cg_ert_rtw/rt_nonfinite.cpp 56 | simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetInf.cpp 57 | simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetNaN.cpp) 58 | ament_target_dependencies(rosbot_localization_node rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs) 59 | 60 | 61 | add_executable(rosbot_way_point_tracker_node 62 | src/rosbot_way_point_tracker_node.cpp 63 | simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/ros2webots_go_to_point_controller_model_cg.cpp 64 | simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rt_nonfinite.cpp 65 | simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetInf.cpp 66 | simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetNaN.cpp) 67 | ament_target_dependencies(rosbot_way_point_tracker_node rclcpp std_msgs geometry_msgs nav_msgs) 68 | 69 | # Install Cpp executables 70 | install(TARGETS 71 | rosbot_cpp_node_test 72 | rosbot_get_keyboard_command 73 | rosbot_tf_publisher 74 | rosbot_localization_node 75 | rosbot_way_point_tracker_node 76 | DESTINATION lib/${PROJECT_NAME} 77 | ) 78 | 79 | # Install Python modules 80 | ament_python_install_package(${PROJECT_NAME}) 81 | 82 | # Install Python executables 83 | install(PROGRAMS 84 | ${PROJECT_NAME}/rosbot_robot_driver.py 85 | DESTINATION lib/${PROJECT_NAME} 86 | ) 87 | 88 | if(BUILD_TESTING) 89 | find_package(ament_lint_auto REQUIRED) 90 | # the following line skips the linter which checks for copyrights 91 | # comment the line when a copyright and license is added to all source files 92 | set(ament_cmake_copyright_FOUND TRUE) 93 | # the following line skips cpplint (only works in a git repo) 94 | # comment the line when this package is in a git repo and when 95 | # a copyright and license is added to all source files 96 | set(ament_cmake_cpplint_FOUND TRUE) 97 | ament_lint_auto_find_test_dependencies() 98 | endif() 99 | 100 | ament_package() 101 | -------------------------------------------------------------------------------- /ros2webots_rosbot/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /ros2webots_rosbot/cpp_models/laser_env_mapper.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_env_mapper.hpp" 2 | 3 | LaserEnvMapper::LaserEnvMapper() { 4 | // Map object constructor 5 | } -------------------------------------------------------------------------------- /ros2webots_rosbot/cpp_models/laser_env_mapper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef LASER_ENV_MAPPER_H 2 | #define LASER_ENV_MAPPER_H 3 | #endif 4 | 5 | class LaserEnvMapper { 6 | private: 7 | public: 8 | LaserEnvMapper(); // Map constructor 9 | }; -------------------------------------------------------------------------------- /ros2webots_rosbot/data_scripts/bag_data_extractor_bagpy.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 4, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "ename": "ImportError", 10 | "evalue": "cannot import name 'Log' from 'rosgraph_msgs.msg' (/opt/ros/iron/lib/python3.10/site-packages/rosgraph_msgs/msg/__init__.py)", 11 | "output_type": "error", 12 | "traceback": [ 13 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", 14 | "\u001b[0;31mImportError\u001b[0m Traceback (most recent call last)", 15 | "Cell \u001b[0;32mIn[4], line 2\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;66;03m# import bagpy\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mbagpy\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m bagreader\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mpandas\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mpd\u001b[39;00m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mseaborn\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01msea\u001b[39;00m\n", 16 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/bagpy/__init__.py:6\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;66;03m# Initial Date: March 2, 2020\u001b[39;00m\n\u001b[1;32m 2\u001b[0m \u001b[38;5;66;03m# Author: Rahul Bhadani\u001b[39;00m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;66;03m# Copyright (c) Rahul Bhadani, Arizona Board of Regents\u001b[39;00m\n\u001b[1;32m 4\u001b[0m \u001b[38;5;66;03m# All rights reserved.\u001b[39;00m\n\u001b[0;32m----> 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbagreader\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m bagreader\n\u001b[1;32m 7\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbagreader\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m animate_timeseries\n\u001b[1;32m 8\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbagreader\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m create_fig\n", 17 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/bagpy/bagreader.py:43\u001b[0m\n\u001b[1;32m 40\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mcsv\u001b[39;00m\n\u001b[1;32m 41\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01minspect\u001b[39;00m\n\u001b[0;32m---> 43\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrosbag\u001b[39;00m\n\u001b[1;32m 44\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mstd_msgs\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmsg\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m String, Header\n\u001b[1;32m 45\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mgeometry_msgs\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmsg\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m Twist, Pose, PoseStamped\n", 18 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/rosbag/__init__.py:33\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;66;03m# Software License Agreement (BSD License)\u001b[39;00m\n\u001b[1;32m 2\u001b[0m \u001b[38;5;66;03m#\u001b[39;00m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;66;03m# Copyright (c) 2009, Willow Garage, Inc.\u001b[39;00m\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 30\u001b[0m \u001b[38;5;66;03m# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\u001b[39;00m\n\u001b[1;32m 31\u001b[0m \u001b[38;5;66;03m# POSSIBILITY OF SUCH DAMAGE.\u001b[39;00m\n\u001b[0;32m---> 33\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbag\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException\n\u001b[1;32m 35\u001b[0m \u001b[38;5;66;03m# Import rosbag main to be used by the rosbag executable\u001b[39;00m\n\u001b[1;32m 36\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrosbag_main\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m rosbagmain\n", 19 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/rosbag/bag.py:69\u001b[0m\n\u001b[1;32m 66\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mgenpy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmessage\u001b[39;00m\n\u001b[1;32m 68\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mroslib\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mnames\u001b[39;00m \u001b[38;5;66;03m# still needed for roslib.names.canonicalize_name()\u001b[39;00m\n\u001b[0;32m---> 69\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\n\u001b[1;32m 70\u001b[0m \u001b[38;5;28;01mtry\u001b[39;00m:\n\u001b[1;32m 71\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mroslz4\u001b[39;00m\n", 20 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/rospy/__init__.py:49\u001b[0m\n\u001b[1;32m 42\u001b[0m \u001b[38;5;66;03m# import symbols into rospy namespace\u001b[39;00m\n\u001b[1;32m 43\u001b[0m \u001b[38;5;66;03m# NOTE: there are much better ways to configure python module\u001b[39;00m\n\u001b[1;32m 44\u001b[0m \u001b[38;5;66;03m# dictionaries, but the rospy codebase isn't quite in shape for that\u001b[39;00m\n\u001b[1;32m 45\u001b[0m \u001b[38;5;66;03m# yet\u001b[39;00m\n\u001b[1;32m 47\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mstd_msgs\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmsg\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m Header\n\u001b[0;32m---> 49\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mclient\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m spin, myargv, init_node, \\\n\u001b[1;32m 50\u001b[0m get_published_topics, \\\n\u001b[1;32m 51\u001b[0m wait_for_message, \\\n\u001b[1;32m 52\u001b[0m get_master, \\\n\u001b[1;32m 53\u001b[0m on_shutdown, \\\n\u001b[1;32m 54\u001b[0m get_param, get_param_cached, get_param_names, set_param, delete_param, has_param, search_param,\\\n\u001b[1;32m 55\u001b[0m DEBUG, INFO, WARN, ERROR, FATAL\n\u001b[1;32m 56\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mtimer\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m sleep, Rate, Timer\n\u001b[1;32m 57\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mcore\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m is_shutdown, signal_shutdown, \\\n\u001b[1;32m 58\u001b[0m get_node_uri, get_ros_root, \\\n\u001b[1;32m 59\u001b[0m logdebug, logwarn, loginfo, logout, logerr, logfatal, \\\n\u001b[0;32m (...)\u001b[0m\n\u001b[1;32m 62\u001b[0m logdebug_once, logwarn_once, loginfo_once, logerr_once, logfatal_once, \\\n\u001b[1;32m 63\u001b[0m parse_rosrpc_uri\n", 21 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/rospy/client.py:54\u001b[0m\n\u001b[1;32m 50\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrosgraph\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mnames\u001b[39;00m\n\u001b[1;32m 52\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mroslib\u001b[39;00m\n\u001b[0;32m---> 54\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mcore\u001b[39;00m\n\u001b[1;32m 55\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mcore\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m logwarn, loginfo, logerr, logdebug\n\u001b[1;32m 56\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mexceptions\u001b[39;00m\n", 22 | "File \u001b[0;32m/usr/local/lib/python3.10/dist-packages/rospy/core.py:75\u001b[0m\n\u001b[1;32m 72\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mnames\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;241m*\u001b[39m\n\u001b[1;32m 73\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mimpl\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mvalidators\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m ParameterInvalid\n\u001b[0;32m---> 75\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mrosgraph_msgs\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mmsg\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m Log\n\u001b[1;32m 76\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mfunctools\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m partial\n\u001b[1;32m 78\u001b[0m _logger \u001b[38;5;241m=\u001b[39m logging\u001b[38;5;241m.\u001b[39mgetLogger(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mrospy.core\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", 23 | "\u001b[0;31mImportError\u001b[0m: cannot import name 'Log' from 'rosgraph_msgs.msg' (/opt/ros/iron/lib/python3.10/site-packages/rosgraph_msgs/msg/__init__.py)" 24 | ] 25 | } 26 | ], 27 | "source": [ 28 | "import bagpy\n", 29 | "from bagpy import bagreader\n", 30 | "import pandas as pd\n", 31 | "import seaborn as sea\n", 32 | "import matplotlib.pyplot as plt\n", 33 | "import numpy as np" 34 | ] 35 | } 36 | ], 37 | "metadata": { 38 | "kernelspec": { 39 | "display_name": "Python 3", 40 | "language": "python", 41 | "name": "python3" 42 | }, 43 | "language_info": { 44 | "codemirror_mode": { 45 | "name": "ipython", 46 | "version": 3 47 | }, 48 | "file_extension": ".py", 49 | "mimetype": "text/x-python", 50 | "name": "python", 51 | "nbconvert_exporter": "python", 52 | "pygments_lexer": "ipython3", 53 | "version": "3.10.12" 54 | } 55 | }, 56 | "nbformat": 4, 57 | "nbformat_minor": 2 58 | } 59 | -------------------------------------------------------------------------------- /ros2webots_rosbot/data_scripts/bag_data_extractor_rosbags.py: -------------------------------------------------------------------------------- 1 | from rosbags.rosbag2 import Reader 2 | from rosbags.serde import deserialize_cdr 3 | import time 4 | 5 | # create reader instance and open for reading 6 | with Reader('/home/mjavadzallaghi/ros2_ws/ros2webots/baglog/rosbot_data_20240125_225927.bag') as reader: 7 | # topic and msgtype information is available on .connections list 8 | for connection in reader.connections: 9 | print(connection.topic, connection.msgtype) 10 | 11 | # iterate over messages 12 | for connection, timestamp, rawdata in reader.messages(): 13 | # print(connection.topic, connection.msgtype) 14 | 15 | if connection.topic == '/webots/rosbot/imu': 16 | print(connection.topic, connection.msgtype) 17 | # print(timestamp) 18 | # print(rawdata) 19 | msg_data = deserialize_cdr(rawdata, connection.msgtype) 20 | print(msg_data, '\n\n\n') 21 | # print(msg_data[0], '\n') 22 | # print(msg_data.) 23 | # msg = deserialize_cdr(rawdata, connection.msgtype) 24 | # print(msg.header.frame_id) 25 | 26 | # # messages() accepts connection filters 27 | # connections = [x for x in reader.connections if x.topic == '/webots/rosbot/imu'] 28 | # for connection, timestamp, rawdata in reader.messages(connections=connections): 29 | # msg = deserialize_cdr(rawdata, connection.msgtype) 30 | # print(msg.header.frame_id) 31 | -------------------------------------------------------------------------------- /ros2webots_rosbot/data_scripts/baglog_process.m: -------------------------------------------------------------------------------- 1 | bag = rosbag("\\wsl.localhost\Ubuntu\home\mjavadzallaghi\ros2_ws\ros2webots\baglog\rosbot_data_20240125_225927.bag") 2 | bag.AvailableTopics; 3 | bag.MessageList; 4 | bagselect1 = select(bag,"Topic","/webots/rosbot/imu"); 5 | msgs = readMessages(bagselect1,"DataFormat","struct"); 6 | msgs{1}.AngularVelocity.X; -------------------------------------------------------------------------------- /ros2webots_rosbot/include/ros2webots_rosbot/rosbot_cpp_node_test.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/include/ros2webots_rosbot/rosbot_cpp_node_test.hpp -------------------------------------------------------------------------------- /ros2webots_rosbot/include/ros2webots_rosbot/rosbot_localization_node.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/include/ros2webots_rosbot/rosbot_localization_node.hpp -------------------------------------------------------------------------------- /ros2webots_rosbot/launch/rosbot_control_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | # start the node for getting velocity command for rosbot from keyboard 7 | Node( 8 | package='ros2webots_rosbot', 9 | namespace='rosbot_keyboard_node_get_velocity', 10 | executable='rosbot_get_keyboard_command', 11 | name='rosbot_keyboard_node_get_velocity', 12 | emulate_tty=True, 13 | ), 14 | ]) -------------------------------------------------------------------------------- /ros2webots_rosbot/launch/rosbot_world_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.substitutions.path_join_substitution import PathJoinSubstitution 4 | from webots_ros2_driver.webots_launcher import WebotsLauncher 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch.substitutions import LaunchConfiguration 7 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 8 | import launch 9 | from webots_ros2_driver.webots_controller import WebotsController 10 | import datetime 11 | 12 | def generate_launch_description(): 13 | 14 | package_dir = get_package_share_directory('ros2webots_rosbot') 15 | world = LaunchConfiguration('world') 16 | 17 | # start webots 18 | webots = WebotsLauncher ( 19 | world=PathJoinSubstitution([package_dir, 'worlds', world]), 20 | ros2_supervisor=True 21 | ) 22 | 23 | # start robot controller 24 | robot_description_path = PathJoinSubstitution([package_dir, 'resource', 'rosBotComponent.urdf']) 25 | rosBot_driver = WebotsController( 26 | robot_name='rosbot_robot', 27 | parameters=[{'robot_description':robot_description_path} 28 | ], 29 | respawn=True 30 | ) 31 | # RUN TF and URDF publisher node of the rosbot for robot visualization 32 | ros_tf_urdf_pub_node = Node( 33 | package='ros2webots_rosbot', 34 | namespace='rosbot_tf_urdf_pub_node', 35 | executable='rosbot_tf_publisher', 36 | name='rosbot_tf_urdf_pub_node', 37 | ) 38 | # Run imu-based localization node for the rosbot 39 | # rosbot_localization_configue_file = PathJoinSubstitution([package_dir, 'resource', 'rosbot_robot_localization.yaml']) 40 | # rosbot_localization_node = Node( 41 | # package='robot_localization', 42 | # executable='ekf_node', 43 | # name='ekf_filter_node', 44 | # output='screen', 45 | # arguments=[rosbot_localization_configue_file], 46 | # ) 47 | 48 | # Localization Node 49 | rosbo_localization_model_node = Node( 50 | package='ros2webots_rosbot', 51 | namespace='rosbot_localization_node_1', 52 | executable='rosbot_localization_node', 53 | name='rosbot_localization_node_1', 54 | ) 55 | 56 | rosbot_way_point_tracker_node = Node( 57 | package='ros2webots_rosbot', 58 | namespace='rosbot_way_point_tracker_node_1', 59 | executable='rosbot_way_point_tracker_node', 60 | name='rosbot_way_point_tracker_node_1', 61 | ) 62 | 63 | 64 | # rosbot data logging 65 | # Construct the bag file name with the timestamp 66 | current_time = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") 67 | bag_file_name = '/home/mjavadzallaghi/ros2_ws/ros2webots/baglog/' + f'rosbot_data_{current_time}' 68 | rosbot_logger_node = launch.actions.ExecuteProcess( 69 | 70 | cmd=['ros2', 'bag', 'record', '/tf', '/webots/rosbot/laser', '/webots/rosbot/imu', '/webots/rosbot/odometry', '/webots/rosbot/odom/wheels_encoder_data', '/webots/rosbot/command_vel', '-o', bag_file_name], 71 | output='screen', 72 | ) 73 | 74 | foxglove_beidge = launch.actions.ExecuteProcess( 75 | cmd=['ros2', 'launch', 'foxglove_bridge', 'foxglove_bridge_launch.xml'], 76 | output='screen', 77 | ) 78 | 79 | return LaunchDescription([ 80 | DeclareLaunchArgument( 81 | 'world', 82 | default_value='world_roboticknowledge.wbt', 83 | description='Choose one of the world files from `/ros2webots_rosbot/worlds/world` directory' 84 | ), 85 | webots, 86 | webots._supervisor, 87 | rosBot_driver, 88 | ros_tf_urdf_pub_node, 89 | # rosbot_localization_node, 90 | rosbo_localization_model_node, 91 | rosbot_way_point_tracker_node, 92 | rosbot_logger_node, 93 | foxglove_beidge, 94 | 95 | 96 | # This action will kill all nodes once the Webots simulation has exited 97 | launch.actions.RegisterEventHandler( 98 | event_handler=launch.event_handlers.OnProcessExit( 99 | target_action=webots, 100 | on_exit=[ 101 | launch.actions.EmitEvent(event=launch.events.Shutdown()) 102 | ], 103 | ) 104 | ) 105 | ]) -------------------------------------------------------------------------------- /ros2webots_rosbot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2webots_rosbot 5 | 0.0.1 6 | ros2webots_rosbot is a ros2 package that runs simulation of a Rosbot in Webots. 7 | Mohammad Javad Zallaghi 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclpy 14 | geometry_msgs 15 | nav_msgs 16 | std_msgs 17 | sensor_msgs 18 | tf2_ros 19 | urdf 20 | ament_index_cpp 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /ros2webots_rosbot/protos/icons/Rosbot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/protos/icons/Rosbot.png -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/frames_2024-01-19_18.13.03.gv: -------------------------------------------------------------------------------- 1 | digraph G { 2 | "base_link" -> "camera"[label=" Broadcaster: default_authority\nAverage rate: 2.25\nBuffer length: 4.0\nMost recent transform: 1705675383.139087\nOldest transform: 1705675379.13907\n"]; 3 | "base_link" -> "cover"[label=" Broadcaster: default_authority\nAverage rate: 2.25\nBuffer length: 4.0\nMost recent transform: 1705675383.139174\nOldest transform: 1705675379.139142\n"]; 4 | "base_link" -> "fl_range"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639182\nOldest transform: 1705675379.139156\n"]; 5 | "base_link" -> "fr_range"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639189\nOldest transform: 1705675379.139164\n"]; 6 | "base_link" -> "imu"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639196\nOldest transform: 1705675379.139172\n"]; 7 | "base_link" -> "laser"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639204\nOldest transform: 1705675379.139184\n"]; 8 | "base_link" -> "rl_range"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639215\nOldest transform: 1705675379.139191\n"]; 9 | "base_link" -> "rr_range"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639222\nOldest transform: 1705675379.139199\n"]; 10 | "camera" -> "camera depth"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639229\nOldest transform: 1705675379.139211\n"]; 11 | "camera" -> "camera rgb"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639239\nOldest transform: 1705675379.139222\n"]; 12 | "base_link" -> "front left wheel"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639252\nOldest transform: 1705675379.139232\n"]; 13 | "base_link" -> "front right wheel"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639265\nOldest transform: 1705675379.139297\n"]; 14 | "imu" -> "imu accelerometer"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639278\nOldest transform: 1705675379.139313\n"]; 15 | "imu" -> "imu compass"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639291\nOldest transform: 1705675379.139329\n"]; 16 | "imu" -> "imu gyro"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639364\nOldest transform: 1705675379.139343\n"]; 17 | "imu" -> "imu inertial_unit"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639378\nOldest transform: 1705675379.139354\n"]; 18 | "laser" -> "solid"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639392\nOldest transform: 1705675379.139365\n"]; 19 | "base_link" -> "rear left wheel"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639405\nOldest transform: 1705675379.139373\n"]; 20 | "base_link" -> "rear right wheel"[label=" Broadcaster: default_authority\nAverage rate: 2.286\nBuffer length: 3.5\nMost recent transform: 1705675382.639418\nOldest transform: 1705675379.139382\n"]; 21 | edge [style=invis]; 22 | subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; 23 | "Recorded at time: 1705675383.4285111"[ shape=plaintext ] ; 24 | }->"base_link"; 25 | } -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/frames_2024-01-19_18.13.03.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/resource/frames_2024-01-19_18.13.03.pdf -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/rosBotComponent.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | true 11 | 55 12 | /webots/rosbot/laser 13 | true 14 | LDS-01 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | true 23 | 55 24 | /webots/rosbot/compass 25 | true 26 | compass 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | True 35 | 36 | 37 | /webots/rosbot/imu 38 | 39 | 55 40 | 41 | 42 | True 43 | 44 | 45 | 46 | 47 | imu inertial_unit 48 | 49 | imu gyro 50 | 51 | imu accelerometer 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/rosbot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/rosbot_robot_localization.yaml: -------------------------------------------------------------------------------- 1 | ### ekf config file ### 2 | ekf_filter_node: 3 | ros__parameters: 4 | frequency: 30.0 5 | sensor_timeout: 0.1 6 | two_d_mode: false 7 | print_diagnostics: false 8 | publish_acceleration: false 9 | publish_tf: false 10 | reset_on_time_jump: true 11 | 12 | map_frame: map # Defaults to "map" if unspecified 13 | odom_frame: odom # Defaults to "odom" if unspecified 14 | base_link_frame: base_link # Defaults to "base_link" if unspecified 15 | world_frame: odom # Defaults to the value of odom_frame if unspecified 16 | 17 | 18 | # IMU configuration 19 | imu0: /webots/rosbot/imu 20 | imu0_config: [false, false, false, 21 | true, true, true, 22 | false, false, false, 23 | true, true, true, 24 | true, true, true] 25 | imu0_queue_size: 5 26 | imu0_nodelay: false 27 | imu0_differential: false 28 | imu0_relative: true 29 | imu0_remove_gravitational_acceleration: true -------------------------------------------------------------------------------- /ros2webots_rosbot/resource/sample.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | -------------------------------------------------------------------------------- /ros2webots_rosbot/ros2webots_rosbot/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/ros2webots_rosbot/__init__.py -------------------------------------------------------------------------------- /ros2webots_rosbot/ros2webots_rosbot/module_to_import.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/ros2webots_rosbot/module_to_import.py -------------------------------------------------------------------------------- /ros2webots_rosbot/ros2webots_rosbot/rosbot_robot_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.qos import QoSProfile 5 | import time 6 | 7 | from geometry_msgs.msg import Twist 8 | from sensor_msgs.msg import JointState 9 | 10 | 11 | HALF_DISTANCE_BETWEEN_WHEELS = 0.045 12 | WHEEL_RADIUS = 0.025 13 | 14 | class RosBotWebotsDriver(): 15 | def init(self, webots_node, properties): 16 | self.__robot = webots_node.robot 17 | 18 | self.__fl_motor = self.__robot.getDevice('fl_wheel_joint') 19 | self.__fr_motor = self.__robot.getDevice('fr_wheel_joint') 20 | self.__rl_motor = self.__robot.getDevice('rl_wheel_joint') 21 | self.__rr_motor = self.__robot.getDevice('rr_wheel_joint') 22 | 23 | self.__fl_motor.setPosition(float('inf')) 24 | self.__fl_motor.setVelocity(0) 25 | 26 | self.__fr_motor.setPosition(float('inf')) 27 | self.__fr_motor.setVelocity(0) 28 | 29 | self.__rl_motor.setPosition(float('inf')) 30 | self.__rl_motor.setVelocity(0) 31 | 32 | self.__rr_motor.setPosition(float('inf')) 33 | self.__rr_motor.setVelocity(0) 34 | 35 | self.__fl_sensor = self.__robot.getDevice('front left wheel motor sensor') 36 | self.__fr_sensor = self.__robot.getDevice('front right wheel motor sensor') 37 | self.__rl_sensor = self.__robot.getDevice('rear left wheel motor sensor') 38 | self.__rr_sensor = self.__robot.getDevice('rear right wheel motor sensor') 39 | 40 | self.__fl_sensor.enable(50) #50 in ms based on the doc 41 | self.__fr_sensor.enable(50) #50 in ms based on the doc 42 | self.__rl_sensor.enable(50) #50 in ms based on the doc 43 | self.__rr_sensor.enable(50) #50 in ms based on the doc 44 | 45 | self.__target_twist = Twist() 46 | 47 | # self.__fl_wheel_twist = Twist() 48 | # self.__fr_wheel_twist = Twist() 49 | # self.__rl_wheel_twist = Twist() 50 | # self.__rr_wheel_twist = Twist() 51 | 52 | self.__wheels_encoder_data = JointState() 53 | 54 | rclpy.init(args=None) 55 | 56 | self.__node = rclpy.create_node('webots_rosbot_driver_node') 57 | self.__node.create_subscription(Twist, '/webots/rosbot/command_vel', self.__cmd_vel_callback, 10) 58 | 59 | 60 | self.__publisher_wheels_encoder_data = self.__node.create_publisher(JointState, '/webots/rosbot/odom/wheels_encoder_data', QoSProfile(depth=10)) 61 | self.__node.create_timer(0.05, self.__publish_wheels_encoder_data) 62 | 63 | 64 | def __publish_wheels_encoder_data(self): 65 | current_time = time.time() 66 | self.__wheels_encoder_data.header.stamp.sec = int(current_time) 67 | self.__wheels_encoder_data.header.stamp.nanosec = int((current_time - int(current_time)) * 1e9) 68 | self.__wheels_encoder_data.position = [self.__fl_sensor.getValue(), self.__fr_sensor.getValue(), self.__rl_sensor.getValue(), self.__rr_sensor.getValue()] 69 | self.__publisher_wheels_encoder_data.publish(self.__wheels_encoder_data) 70 | 71 | 72 | def __cmd_vel_callback(self, twist): 73 | self.__target_twist = twist 74 | 75 | def step(self): 76 | rclpy.spin_once(self.__node, timeout_sec=0) 77 | 78 | forward_speed = self.__target_twist.linear.x 79 | angular_speed = self.__target_twist.angular.z 80 | 81 | command_motor_left = (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS 82 | command_motor_right = (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS 83 | 84 | self.__fl_motor.setVelocity(command_motor_left) 85 | self.__rl_motor.setVelocity(command_motor_left) 86 | self.__fr_motor.setVelocity(command_motor_right) 87 | self.__rr_motor.setVelocity(command_motor_right) -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg.sldd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg.sldd -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg.slx -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/ros2webots_go_to_point_controller_model_cg.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_go_to_point_controller_model_cg.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #include "ros2webots_go_to_point_controller_model_cg.h" 16 | #include "rtwtypes.h" 17 | #include 18 | #include "ros2webots_go_to_point_controller_model_cg_private.h" 19 | #include "rt_defines.h" 20 | 21 | extern "C" 22 | { 23 | 24 | #include "rt_nonfinite.h" 25 | 26 | } 27 | 28 | // Named constants for Chart: '/Chart' 29 | const uint8_T ros2webots_go_IN_controller_off{ 1U }; 30 | 31 | const uint8_T ros2webots_go__IN_controller_on{ 2U }; 32 | 33 | const real_T ros2webots_param_error_distance{ 0.05 }; 34 | 35 | // Exported data definition 36 | 37 | // Const memory section 38 | // Definition for custom storage class: Const 39 | const real_T laterl_PID_kd{ 0.0 }; // Referenced by: '/Derivative Gain' 40 | 41 | // Lateral PID controller gain: Kd 42 | const real_T laterl_PID_ki{ 0.3 }; // Referenced by: '/Integral Gain' 43 | 44 | // Lateral PID controller gain: Ki 45 | const real_T laterl_PID_kp{ 1. }; // Referenced by: '/Proportional Gain' 46 | 47 | // Lateral PID controller gain: Kp 48 | const real_T longitudinal_speed_gain{ 0.1 };// Referenced by: '/Gain' 49 | 50 | // The gain thay manipulate main state machine velocity command 51 | real_T rt_atan2d_snf(real_T u0, real_T u1) 52 | { 53 | real_T y; 54 | if (std::isnan(u0) || std::isnan(u1)) { 55 | y = (rtNaN); 56 | } else if (std::isinf(u0) && std::isinf(u1)) { 57 | int32_T tmp; 58 | int32_T tmp_0; 59 | if (u0 > 0.0) { 60 | tmp = 1; 61 | } else { 62 | tmp = -1; 63 | } 64 | 65 | if (u1 > 0.0) { 66 | tmp_0 = 1; 67 | } else { 68 | tmp_0 = -1; 69 | } 70 | 71 | y = std::atan2(static_cast(tmp), static_cast(tmp_0)); 72 | } else if (u1 == 0.0) { 73 | if (u0 > 0.0) { 74 | y = RT_PI / 2.0; 75 | } else if (u0 < 0.0) { 76 | y = -(RT_PI / 2.0); 77 | } else { 78 | y = 0.0; 79 | } 80 | } else { 81 | y = std::atan2(u0, u1); 82 | } 83 | 84 | return y; 85 | } 86 | 87 | // Model step function 88 | void ros2webots_go_to_point_controller_model_cg::step() 89 | { 90 | real_T rtb_MatrixMultiply_tmp; 91 | real_T rtb_MatrixMultiply_tmp_0; 92 | real_T rtb_VectorConcatenate_idx_0; 93 | real_T rtb_VectorConcatenate_idx_1; 94 | real_T rtb_error_heading; 95 | uint8_T rtb_command_vel; 96 | 97 | // Sum: '/Subtract' incorporates: 98 | // Inport: '/x_act' 99 | // Inport: '/x_des' 100 | 101 | rtb_VectorConcatenate_idx_0 = ros2webots_go_to_point_contro_U.x_des - 102 | ros2webots_go_to_point_contro_U.x_act; 103 | 104 | // Sum: '/Subtract1' incorporates: 105 | // Inport: '/y_act' 106 | // Inport: '/y_des' 107 | 108 | rtb_VectorConcatenate_idx_1 = ros2webots_go_to_point_contro_U.y_des - 109 | ros2webots_go_to_point_contro_U.y_act; 110 | 111 | // MATLAB Function: '/Rotation matrix: 2D, Z' incorporates: 112 | // Inport: '/yaw_act' 113 | 114 | rtb_MatrixMultiply_tmp = std::sin(ros2webots_go_to_point_contro_U.yaw_act); 115 | rtb_MatrixMultiply_tmp_0 = std::cos(ros2webots_go_to_point_contro_U.yaw_act); 116 | 117 | // Product: '/MatrixMultiply' incorporates: 118 | // MATLAB Function: '/Rotation matrix: 2D, Z' 119 | 120 | rtb_error_heading = rtb_MatrixMultiply_tmp_0 * rtb_VectorConcatenate_idx_0 + 121 | rtb_MatrixMultiply_tmp * rtb_VectorConcatenate_idx_1; 122 | rtb_VectorConcatenate_idx_1 = -rtb_MatrixMultiply_tmp * 123 | rtb_VectorConcatenate_idx_0 + rtb_MatrixMultiply_tmp_0 * 124 | rtb_VectorConcatenate_idx_1; 125 | 126 | // MATLAB Function: '/Error calculator' incorporates: 127 | // Product: '/MatrixMultiply' 128 | 129 | rtb_VectorConcatenate_idx_0 = std::sqrt(rtb_error_heading * rtb_error_heading 130 | + rtb_VectorConcatenate_idx_1 * rtb_VectorConcatenate_idx_1); 131 | rtb_error_heading = rt_atan2d_snf(rtb_VectorConcatenate_idx_1, 132 | rtb_error_heading); 133 | 134 | // Chart: '/Chart' 135 | if (ros2webots_go_to_point_contr_DW.is_active_c8_ros2webots_go_to_p == 0U) { 136 | ros2webots_go_to_point_contr_DW.is_active_c8_ros2webots_go_to_p = 1U; 137 | ros2webots_go_to_point_contr_DW.is_c8_ros2webots_go_to_point_co = 138 | ros2webots_go_IN_controller_off; 139 | 140 | // Outport: '/controller_state' 141 | ros2webots_go_to_point_contro_Y.controller_state = 0U; 142 | rtb_command_vel = 0U; 143 | } else if (ros2webots_go_to_point_contr_DW.is_c8_ros2webots_go_to_point_co == 144 | ros2webots_go_IN_controller_off) { 145 | if (rtb_VectorConcatenate_idx_0 >= ros2webots_param_error_distance) { 146 | ros2webots_go_to_point_contr_DW.is_c8_ros2webots_go_to_point_co = 147 | ros2webots_go__IN_controller_on; 148 | 149 | // Outport: '/controller_state' 150 | ros2webots_go_to_point_contro_Y.controller_state = 1U; 151 | rtb_command_vel = 1U; 152 | } else { 153 | // Outport: '/controller_state' 154 | ros2webots_go_to_point_contro_Y.controller_state = 0U; 155 | rtb_command_vel = 0U; 156 | } 157 | 158 | // case IN_controller_on: 159 | } else if (rtb_VectorConcatenate_idx_0 < 0.21000000000000002) { 160 | ros2webots_go_to_point_contr_DW.is_c8_ros2webots_go_to_point_co = 161 | ros2webots_go_IN_controller_off; 162 | 163 | // Outport: '/controller_state' 164 | ros2webots_go_to_point_contro_Y.controller_state = 0U; 165 | rtb_command_vel = 0U; 166 | } else { 167 | // Outport: '/controller_state' 168 | ros2webots_go_to_point_contro_Y.controller_state = 1U; 169 | rtb_command_vel = 1U; 170 | } 171 | 172 | // End of Chart: '/Chart' 173 | 174 | // Outport: '/command_velocity' incorporates: 175 | // DataTypeConversion: '/Data Type Conversion' 176 | // Gain: '/Gain' 177 | 178 | ros2webots_go_to_point_contro_Y.command_velocity = longitudinal_speed_gain * 179 | static_cast(rtb_command_vel); 180 | 181 | // Gain: '/Filter Coefficient' incorporates: 182 | // DiscreteIntegrator: '/Filter' 183 | // Gain: '/Derivative Gain' 184 | // Sum: '/SumD' 185 | 186 | rtb_VectorConcatenate_idx_0 = (laterl_PID_kd * rtb_error_heading - 187 | ros2webots_go_to_point_contr_DW.Filter_DSTATE) * 100.0; 188 | 189 | // Sum: '/Sum' incorporates: 190 | // DiscreteIntegrator: '/Integrator' 191 | // Gain: '/Proportional Gain' 192 | 193 | ros2webots_go_to_point_contro_Y.command_steering_angle = (laterl_PID_kp * 194 | rtb_error_heading + ros2webots_go_to_point_contr_DW.Integrator_DSTATE) + 195 | rtb_VectorConcatenate_idx_0; 196 | 197 | // Saturate: '/Saturation' 198 | if (ros2webots_go_to_point_contro_Y.command_steering_angle > 199 | (2*0.78539816339744828)) { 200 | // Sum: '/Sum' incorporates: 201 | // Outport: '/command_steering_angle' 202 | 203 | ros2webots_go_to_point_contro_Y.command_steering_angle = 2*0.78539816339744828; 204 | } else if (ros2webots_go_to_point_contro_Y.command_steering_angle < 205 | (2*-0.78539816339744828)) { 206 | // Sum: '/Sum' incorporates: 207 | // Outport: '/command_steering_angle' 208 | 209 | ros2webots_go_to_point_contro_Y.command_steering_angle = 210 | 2*-0.78539816339744828; 211 | } 212 | 213 | // End of Saturate: '/Saturation' 214 | 215 | // Update for DiscreteIntegrator: '/Integrator' incorporates: 216 | // Gain: '/Integral Gain' 217 | 218 | ros2webots_go_to_point_contr_DW.Integrator_DSTATE += laterl_PID_ki * 219 | rtb_error_heading * 0.05; 220 | 221 | // Update for DiscreteIntegrator: '/Filter' 222 | ros2webots_go_to_point_contr_DW.Filter_DSTATE += 0.05 * 223 | rtb_VectorConcatenate_idx_0; 224 | } 225 | 226 | // Model initialize function 227 | void ros2webots_go_to_point_controller_model_cg::initialize() 228 | { 229 | // Registration code 230 | 231 | // initialize non-finites 232 | rt_InitInfAndNaN(sizeof(real_T)); 233 | } 234 | 235 | // Model terminate function 236 | void ros2webots_go_to_point_controller_model_cg::terminate() 237 | { 238 | // (no terminate code required) 239 | } 240 | 241 | // Constructor 242 | ros2webots_go_to_point_controller_model_cg:: 243 | ros2webots_go_to_point_controller_model_cg() : 244 | ros2webots_go_to_point_contro_U(), 245 | ros2webots_go_to_point_contro_Y(), 246 | ros2webots_go_to_point_contr_DW(), 247 | ros2webots_go_to_point_contr_M() 248 | { 249 | // Currently there is no constructor body generated. 250 | } 251 | 252 | // Destructor 253 | // Currently there is no destructor body generated. 254 | ros2webots_go_to_point_controller_model_cg:: 255 | ~ros2webots_go_to_point_controller_model_cg() = default; 256 | 257 | // Real-Time Model get method 258 | ros2webots_go_to_point_controller_model_cg::RT_MODEL_ros2webots_go_to_poi_T 259 | * ros2webots_go_to_point_controller_model_cg::getRTM() 260 | { 261 | return (&ros2webots_go_to_point_contr_M); 262 | } 263 | 264 | // 265 | // File trailer for generated code. 266 | // 267 | // [EOF] 268 | // 269 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/ros2webots_go_to_point_controller_model_cg_private.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_go_to_point_controller_model_cg_private.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_ros2webots_go_to_point_controller_model_cg_private_h_ 16 | #define RTW_HEADER_ros2webots_go_to_point_controller_model_cg_private_h_ 17 | #include "rtwtypes.h" 18 | #include "ros2webots_go_to_point_controller_model_cg_types.h" 19 | 20 | extern real_T rt_atan2d_snf(real_T u0, real_T u1); 21 | 22 | #endif // RTW_HEADER_ros2webots_go_to_point_controller_model_cg_private_h_ 23 | 24 | // 25 | // File trailer for generated code. 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/ros2webots_go_to_point_controller_model_cg_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_go_to_point_controller_model_cg_types.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_ros2webots_go_to_point_controller_model_cg_types_h_ 16 | #define RTW_HEADER_ros2webots_go_to_point_controller_model_cg_types_h_ 17 | #endif // RTW_HEADER_ros2webots_go_to_point_controller_model_cg_types_h_ 18 | 19 | // 20 | // File trailer for generated code. 21 | // 22 | // [EOF] 23 | // 24 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetInf.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetInf.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #include "rtwtypes.h" 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rtGetInf.h" 22 | 23 | } 24 | 25 | #include 26 | 27 | extern "C" 28 | { 29 | 30 | #include "rt_nonfinite.h" 31 | 32 | } 33 | 34 | #define NumBitsPerChar 8U 35 | 36 | extern "C" 37 | { 38 | // 39 | // Initialize rtInf needed by the generated code. 40 | // Inf is initialized as non-signaling. Assumes IEEE. 41 | // 42 | real_T rtGetInf(void) 43 | { 44 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 45 | 46 | real_T inf{ 0.0 }; 47 | 48 | if (bitsPerReal == 32U) { 49 | inf = rtGetInfF(); 50 | } else { 51 | union { 52 | LittleEndianIEEEDouble bitVal; 53 | real_T fltVal; 54 | } tmpVal; 55 | 56 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 57 | tmpVal.bitVal.words.wordL = 0x00000000U; 58 | inf = tmpVal.fltVal; 59 | } 60 | 61 | return inf; 62 | } 63 | 64 | // 65 | // Initialize rtInfF needed by the generated code. 66 | // Inf is initialized as non-signaling. Assumes IEEE. 67 | // 68 | real32_T rtGetInfF(void) 69 | { 70 | IEEESingle infF; 71 | infF.wordL.wordLuint = 0x7F800000U; 72 | return infF.wordL.wordLreal; 73 | } 74 | 75 | // 76 | // Initialize rtMinusInf needed by the generated code. 77 | // Inf is initialized as non-signaling. Assumes IEEE. 78 | // 79 | real_T rtGetMinusInf(void) 80 | { 81 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 82 | 83 | real_T minf{ 0.0 }; 84 | 85 | if (bitsPerReal == 32U) { 86 | minf = rtGetMinusInfF(); 87 | } else { 88 | union { 89 | LittleEndianIEEEDouble bitVal; 90 | real_T fltVal; 91 | } tmpVal; 92 | 93 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 94 | tmpVal.bitVal.words.wordL = 0x00000000U; 95 | minf = tmpVal.fltVal; 96 | } 97 | 98 | return minf; 99 | } 100 | 101 | // 102 | // Initialize rtMinusInfF needed by the generated code. 103 | // Inf is initialized as non-signaling. Assumes IEEE. 104 | // 105 | real32_T rtGetMinusInfF(void) 106 | { 107 | IEEESingle minfF; 108 | minfF.wordL.wordLuint = 0xFF800000U; 109 | return minfF.wordL.wordLreal; 110 | } 111 | } 112 | 113 | // 114 | // File trailer for generated code. 115 | // 116 | // [EOF] 117 | // 118 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetInf.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetInf.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtGetInf_h_ 16 | #define RTW_HEADER_rtGetInf_h_ 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rt_nonfinite.h" 22 | 23 | } 24 | 25 | #include "rtwtypes.h" 26 | #ifdef __cplusplus 27 | 28 | extern "C" 29 | { 30 | 31 | #endif 32 | 33 | extern real_T rtGetInf(void); 34 | extern real32_T rtGetInfF(void); 35 | extern real_T rtGetMinusInf(void); 36 | extern real32_T rtGetMinusInfF(void); 37 | 38 | #ifdef __cplusplus 39 | 40 | } // extern "C" 41 | 42 | #endif 43 | #endif // RTW_HEADER_rtGetInf_h_ 44 | 45 | // 46 | // File trailer for generated code. 47 | // 48 | // [EOF] 49 | // 50 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetNaN.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetNaN.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #include "rtwtypes.h" 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rtGetNaN.h" 22 | 23 | } 24 | 25 | #include 26 | 27 | extern "C" 28 | { 29 | 30 | #include "rt_nonfinite.h" 31 | 32 | } 33 | 34 | #define NumBitsPerChar 8U 35 | 36 | extern "C" 37 | { 38 | // 39 | // Initialize rtNaN needed by the generated code. 40 | // NaN is initialized as non-signaling. Assumes IEEE. 41 | // 42 | real_T rtGetNaN(void) 43 | { 44 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 45 | 46 | real_T nan{ 0.0 }; 47 | 48 | if (bitsPerReal == 32U) { 49 | nan = rtGetNaNF(); 50 | } else { 51 | union { 52 | LittleEndianIEEEDouble bitVal; 53 | real_T fltVal; 54 | } tmpVal; 55 | 56 | tmpVal.bitVal.words.wordH = 0xFFF80000U; 57 | tmpVal.bitVal.words.wordL = 0x00000000U; 58 | nan = tmpVal.fltVal; 59 | } 60 | 61 | return nan; 62 | } 63 | 64 | // 65 | // Initialize rtNaNF needed by the generated code. 66 | // NaN is initialized as non-signaling. Assumes IEEE. 67 | // 68 | real32_T rtGetNaNF(void) 69 | { 70 | IEEESingle nanF{ { 0.0F } }; 71 | 72 | nanF.wordL.wordLuint = 0xFFC00000U; 73 | return nanF.wordL.wordLreal; 74 | } 75 | } 76 | 77 | // 78 | // File trailer for generated code. 79 | // 80 | // [EOF] 81 | // 82 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetNaN.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtGetNaN_h_ 16 | #define RTW_HEADER_rtGetNaN_h_ 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rt_nonfinite.h" 22 | 23 | } 24 | 25 | #include "rtwtypes.h" 26 | #ifdef __cplusplus 27 | 28 | extern "C" 29 | { 30 | 31 | #endif 32 | 33 | extern real_T rtGetNaN(void); 34 | extern real32_T rtGetNaNF(void); 35 | 36 | #ifdef __cplusplus 37 | 38 | } // extern "C" 39 | 40 | #endif 41 | #endif // RTW_HEADER_rtGetNaN_h_ 42 | 43 | // 44 | // File trailer for generated code. 45 | // 46 | // [EOF] 47 | // 48 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rt_defines.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_defines.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rt_defines_h_ 16 | #define RTW_HEADER_rt_defines_h_ 17 | 18 | //===========* 19 | // Constants * 20 | // =========== 21 | #define RT_PI 3.14159265358979323846 22 | #define RT_PIF 3.1415927F 23 | #define RT_LN_10 2.30258509299404568402 24 | #define RT_LN_10F 2.3025851F 25 | #define RT_LOG10E 0.43429448190325182765 26 | #define RT_LOG10EF 0.43429449F 27 | #define RT_E 2.7182818284590452354 28 | #define RT_EF 2.7182817F 29 | 30 | // 31 | // UNUSED_PARAMETER(x) 32 | // Used to specify that a function parameter (argument) is required but not 33 | // accessed by the function body. 34 | 35 | #ifndef UNUSED_PARAMETER 36 | #if defined(__LCC__) 37 | #define UNUSED_PARAMETER(x) // do nothing 38 | #else 39 | 40 | // 41 | // This is the semi-ANSI standard way of indicating that an 42 | // unused function parameter is required. 43 | 44 | #define UNUSED_PARAMETER(x) (void) (x) 45 | #endif 46 | #endif 47 | #endif // RTW_HEADER_rt_defines_h_ 48 | 49 | // 50 | // File trailer for generated code. 51 | // 52 | // [EOF] 53 | // 54 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rt_nonfinite.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_nonfinite.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | extern "C" 17 | { 18 | 19 | #include "rtGetNaN.h" 20 | 21 | } 22 | 23 | extern "C" 24 | { 25 | 26 | #include "rtGetInf.h" 27 | 28 | } 29 | 30 | #include 31 | #include "rtwtypes.h" 32 | 33 | extern "C" 34 | { 35 | 36 | #include "rt_nonfinite.h" 37 | 38 | } 39 | 40 | #define NumBitsPerChar 8U 41 | 42 | extern "C" 43 | { 44 | real_T rtInf; 45 | real_T rtMinusInf; 46 | real_T rtNaN; 47 | real32_T rtInfF; 48 | real32_T rtMinusInfF; 49 | real32_T rtNaNF; 50 | } 51 | 52 | extern "C" 53 | { 54 | // 55 | // Initialize the rtInf, rtMinusInf, and rtNaN needed by the 56 | // generated code. NaN is initialized as non-signaling. Assumes IEEE. 57 | // 58 | void rt_InitInfAndNaN(size_t realSize) 59 | { 60 | (void) (realSize); 61 | rtNaN = rtGetNaN(); 62 | rtNaNF = rtGetNaNF(); 63 | rtInf = rtGetInf(); 64 | rtInfF = rtGetInfF(); 65 | rtMinusInf = rtGetMinusInf(); 66 | rtMinusInfF = rtGetMinusInfF(); 67 | } 68 | 69 | // Test if value is infinite 70 | boolean_T rtIsInf(real_T value) 71 | { 72 | return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); 73 | } 74 | 75 | // Test if single-precision value is infinite 76 | boolean_T rtIsInfF(real32_T value) 77 | { 78 | return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); 79 | } 80 | 81 | // Test if value is not a number 82 | boolean_T rtIsNaN(real_T value) 83 | { 84 | boolean_T result{ (boolean_T) 0 }; 85 | 86 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 87 | 88 | if (bitsPerReal == 32U) { 89 | result = rtIsNaNF((real32_T)value); 90 | } else { 91 | union { 92 | LittleEndianIEEEDouble bitVal; 93 | real_T fltVal; 94 | } tmpVal; 95 | 96 | tmpVal.fltVal = value; 97 | result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 98 | 0x7FF00000 && 99 | ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || 100 | (tmpVal.bitVal.words.wordL != 0) )); 101 | } 102 | 103 | return result; 104 | } 105 | 106 | // Test if single-precision value is not a number 107 | boolean_T rtIsNaNF(real32_T value) 108 | { 109 | IEEESingle tmp; 110 | tmp.wordL.wordLreal = value; 111 | return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && 112 | (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); 113 | } 114 | } 115 | 116 | // 117 | // File trailer for generated code. 118 | // 119 | // [EOF] 120 | // 121 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_nonfinite.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rt_nonfinite_h_ 16 | #define RTW_HEADER_rt_nonfinite_h_ 17 | #include 18 | #include "rtwtypes.h" 19 | #define NOT_USING_NONFINITE_LITERALS 1 20 | #ifdef __cplusplus 21 | 22 | extern "C" 23 | { 24 | 25 | #endif 26 | 27 | extern real_T rtInf; 28 | extern real_T rtMinusInf; 29 | extern real_T rtNaN; 30 | extern real32_T rtInfF; 31 | extern real32_T rtMinusInfF; 32 | extern real32_T rtNaNF; 33 | extern void rt_InitInfAndNaN(size_t realSize); 34 | extern boolean_T rtIsInf(real_T value); 35 | extern boolean_T rtIsInfF(real32_T value); 36 | extern boolean_T rtIsNaN(real_T value); 37 | extern boolean_T rtIsNaNF(real32_T value); 38 | struct BigEndianIEEEDouble { 39 | struct { 40 | uint32_T wordH; 41 | uint32_T wordL; 42 | } words; 43 | }; 44 | 45 | struct LittleEndianIEEEDouble { 46 | struct { 47 | uint32_T wordL; 48 | uint32_T wordH; 49 | } words; 50 | }; 51 | 52 | struct IEEESingle { 53 | union { 54 | real32_T wordLreal; 55 | uint32_T wordLuint; 56 | } wordL; 57 | }; 58 | 59 | #ifdef __cplusplus 60 | 61 | } // extern "C" 62 | 63 | #endif 64 | #endif // RTW_HEADER_rt_nonfinite_h_ 65 | 66 | // 67 | // File trailer for generated code. 68 | // 69 | // [EOF] 70 | // 71 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtmodel.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtmodel.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtmodel_h_ 16 | #define RTW_HEADER_rtmodel_h_ 17 | #include "ros2webots_go_to_point_controller_model_cg.h" 18 | #define MODEL_CLASSNAME ros2webots_go_to_point_controller_model_cg 19 | #define MODEL_STEPNAME step 20 | 21 | // 22 | // ROOT_IO_FORMAT: 0 (Individual arguments) 23 | // ROOT_IO_FORMAT: 1 (Structure reference) 24 | // ROOT_IO_FORMAT: 2 (Part of model data structure) 25 | 26 | #define ROOT_IO_FORMAT 1 27 | 28 | // Macros generated for backwards compatibility 29 | #ifndef rtmGetStopRequested 30 | #define rtmGetStopRequested(rtm) ((void*) 0) 31 | #endif 32 | #endif // RTW_HEADER_rtmodel_h_ 33 | 34 | // 35 | // File trailer for generated code. 36 | // 37 | // [EOF] 38 | // 39 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_go_to_point_controller_model_cg_ert_rtw/rtwtypes.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtwtypes.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_go_to_point_controller_model_cg'. 5 | // 6 | // Model version : 1.347 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sat Mar 2 16:57:53 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #ifndef RTWTYPES_H 17 | #define RTWTYPES_H 18 | 19 | // Logical type definitions 20 | #if (!defined(__cplusplus)) 21 | #ifndef false 22 | #define false (0U) 23 | #endif 24 | 25 | #ifndef true 26 | #define true (1U) 27 | #endif 28 | #endif 29 | 30 | //=======================================================================* 31 | // Target hardware information 32 | // Device type: Intel->x86-64 (Windows64) 33 | // Number of bits: char: 8 short: 16 int: 32 34 | // long: 32 35 | // native word size: 64 36 | // Byte ordering: LittleEndian 37 | // Signed integer division rounds to: Zero 38 | // Shift right on a signed integer as arithmetic shift: on 39 | // ======================================================================= 40 | 41 | //=======================================================================* 42 | // Fixed width word size data types: * 43 | // int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 44 | // uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 45 | // real32_T, real64_T - 32 and 64 bit floating point numbers * 46 | // ======================================================================= 47 | typedef signed char int8_T; 48 | typedef unsigned char uint8_T; 49 | typedef short int16_T; 50 | typedef unsigned short uint16_T; 51 | typedef int int32_T; 52 | typedef unsigned int uint32_T; 53 | typedef float real32_T; 54 | typedef double real64_T; 55 | 56 | //===========================================================================* 57 | // Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * 58 | // real_T, time_T, ulong_T. * 59 | // =========================================================================== 60 | typedef double real_T; 61 | typedef double time_T; 62 | typedef unsigned char boolean_T; 63 | typedef int int_T; 64 | typedef unsigned int uint_T; 65 | typedef unsigned long ulong_T; 66 | typedef char char_T; 67 | typedef unsigned char uchar_T; 68 | typedef char_T byte_T; 69 | 70 | //===========================================================================* 71 | // Complex number type definitions * 72 | // =========================================================================== 73 | #define CREAL_T 74 | 75 | typedef struct { 76 | real32_T re; 77 | real32_T im; 78 | } creal32_T; 79 | 80 | typedef struct { 81 | real64_T re; 82 | real64_T im; 83 | } creal64_T; 84 | 85 | typedef struct { 86 | real_T re; 87 | real_T im; 88 | } creal_T; 89 | 90 | #define CINT8_T 91 | 92 | typedef struct { 93 | int8_T re; 94 | int8_T im; 95 | } cint8_T; 96 | 97 | #define CUINT8_T 98 | 99 | typedef struct { 100 | uint8_T re; 101 | uint8_T im; 102 | } cuint8_T; 103 | 104 | #define CINT16_T 105 | 106 | typedef struct { 107 | int16_T re; 108 | int16_T im; 109 | } cint16_T; 110 | 111 | #define CUINT16_T 112 | 113 | typedef struct { 114 | uint16_T re; 115 | uint16_T im; 116 | } cuint16_T; 117 | 118 | #define CINT32_T 119 | 120 | typedef struct { 121 | int32_T re; 122 | int32_T im; 123 | } cint32_T; 124 | 125 | #define CUINT32_T 126 | 127 | typedef struct { 128 | uint32_T re; 129 | uint32_T im; 130 | } cuint32_T; 131 | 132 | //=======================================================================* 133 | // Min and Max: * 134 | // int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 135 | // uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 136 | // ======================================================================= 137 | #define MAX_int8_T ((int8_T)(127)) 138 | #define MIN_int8_T ((int8_T)(-128)) 139 | #define MAX_uint8_T ((uint8_T)(255U)) 140 | #define MAX_int16_T ((int16_T)(32767)) 141 | #define MIN_int16_T ((int16_T)(-32768)) 142 | #define MAX_uint16_T ((uint16_T)(65535U)) 143 | #define MAX_int32_T ((int32_T)(2147483647)) 144 | #define MIN_int32_T ((int32_T)(-2147483647-1)) 145 | #define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) 146 | 147 | // Block D-Work pointer type 148 | typedef void * pointer_T; 149 | 150 | #endif // RTWTYPES_H 151 | 152 | // 153 | // File trailer for generated code. 154 | // 155 | // [EOF] 156 | // 157 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model.sldd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/simulink_models/ros2webots_localization_model.sldd -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/simulink_models/ros2webots_localization_model.slx -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg.slx -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/ros2webots_localization_model_cg.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_localization_model_cg.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_ros2webots_localization_model_cg_h_ 16 | #define RTW_HEADER_ros2webots_localization_model_cg_h_ 17 | #include "rtwtypes.h" 18 | #include "ros2webots_localization_model_cg_types.h" 19 | 20 | extern "C" 21 | { 22 | 23 | #include "rtGetNaN.h" 24 | 25 | } 26 | 27 | extern "C" 28 | { 29 | 30 | #include "rt_nonfinite.h" 31 | 32 | } 33 | 34 | // Macros for accessing real-time model data structure 35 | #ifndef rtmGetErrorStatus 36 | #define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) 37 | #endif 38 | 39 | #ifndef rtmSetErrorStatus 40 | #define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) 41 | #endif 42 | 43 | #define ros2webots_localization_model_cg_M (ros2webots_localization_mode_M) 44 | 45 | // Exported data declaration 46 | 47 | // Const memory section 48 | // Declaration for custom storage class: Const 49 | extern const real_T rosbot_wheel_radius;// Referenced by: 50 | // '/Multiply' 51 | // '/Multiply' 52 | // '/Multiply' 53 | // '/Multiply' 54 | 55 | 56 | // ROSBot wheel radius 57 | extern const real_T wheel_based_localization_x_initial_coord;// Referenced by: '/Delay' 58 | 59 | // Initial x coordinate of localization using only wheel pose data 60 | extern const real_T wheel_based_localization_y_initial_coord;// Referenced by: '/Delay1' 61 | 62 | // Initial y coordinate of localization using only wheel pose data 63 | 64 | // Class declaration for model ros2webots_localization_model_cg 65 | class ros2webots_localization_model_cg final 66 | { 67 | // public data and function members 68 | public: 69 | // Block states (default storage) for system '' 70 | struct DW_ros2webots_localization_mo_T { 71 | real_T Delay_DSTATE; // '/Delay' 72 | real_T Delay_DSTATE_g; // '/Delay' 73 | real_T Delay_DSTATE_j; // '/Delay' 74 | real_T Delay_DSTATE_o; // '/Delay' 75 | real_T Delay_DSTATE_i; // '/Delay' 76 | real_T Delay1_DSTATE; // '/Delay1' 77 | }; 78 | 79 | // External inputs (root inport signals with default storage) 80 | struct ExtU_ros2webots_localization__T { 81 | real_T imu_orientation_vector[4]; // '/imu_orientation_vector' 82 | real_T imu_angular_velocity_vector[3];// '/imu_angular_velocity_vector' 83 | real_T imu_linear_acceleration_vector[3]; 84 | // '/imu_linear_acceleration_vector' 85 | real_T fl_wheel_pose; // '/fl_wheel_pose' 86 | real_T fr_wheel_pose; // '/fr_wheel_pose' 87 | real_T rl_wheel_pose; // '/rl_wheel_pose' 88 | real_T rr_wheel_pose; // '/rr_wheel_pose' 89 | real_T fl_wheel_pose_update_dt; // '/fl_wheel_pose_update_dt' 90 | real_T fr_wheel_pose_update_dt; // '/fr_wheel_pose_update_dt' 91 | real_T rl_wheel_pose_update_dt; // '/rl_wheel_pose_update_dt' 92 | real_T rr_wheel_pose_update_dt; // '/rr_wheel_pose_update_dt' 93 | }; 94 | 95 | // External outputs (root outports fed by signals with default storage) 96 | struct ExtY_ros2webots_localization__T { 97 | real_T odometry_vector[3]; // '/odometry_vector' 98 | }; 99 | 100 | // Real-time Model Data Structure 101 | struct RT_MODEL_ros2webots_localizat_T { 102 | const char_T * volatile errorStatus; 103 | }; 104 | 105 | // Copy Constructor 106 | ros2webots_localization_model_cg(ros2webots_localization_model_cg const&) = 107 | delete; 108 | 109 | // Assignment Operator 110 | ros2webots_localization_model_cg& operator= (ros2webots_localization_model_cg 111 | const&) & = delete; 112 | 113 | // Move Constructor 114 | ros2webots_localization_model_cg(ros2webots_localization_model_cg &&) = delete; 115 | 116 | // Move Assignment Operator 117 | ros2webots_localization_model_cg& operator= (ros2webots_localization_model_cg &&) 118 | = delete; 119 | 120 | // Real-Time Model get method 121 | ros2webots_localization_model_cg::RT_MODEL_ros2webots_localizat_T * getRTM(); 122 | 123 | // Root inports set method 124 | void setExternalInputs(const ExtU_ros2webots_localization__T 125 | *pExtU_ros2webots_localization__T) 126 | { 127 | ros2webots_localization_model_U = *pExtU_ros2webots_localization__T; 128 | } 129 | 130 | // Root outports get method 131 | const ExtY_ros2webots_localization__T &getExternalOutputs() const 132 | { 133 | return ros2webots_localization_model_Y; 134 | } 135 | 136 | // model initialize function 137 | void initialize(); 138 | 139 | // model step function 140 | void step(); 141 | 142 | // model terminate function 143 | static void terminate(); 144 | 145 | // Constructor 146 | ros2webots_localization_model_cg(); 147 | 148 | // Destructor 149 | ~ros2webots_localization_model_cg(); 150 | 151 | // private data and function members 152 | private: 153 | // External inputs 154 | ExtU_ros2webots_localization__T ros2webots_localization_model_U; 155 | 156 | // External outputs 157 | ExtY_ros2webots_localization__T ros2webots_localization_model_Y; 158 | 159 | // Block states 160 | DW_ros2webots_localization_mo_T ros2webots_localization_mode_DW; 161 | 162 | // private member function(s) for subsystem '/If Action Subsystem' 163 | static void ros2webots_lo_IfActionSubsystem(real_T rtu_In1, real_T 164 | rtu_dt_data_change, real_T *rty_Out1); 165 | 166 | // Real-Time Model 167 | RT_MODEL_ros2webots_localizat_T ros2webots_localization_mode_M; 168 | }; 169 | 170 | //- 171 | // These blocks were eliminated from the model due to optimizations: 172 | // 173 | // Block '/Scope' : Unused code path elimination 174 | // Block '/Scope' : Unused code path elimination 175 | // Block '/Scope' : Unused code path elimination 176 | // Block '/Scope' : Unused code path elimination 177 | // Block '/Scope' : Unused code path elimination 178 | 179 | 180 | //- 181 | // The generated code includes comments that allow you to trace directly 182 | // back to the appropriate location in the model. The basic format 183 | // is /block_name, where system is the system number (uniquely 184 | // assigned by Simulink) and block_name is the name of the block. 185 | // 186 | // Use the MATLAB hilite_system command to trace the generated code back 187 | // to the model. For example, 188 | // 189 | // hilite_system('') - opens system 3 190 | // hilite_system('/Kp') - opens and selects block Kp which resides in S3 191 | // 192 | // Here is the system hierarchy for this model 193 | // 194 | // '' : 'ros2webots_localization_model_cg' 195 | // '' : 'ros2webots_localization_model_cg/localization' 196 | // '' : 'ros2webots_localization_model_cg/localization/FL Wheel Velocity Estimator' 197 | // '' : 'ros2webots_localization_model_cg/localization/FR Wheel Velocity Estimator' 198 | // '' : 'ros2webots_localization_model_cg/localization/Left Side Velocity' 199 | // '' : 'ros2webots_localization_model_cg/localization/Middle Velocity' 200 | // '' : 'ros2webots_localization_model_cg/localization/RL Wheel Velocity Estimator' 201 | // '' : 'ros2webots_localization_model_cg/localization/RR Wheel Velocity Estimator' 202 | // '' : 'ros2webots_localization_model_cg/localization/Right Side Velocity' 203 | // '' : 'ros2webots_localization_model_cg/localization/Subsystem' 204 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator' 205 | // '' : 'ros2webots_localization_model_cg/localization/FL Wheel Velocity Estimator/fl_wheel_speed_calc' 206 | // '' : 'ros2webots_localization_model_cg/localization/FL Wheel Velocity Estimator/wheel rps 2 wheel mps' 207 | // '' : 'ros2webots_localization_model_cg/localization/FL Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem' 208 | // '' : 'ros2webots_localization_model_cg/localization/FL Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem1' 209 | // '' : 'ros2webots_localization_model_cg/localization/FR Wheel Velocity Estimator/fl_wheel_speed_calc' 210 | // '' : 'ros2webots_localization_model_cg/localization/FR Wheel Velocity Estimator/wheel rps 2 wheel mps' 211 | // '' : 'ros2webots_localization_model_cg/localization/FR Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem' 212 | // '' : 'ros2webots_localization_model_cg/localization/FR Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem1' 213 | // '' : 'ros2webots_localization_model_cg/localization/RL Wheel Velocity Estimator/fl_wheel_speed_calc' 214 | // '' : 'ros2webots_localization_model_cg/localization/RL Wheel Velocity Estimator/wheel rps 2 wheel mps' 215 | // '' : 'ros2webots_localization_model_cg/localization/RL Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem' 216 | // '' : 'ros2webots_localization_model_cg/localization/RL Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem1' 217 | // '' : 'ros2webots_localization_model_cg/localization/RR Wheel Velocity Estimator/fl_wheel_speed_calc' 218 | // '' : 'ros2webots_localization_model_cg/localization/RR Wheel Velocity Estimator/wheel rps 2 wheel mps' 219 | // '' : 'ros2webots_localization_model_cg/localization/RR Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem' 220 | // '' : 'ros2webots_localization_model_cg/localization/RR Wheel Velocity Estimator/fl_wheel_speed_calc/If Action Subsystem1' 221 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles' 222 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Angle Calculation' 223 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Quaternion Normalize' 224 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input' 225 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem' 226 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem1' 227 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem2' 228 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Quaternion Normalize/Quaternion Modulus' 229 | // '' : 'ros2webots_localization_model_cg/localization/yaw_estimator/Quaternions to Rotation Angles/Quaternion Normalize/Quaternion Modulus/Quaternion Norm' 230 | 231 | #endif // RTW_HEADER_ros2webots_localization_model_cg_h_ 232 | 233 | // 234 | // File trailer for generated code. 235 | // 236 | // [EOF] 237 | // 238 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/ros2webots_localization_model_cg_private.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_localization_model_cg_private.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_ros2webots_localization_model_cg_private_h_ 16 | #define RTW_HEADER_ros2webots_localization_model_cg_private_h_ 17 | #include "rtwtypes.h" 18 | #include "ros2webots_localization_model_cg_types.h" 19 | 20 | extern real_T rt_atan2d_snf(real_T u0, real_T u1); 21 | 22 | #endif // RTW_HEADER_ros2webots_localization_model_cg_private_h_ 23 | 24 | // 25 | // File trailer for generated code. 26 | // 27 | // [EOF] 28 | // 29 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/ros2webots_localization_model_cg_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: ros2webots_localization_model_cg_types.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_ros2webots_localization_model_cg_types_h_ 16 | #define RTW_HEADER_ros2webots_localization_model_cg_types_h_ 17 | #endif // RTW_HEADER_ros2webots_localization_model_cg_types_h_ 18 | 19 | // 20 | // File trailer for generated code. 21 | // 22 | // [EOF] 23 | // 24 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetInf.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetInf.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #include "rtwtypes.h" 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rtGetInf.h" 22 | 23 | } 24 | 25 | #include 26 | 27 | extern "C" 28 | { 29 | 30 | #include "rt_nonfinite.h" 31 | 32 | } 33 | 34 | #define NumBitsPerChar 8U 35 | 36 | extern "C" 37 | { 38 | // 39 | // Initialize rtInf needed by the generated code. 40 | // Inf is initialized as non-signaling. Assumes IEEE. 41 | // 42 | real_T rtGetInf(void) 43 | { 44 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 45 | 46 | real_T inf{ 0.0 }; 47 | 48 | if (bitsPerReal == 32U) { 49 | inf = rtGetInfF(); 50 | } else { 51 | union { 52 | LittleEndianIEEEDouble bitVal; 53 | real_T fltVal; 54 | } tmpVal; 55 | 56 | tmpVal.bitVal.words.wordH = 0x7FF00000U; 57 | tmpVal.bitVal.words.wordL = 0x00000000U; 58 | inf = tmpVal.fltVal; 59 | } 60 | 61 | return inf; 62 | } 63 | 64 | // 65 | // Initialize rtInfF needed by the generated code. 66 | // Inf is initialized as non-signaling. Assumes IEEE. 67 | // 68 | real32_T rtGetInfF(void) 69 | { 70 | IEEESingle infF; 71 | infF.wordL.wordLuint = 0x7F800000U; 72 | return infF.wordL.wordLreal; 73 | } 74 | 75 | // 76 | // Initialize rtMinusInf needed by the generated code. 77 | // Inf is initialized as non-signaling. Assumes IEEE. 78 | // 79 | real_T rtGetMinusInf(void) 80 | { 81 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 82 | 83 | real_T minf{ 0.0 }; 84 | 85 | if (bitsPerReal == 32U) { 86 | minf = rtGetMinusInfF(); 87 | } else { 88 | union { 89 | LittleEndianIEEEDouble bitVal; 90 | real_T fltVal; 91 | } tmpVal; 92 | 93 | tmpVal.bitVal.words.wordH = 0xFFF00000U; 94 | tmpVal.bitVal.words.wordL = 0x00000000U; 95 | minf = tmpVal.fltVal; 96 | } 97 | 98 | return minf; 99 | } 100 | 101 | // 102 | // Initialize rtMinusInfF needed by the generated code. 103 | // Inf is initialized as non-signaling. Assumes IEEE. 104 | // 105 | real32_T rtGetMinusInfF(void) 106 | { 107 | IEEESingle minfF; 108 | minfF.wordL.wordLuint = 0xFF800000U; 109 | return minfF.wordL.wordLreal; 110 | } 111 | } 112 | 113 | // 114 | // File trailer for generated code. 115 | // 116 | // [EOF] 117 | // 118 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetInf.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetInf.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtGetInf_h_ 16 | #define RTW_HEADER_rtGetInf_h_ 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rt_nonfinite.h" 22 | 23 | } 24 | 25 | #include "rtwtypes.h" 26 | #ifdef __cplusplus 27 | 28 | extern "C" 29 | { 30 | 31 | #endif 32 | 33 | extern real_T rtGetInf(void); 34 | extern real32_T rtGetInfF(void); 35 | extern real_T rtGetMinusInf(void); 36 | extern real32_T rtGetMinusInfF(void); 37 | 38 | #ifdef __cplusplus 39 | 40 | } // extern "C" 41 | 42 | #endif 43 | #endif // RTW_HEADER_rtGetInf_h_ 44 | 45 | // 46 | // File trailer for generated code. 47 | // 48 | // [EOF] 49 | // 50 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetNaN.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetNaN.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #include "rtwtypes.h" 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rtGetNaN.h" 22 | 23 | } 24 | 25 | #include 26 | 27 | extern "C" 28 | { 29 | 30 | #include "rt_nonfinite.h" 31 | 32 | } 33 | 34 | #define NumBitsPerChar 8U 35 | 36 | extern "C" 37 | { 38 | // 39 | // Initialize rtNaN needed by the generated code. 40 | // NaN is initialized as non-signaling. Assumes IEEE. 41 | // 42 | real_T rtGetNaN(void) 43 | { 44 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 45 | 46 | real_T nan{ 0.0 }; 47 | 48 | if (bitsPerReal == 32U) { 49 | nan = rtGetNaNF(); 50 | } else { 51 | union { 52 | LittleEndianIEEEDouble bitVal; 53 | real_T fltVal; 54 | } tmpVal; 55 | 56 | tmpVal.bitVal.words.wordH = 0xFFF80000U; 57 | tmpVal.bitVal.words.wordL = 0x00000000U; 58 | nan = tmpVal.fltVal; 59 | } 60 | 61 | return nan; 62 | } 63 | 64 | // 65 | // Initialize rtNaNF needed by the generated code. 66 | // NaN is initialized as non-signaling. Assumes IEEE. 67 | // 68 | real32_T rtGetNaNF(void) 69 | { 70 | IEEESingle nanF{ { 0.0F } }; 71 | 72 | nanF.wordL.wordLuint = 0xFFC00000U; 73 | return nanF.wordL.wordLreal; 74 | } 75 | } 76 | 77 | // 78 | // File trailer for generated code. 79 | // 80 | // [EOF] 81 | // 82 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtGetNaN.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtGetNaN.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtGetNaN_h_ 16 | #define RTW_HEADER_rtGetNaN_h_ 17 | 18 | extern "C" 19 | { 20 | 21 | #include "rt_nonfinite.h" 22 | 23 | } 24 | 25 | #include "rtwtypes.h" 26 | #ifdef __cplusplus 27 | 28 | extern "C" 29 | { 30 | 31 | #endif 32 | 33 | extern real_T rtGetNaN(void); 34 | extern real32_T rtGetNaNF(void); 35 | 36 | #ifdef __cplusplus 37 | 38 | } // extern "C" 39 | 40 | #endif 41 | #endif // RTW_HEADER_rtGetNaN_h_ 42 | 43 | // 44 | // File trailer for generated code. 45 | // 46 | // [EOF] 47 | // 48 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rt_defines.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_defines.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rt_defines_h_ 16 | #define RTW_HEADER_rt_defines_h_ 17 | 18 | //===========* 19 | // Constants * 20 | // =========== 21 | #define RT_PI 3.14159265358979323846 22 | #define RT_PIF 3.1415927F 23 | #define RT_LN_10 2.30258509299404568402 24 | #define RT_LN_10F 2.3025851F 25 | #define RT_LOG10E 0.43429448190325182765 26 | #define RT_LOG10EF 0.43429449F 27 | #define RT_E 2.7182818284590452354 28 | #define RT_EF 2.7182817F 29 | 30 | // 31 | // UNUSED_PARAMETER(x) 32 | // Used to specify that a function parameter (argument) is required but not 33 | // accessed by the function body. 34 | 35 | #ifndef UNUSED_PARAMETER 36 | #if defined(__LCC__) 37 | #define UNUSED_PARAMETER(x) // do nothing 38 | #else 39 | 40 | // 41 | // This is the semi-ANSI standard way of indicating that an 42 | // unused function parameter is required. 43 | 44 | #define UNUSED_PARAMETER(x) (void) (x) 45 | #endif 46 | #endif 47 | #endif // RTW_HEADER_rt_defines_h_ 48 | 49 | // 50 | // File trailer for generated code. 51 | // 52 | // [EOF] 53 | // 54 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rt_nonfinite.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_nonfinite.cpp 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | extern "C" 17 | { 18 | 19 | #include "rtGetNaN.h" 20 | 21 | } 22 | 23 | extern "C" 24 | { 25 | 26 | #include "rtGetInf.h" 27 | 28 | } 29 | 30 | #include 31 | #include "rtwtypes.h" 32 | 33 | extern "C" 34 | { 35 | 36 | #include "rt_nonfinite.h" 37 | 38 | } 39 | 40 | #define NumBitsPerChar 8U 41 | 42 | extern "C" 43 | { 44 | real_T rtInf; 45 | real_T rtMinusInf; 46 | real_T rtNaN; 47 | real32_T rtInfF; 48 | real32_T rtMinusInfF; 49 | real32_T rtNaNF; 50 | } 51 | 52 | extern "C" 53 | { 54 | // 55 | // Initialize the rtInf, rtMinusInf, and rtNaN needed by the 56 | // generated code. NaN is initialized as non-signaling. Assumes IEEE. 57 | // 58 | void rt_InitInfAndNaN(size_t realSize) 59 | { 60 | (void) (realSize); 61 | rtNaN = rtGetNaN(); 62 | rtNaNF = rtGetNaNF(); 63 | rtInf = rtGetInf(); 64 | rtInfF = rtGetInfF(); 65 | rtMinusInf = rtGetMinusInf(); 66 | rtMinusInfF = rtGetMinusInfF(); 67 | } 68 | 69 | // Test if value is infinite 70 | boolean_T rtIsInf(real_T value) 71 | { 72 | return (boolean_T)((value==rtInf || value==rtMinusInf) ? 1U : 0U); 73 | } 74 | 75 | // Test if single-precision value is infinite 76 | boolean_T rtIsInfF(real32_T value) 77 | { 78 | return (boolean_T)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); 79 | } 80 | 81 | // Test if value is not a number 82 | boolean_T rtIsNaN(real_T value) 83 | { 84 | boolean_T result{ (boolean_T) 0 }; 85 | 86 | size_t bitsPerReal{ sizeof(real_T) * (NumBitsPerChar) }; 87 | 88 | if (bitsPerReal == 32U) { 89 | result = rtIsNaNF((real32_T)value); 90 | } else { 91 | union { 92 | LittleEndianIEEEDouble bitVal; 93 | real_T fltVal; 94 | } tmpVal; 95 | 96 | tmpVal.fltVal = value; 97 | result = (boolean_T)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 98 | 0x7FF00000 && 99 | ( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 || 100 | (tmpVal.bitVal.words.wordL != 0) )); 101 | } 102 | 103 | return result; 104 | } 105 | 106 | // Test if single-precision value is not a number 107 | boolean_T rtIsNaNF(real32_T value) 108 | { 109 | IEEESingle tmp; 110 | tmp.wordL.wordLreal = value; 111 | return (boolean_T)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 && 112 | (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); 113 | } 114 | } 115 | 116 | // 117 | // File trailer for generated code. 118 | // 119 | // [EOF] 120 | // 121 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rt_nonfinite.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rt_nonfinite.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rt_nonfinite_h_ 16 | #define RTW_HEADER_rt_nonfinite_h_ 17 | #include 18 | #include "rtwtypes.h" 19 | #define NOT_USING_NONFINITE_LITERALS 1 20 | #ifdef __cplusplus 21 | 22 | extern "C" 23 | { 24 | 25 | #endif 26 | 27 | extern real_T rtInf; 28 | extern real_T rtMinusInf; 29 | extern real_T rtNaN; 30 | extern real32_T rtInfF; 31 | extern real32_T rtMinusInfF; 32 | extern real32_T rtNaNF; 33 | extern void rt_InitInfAndNaN(size_t realSize); 34 | extern boolean_T rtIsInf(real_T value); 35 | extern boolean_T rtIsInfF(real32_T value); 36 | extern boolean_T rtIsNaN(real_T value); 37 | extern boolean_T rtIsNaNF(real32_T value); 38 | struct BigEndianIEEEDouble { 39 | struct { 40 | uint32_T wordH; 41 | uint32_T wordL; 42 | } words; 43 | }; 44 | 45 | struct LittleEndianIEEEDouble { 46 | struct { 47 | uint32_T wordL; 48 | uint32_T wordH; 49 | } words; 50 | }; 51 | 52 | struct IEEESingle { 53 | union { 54 | real32_T wordLreal; 55 | uint32_T wordLuint; 56 | } wordL; 57 | }; 58 | 59 | #ifdef __cplusplus 60 | 61 | } // extern "C" 62 | 63 | #endif 64 | #endif // RTW_HEADER_rt_nonfinite_h_ 65 | 66 | // 67 | // File trailer for generated code. 68 | // 69 | // [EOF] 70 | // 71 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtmodel.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtmodel.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | #ifndef RTW_HEADER_rtmodel_h_ 16 | #define RTW_HEADER_rtmodel_h_ 17 | #include "ros2webots_localization_model_cg.h" 18 | #define MODEL_CLASSNAME ros2webots_localization_model_cg 19 | #define MODEL_STEPNAME step 20 | 21 | // 22 | // ROOT_IO_FORMAT: 0 (Individual arguments) 23 | // ROOT_IO_FORMAT: 1 (Structure reference) 24 | // ROOT_IO_FORMAT: 2 (Part of model data structure) 25 | 26 | #define ROOT_IO_FORMAT 1 27 | 28 | // Macros generated for backwards compatibility 29 | #ifndef rtmGetStopRequested 30 | #define rtmGetStopRequested(rtm) ((void*) 0) 31 | #endif 32 | #endif // RTW_HEADER_rtmodel_h_ 33 | 34 | // 35 | // File trailer for generated code. 36 | // 37 | // [EOF] 38 | // 39 | -------------------------------------------------------------------------------- /ros2webots_rosbot/simulink_models/ros2webots_localization_model_cg_ert_rtw/rtwtypes.h: -------------------------------------------------------------------------------- 1 | // 2 | // File: rtwtypes.h 3 | // 4 | // Code generated for Simulink model 'ros2webots_localization_model_cg'. 5 | // 6 | // Model version : 1.252 7 | // Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 8 | // C/C++ source code generated on : Sun Feb 4 13:15:47 2024 9 | // 10 | // Target selection: ert.tlc 11 | // Embedded hardware selection: Intel->x86-64 (Windows64) 12 | // Code generation objectives: Unspecified 13 | // Validation result: Not run 14 | // 15 | 16 | #ifndef RTWTYPES_H 17 | #define RTWTYPES_H 18 | 19 | // Logical type definitions 20 | #if (!defined(__cplusplus)) 21 | #ifndef false 22 | #define false (0U) 23 | #endif 24 | 25 | #ifndef true 26 | #define true (1U) 27 | #endif 28 | #endif 29 | 30 | //=======================================================================* 31 | // Target hardware information 32 | // Device type: Intel->x86-64 (Windows64) 33 | // Number of bits: char: 8 short: 16 int: 32 34 | // long: 32 35 | // native word size: 64 36 | // Byte ordering: LittleEndian 37 | // Signed integer division rounds to: Zero 38 | // Shift right on a signed integer as arithmetic shift: on 39 | // ======================================================================= 40 | 41 | //=======================================================================* 42 | // Fixed width word size data types: * 43 | // int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 44 | // uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 45 | // real32_T, real64_T - 32 and 64 bit floating point numbers * 46 | // ======================================================================= 47 | typedef signed char int8_T; 48 | typedef unsigned char uint8_T; 49 | typedef short int16_T; 50 | typedef unsigned short uint16_T; 51 | typedef int int32_T; 52 | typedef unsigned int uint32_T; 53 | typedef float real32_T; 54 | typedef double real64_T; 55 | 56 | //===========================================================================* 57 | // Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * 58 | // real_T, time_T, ulong_T. * 59 | // =========================================================================== 60 | typedef double real_T; 61 | typedef double time_T; 62 | typedef unsigned char boolean_T; 63 | typedef int int_T; 64 | typedef unsigned int uint_T; 65 | typedef unsigned long ulong_T; 66 | typedef char char_T; 67 | typedef unsigned char uchar_T; 68 | typedef char_T byte_T; 69 | 70 | //===========================================================================* 71 | // Complex number type definitions * 72 | // =========================================================================== 73 | #define CREAL_T 74 | 75 | typedef struct { 76 | real32_T re; 77 | real32_T im; 78 | } creal32_T; 79 | 80 | typedef struct { 81 | real64_T re; 82 | real64_T im; 83 | } creal64_T; 84 | 85 | typedef struct { 86 | real_T re; 87 | real_T im; 88 | } creal_T; 89 | 90 | #define CINT8_T 91 | 92 | typedef struct { 93 | int8_T re; 94 | int8_T im; 95 | } cint8_T; 96 | 97 | #define CUINT8_T 98 | 99 | typedef struct { 100 | uint8_T re; 101 | uint8_T im; 102 | } cuint8_T; 103 | 104 | #define CINT16_T 105 | 106 | typedef struct { 107 | int16_T re; 108 | int16_T im; 109 | } cint16_T; 110 | 111 | #define CUINT16_T 112 | 113 | typedef struct { 114 | uint16_T re; 115 | uint16_T im; 116 | } cuint16_T; 117 | 118 | #define CINT32_T 119 | 120 | typedef struct { 121 | int32_T re; 122 | int32_T im; 123 | } cint32_T; 124 | 125 | #define CUINT32_T 126 | 127 | typedef struct { 128 | uint32_T re; 129 | uint32_T im; 130 | } cuint32_T; 131 | 132 | //=======================================================================* 133 | // Min and Max: * 134 | // int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * 135 | // uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * 136 | // ======================================================================= 137 | #define MAX_int8_T ((int8_T)(127)) 138 | #define MIN_int8_T ((int8_T)(-128)) 139 | #define MAX_uint8_T ((uint8_T)(255U)) 140 | #define MAX_int16_T ((int16_T)(32767)) 141 | #define MIN_int16_T ((int16_T)(-32768)) 142 | #define MAX_uint16_T ((uint16_T)(65535U)) 143 | #define MAX_int32_T ((int32_T)(2147483647)) 144 | #define MIN_int32_T ((int32_T)(-2147483647-1)) 145 | #define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) 146 | 147 | // Block D-Work pointer type 148 | typedef void * pointer_T; 149 | 150 | #endif // RTWTYPES_H 151 | 152 | // 153 | // File trailer for generated code. 154 | // 155 | // [EOF] 156 | // 157 | -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_cpp_node_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "ros2webots_rosbot/rosbot_cpp_node_test.hpp" 7 | 8 | #include "rclcpp/rclcpp.hpp" 9 | #include "std_msgs/msg/string.hpp" 10 | 11 | using namespace std::chrono_literals; 12 | 13 | class MinimalPublisher : public rclcpp::Node 14 | { 15 | public: 16 | MinimalPublisher() 17 | : Node("minimal_publisher"), count_(0) 18 | { 19 | publisher_ = this->create_publisher("topic_test", 10); 20 | timer_ = this->create_wall_timer( 21 | 500ms, std::bind(&MinimalPublisher::timer_callback, this)); 22 | 23 | // Adding the subscriber to the "topic_test" 24 | subscriber_ = this->create_subscription( 25 | "topic_test", 10, std::bind(&MinimalPublisher::subscriber_callback, this, std::placeholders::_1)); 26 | } 27 | 28 | private: 29 | void timer_callback() 30 | { 31 | auto message = std_msgs::msg::String(); 32 | message.data = "Hello, world! " + std::to_string(count_++); 33 | RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); 34 | publisher_->publish(message); 35 | } 36 | 37 | void subscriber_callback(const std_msgs::msg::String::SharedPtr msg) 38 | { 39 | RCLCPP_INFO(this->get_logger(), "Received: '%s'", msg->data.c_str()); 40 | } 41 | 42 | rclcpp::TimerBase::SharedPtr timer_; 43 | rclcpp::Publisher::SharedPtr publisher_; 44 | rclcpp::Subscription::SharedPtr subscriber_; 45 | size_t count_; 46 | }; 47 | 48 | int main(int argc, char * argv[]) 49 | { 50 | rclcpp::init(argc, argv); 51 | rclcpp::spin(std::make_shared()); 52 | rclcpp::shutdown(); 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_env_mapper_node.cpp: -------------------------------------------------------------------------------- 1 | // TO DO: Implement LaserScan/PointCloud2 subscription mechanism [Select one of them] 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "ros2webots_rosbot/rosbot_localization_node.hpp" 10 | 11 | // Localization SDK main header 12 | #include "laser_env_mapper.hpp" 13 | 14 | #include "rclcpp/rclcpp.hpp" 15 | 16 | #include "nav_msgs/msg/odometry.hpp" 17 | #include "nav_msgs/msg/occupancy_grid.hpp" 18 | 19 | using namespace std::chrono_literals; 20 | 21 | class MinimalPublisher : public rclcpp::Node 22 | { 23 | public: 24 | MinimalPublisher() 25 | : Node("minimal_publisher"), count_(0) 26 | { 27 | subscriberOdom_ = this->create_subscription("/webots/rosbot/odometry", 10, std::bind(&MinimalPublisher::subscriberOdom_callback, this, std::placeholders::_1)); 28 | publisherGridMapData_ = this->create_publisher("/webots/rosbot/occupancy_grid_map", 10); 29 | // Initialize map object 30 | 31 | } 32 | 33 | private: 34 | 35 | void subscriberOdom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { 36 | OdomMessageData = *msg; 37 | } 38 | 39 | double getFullTime(const uint32_t MsgHeaderSec, const uint32_t MsgHeaderNSec) { 40 | double timeWithFullRes; 41 | timeWithFullRes = MsgHeaderSec + MsgHeaderNSec* 1e-9; 42 | return timeWithFullRes; 43 | } 44 | 45 | // Environment mapping SDK object 46 | LaserEnvMapper map_env_laser; 47 | 48 | // Odom data subcriber 49 | rclcpp::Subscription::SharedPtr subscriberOdom_; 50 | 51 | // Odom sub message data 52 | nav_msgs::msg::Odometry OdomMessageData; 53 | 54 | // Map data publisher 55 | rclcpp::Publisher::SharedPtr publisherGridMapData_; 56 | 57 | // Map pub message data 58 | nav_msgs::msg::OccupancyGrid OccupancyGridMapData; 59 | 60 | size_t count_; 61 | }; 62 | 63 | int main(int argc, char * argv[]) 64 | { 65 | rclcpp::init(argc, argv); 66 | rclcpp::spin(std::make_shared()); 67 | rclcpp::shutdown(); 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_get_keyboard_command.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "geometry_msgs/msg/twist.hpp" 6 | 7 | using namespace std::chrono_literals; 8 | 9 | char getch() { 10 | char buf = 0; 11 | struct termios old = {}; // Initialize all fields to their default values 12 | fflush(stdout); 13 | if (tcgetattr(0, &old) < 0) 14 | perror("tcsetattr()"); 15 | old.c_lflag &= ~ICANON; 16 | old.c_lflag &= ~ECHO; 17 | old.c_cc[VMIN] = 1; 18 | old.c_cc[VTIME] = 0; 19 | if (tcsetattr(0, TCSANOW, &old) < 0) 20 | perror("tcsetattr ICANON"); 21 | if (read(0, &buf, 1) < 0) 22 | perror("read()"); 23 | old.c_lflag |= ICANON; 24 | old.c_lflag |= ECHO; 25 | if (tcsetattr(0, TCSADRAIN, &old) < 0) 26 | perror("tcsetattr ~ICANON"); 27 | return buf; 28 | } 29 | 30 | class KeyboardTeleopNode : public rclcpp::Node { 31 | public: 32 | KeyboardTeleopNode() : Node("rosbot_get_command_keyboard") { 33 | publisher_ = create_publisher("/webots/rosbot/command_vel", 10); 34 | timer_ = create_wall_timer(50ms, std::bind(&KeyboardTeleopNode::keyboardCallback, this)); 35 | } 36 | 37 | private: 38 | void keyboardCallback() { 39 | char key = getch(); 40 | geometry_msgs::msg::Twist twist; 41 | 42 | switch (key) { 43 | case 65: // Up arrow 44 | twist.linear.x = 0.2; 45 | RCLCPP_INFO(logger_, "Up arrow has been pressed"); 46 | break; 47 | case 66: // Down arrow 48 | twist.linear.x = -0.2; 49 | RCLCPP_INFO(logger_, "Down arrow has been pressed"); 50 | break; 51 | case 68: // Left arrow 52 | twist.angular.z = 0.5; 53 | RCLCPP_INFO(logger_, "Left arrow has been pressed"); 54 | break; 55 | case 67: // Right arrow 56 | twist.angular.z = -0.5; 57 | RCLCPP_INFO(logger_, "Right arrow has been pressed"); 58 | break; 59 | default: 60 | twist.linear.x = 0.0; 61 | twist.angular.z = 0.0; 62 | break; 63 | } 64 | 65 | publisher_->publish(twist); 66 | } 67 | 68 | rclcpp::Publisher::SharedPtr publisher_; 69 | rclcpp::TimerBase::SharedPtr timer_; 70 | rclcpp::Logger logger_ = this->get_logger(); 71 | }; 72 | 73 | int main(int argc, char **argv) { 74 | rclcpp::init(argc, argv); 75 | auto node = std::make_shared(); 76 | rclcpp::spin(node); 77 | rclcpp::shutdown(); 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_localization_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "ros2webots_rosbot/rosbot_localization_node.hpp" 8 | 9 | // Localization SDK main header 10 | #include "ros2webots_localization_model_cg.h" 11 | 12 | #include "rclcpp/rclcpp.hpp" 13 | #include "std_msgs/msg/string.hpp" 14 | #include "nav_msgs/msg/odometry.hpp" 15 | #include "sensor_msgs/msg/imu.hpp" 16 | #include "sensor_msgs/msg/joint_state.hpp" 17 | 18 | using namespace std::chrono_literals; 19 | 20 | class MinimalPublisher : public rclcpp::Node 21 | { 22 | public: 23 | MinimalPublisher() 24 | : Node("minimal_publisher"), count_(0) 25 | { 26 | subscriberIMU_ = this->create_subscription("/webots/rosbot/imu", 10, std::bind(&MinimalPublisher::subscriberIMU_callback, this, std::placeholders::_1)); 27 | subscriberWheelsData_ = this->create_subscription("/webots/rosbot/odom/wheels_encoder_data", 10, std::bind(&MinimalPublisher::subscriberWheelsData_callback, this, std::placeholders::_1)); 28 | // Initialized Localozition SDK object 29 | localization_sdk_obj_.initialize(); 30 | 31 | publisherOdomData_ = this->create_publisher("/webots/rosbot/odometry", 10); 32 | // timerOdomDataPub_ = this->create_wall_timer(50ms, std::bind(&MinimalPublisher::OdomDataPub_callback, this)); 33 | 34 | imu_data_activated = false; 35 | wheel_data_activated = false; 36 | } 37 | 38 | private: 39 | 40 | void OdomDataPub_callback() { 41 | // if (areTimestampsSynced(Imu_data_received.header.stamp.sec, Imu_data_received.header.stamp.nanosec, Wheels_encoder_data_received.header.stamp.sec, Wheels_encoder_data_received.header.stamp.nanosec)) { 42 | // RunLocalizationProcess(); 43 | // } // What about else? 44 | bool imuANDwheelDATAareSYNC = areTimestampsSynced(Imu_data_received.header.stamp.sec, Imu_data_received.header.stamp.nanosec, Wheels_encoder_data_received.header.stamp.sec, Wheels_encoder_data_received.header.stamp.nanosec); 45 | if (imu_data_activated && wheel_data_activated && imuANDwheelDATAareSYNC) { 46 | RunLocalizationProcess(); 47 | } 48 | auto current_time = std::chrono::system_clock::now(); 49 | auto timestamp = std::chrono::duration_cast(current_time.time_since_epoch()); 50 | OdomMessage.header.stamp.sec = static_cast(timestamp.count()); 51 | OdomMessage.header.stamp.nanosec = static_cast((current_time.time_since_epoch() - timestamp).count()); 52 | OdomMessage.header.frame_id = "odometry_frame"; 53 | OdomMessage.child_frame_id = "base_link"; 54 | OdomMessage.pose.pose.position.x = localization_output_data.odometry_vector[0]; 55 | OdomMessage.pose.pose.position.y = localization_output_data.odometry_vector[1]; 56 | OdomMessage.pose.pose.orientation.z = localization_output_data.odometry_vector[2]; 57 | publisherOdomData_->publish(OdomMessage); 58 | } 59 | 60 | void RunLocalizationProcess() { 61 | // Set inputs 62 | localization_input_data.imu_orientation_vector[0] = Imu_data_received.orientation.x; 63 | localization_input_data.imu_orientation_vector[1] = Imu_data_received.orientation.y; 64 | localization_input_data.imu_orientation_vector[2] = Imu_data_received.orientation.z; 65 | localization_input_data.imu_orientation_vector[3] = Imu_data_received.orientation.w; 66 | localization_input_data.imu_angular_velocity_vector[0] = Imu_data_received.angular_velocity.x; 67 | localization_input_data.imu_angular_velocity_vector[1] = Imu_data_received.angular_velocity.y; 68 | localization_input_data.imu_angular_velocity_vector[2] = Imu_data_received.angular_velocity.z; 69 | localization_input_data.imu_linear_acceleration_vector[0] = Imu_data_received.linear_acceleration.x; 70 | localization_input_data.imu_linear_acceleration_vector[1] = Imu_data_received.linear_acceleration.y; 71 | localization_input_data.imu_linear_acceleration_vector[2] = Imu_data_received.linear_acceleration.z; 72 | localization_input_data.fl_wheel_pose = Wheels_encoder_data_received.position[0]; 73 | localization_input_data.fr_wheel_pose = Wheels_encoder_data_received.position[1]; 74 | localization_input_data.rl_wheel_pose = Wheels_encoder_data_received.position[2]; 75 | localization_input_data.rr_wheel_pose = Wheels_encoder_data_received.position[3]; 76 | double timeOldWheelData = getFullTime(Wheels_encoder_data_received_old.header.stamp.sec, Wheels_encoder_data_received_old.header.stamp.nanosec); 77 | double timeNewWheelData = getFullTime(Wheels_encoder_data_received.header.stamp.sec, Wheels_encoder_data_received.header.stamp.nanosec); 78 | double dt_wheel_data = timeNewWheelData - timeOldWheelData; 79 | localization_input_data.fl_wheel_pose_update_dt = dt_wheel_data; 80 | localization_input_data.fr_wheel_pose_update_dt = dt_wheel_data; 81 | localization_input_data.rl_wheel_pose_update_dt = dt_wheel_data; 82 | localization_input_data.rr_wheel_pose_update_dt = dt_wheel_data; 83 | localization_sdk_obj_.setExternalInputs(&localization_input_data); 84 | // Do localization 85 | localization_sdk_obj_.step(); 86 | // Get outputs 87 | localization_output_data = localization_sdk_obj_.getExternalOutputs(); 88 | } 89 | 90 | void subscriberIMU_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { 91 | // RCLCPP_INFO(this->get_logger(), "Received IMU data"); 92 | Imu_data_received = *msg; 93 | imu_data_activated = true; 94 | // RCLCPP_INFO(this->get_logger(), "IMU orientation w:%f", Imu_data_received.orientation.w); 95 | } 96 | 97 | void subscriberWheelsData_callback(const sensor_msgs::msg::JointState::SharedPtr msg) { 98 | Wheels_encoder_data_received = *msg; 99 | wheel_data_activated = true; 100 | OdomDataPub_callback(); 101 | Wheels_encoder_data_received_old = Wheels_encoder_data_received; 102 | } 103 | 104 | bool areTimestampsSynced(uint32_t imuSec, uint32_t imuNsec, uint32_t jointStateSec, uint32_t jointStateNsec, double threshold = 0.2) { 105 | // Convert seconds and nanoseconds to a single double value for easier comparison 106 | double imuTimestamp = imuSec + imuNsec * 1e-9; 107 | double jointStateTimestamp = jointStateSec + jointStateNsec * 1e-9; 108 | 109 | // Calculate the time difference between IMU and joint state timestamps 110 | double timeDifference = std::fabs(imuTimestamp - jointStateTimestamp); 111 | 112 | // RCLCPP_INFO(this->get_logger(), "Timestamp differene: %f", timeDifference); 113 | 114 | // Check if the time difference is within the specified threshold 115 | if (timeDifference <= threshold) { 116 | return true; // Timestamps are considered synced 117 | } else { 118 | return false; // Timestamps are not synced 119 | } 120 | } 121 | 122 | double getFullTime(const uint32_t MsgHeaderSec, const uint32_t MsgHeaderNSec) { 123 | double timeWithFullRes; 124 | timeWithFullRes = MsgHeaderSec + MsgHeaderNSec* 1e-9; 125 | return timeWithFullRes; 126 | } 127 | 128 | 129 | // Odom pablisher timer 130 | rclcpp::TimerBase::SharedPtr timerOdomDataPub_; 131 | // Odom publisher 132 | rclcpp::Publisher::SharedPtr publisherOdomData_; 133 | // IMU data subcriber 134 | rclcpp::Subscription::SharedPtr subscriberIMU_; 135 | // Wheel encoders data subcriber 136 | rclcpp::Subscription::SharedPtr subscriberWheelsData_; 137 | 138 | // Create Localizition SDK object 139 | ros2webots_localization_model_cg localization_sdk_obj_; 140 | 141 | // Localization input data struct 142 | ros2webots_localization_model_cg::ExtU_ros2webots_localization__T localization_input_data; 143 | 144 | // Localization output data struct 145 | ros2webots_localization_model_cg::ExtY_ros2webots_localization__T localization_output_data; 146 | 147 | // IMU data struct (received) 148 | sensor_msgs::msg::Imu Imu_data_received; 149 | 150 | // Wheels encoder data struct (received) 151 | sensor_msgs::msg::JointState Wheels_encoder_data_received; 152 | 153 | // Wheels encoder data struct (received) 154 | sensor_msgs::msg::JointState Wheels_encoder_data_received_old; 155 | 156 | // Odom pub message 157 | nav_msgs::msg::Odometry OdomMessage; 158 | 159 | size_t count_; 160 | 161 | bool imu_data_activated; 162 | bool wheel_data_activated; 163 | }; 164 | 165 | int main(int argc, char * argv[]) 166 | { 167 | rclcpp::init(argc, argv); 168 | rclcpp::spin(std::make_shared()); 169 | rclcpp::shutdown(); 170 | return 0; 171 | } 172 | -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_tf_publisher.cpp: -------------------------------------------------------------------------------- 1 | // TO DO: Why graphic of the robot won't load in rviz2? 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "std_msgs/msg/string.hpp" 9 | #include 10 | #include 11 | #include "nav_msgs/msg/odometry.hpp" 12 | 13 | using namespace std::chrono_literals; 14 | 15 | 16 | class RosbotDescriptionPublisher : public rclcpp::Node { 17 | public: 18 | RosbotDescriptionPublisher() : Node("rosbot_description_publisher_node") { 19 | urdf_path_ = ament_index_cpp::get_package_share_directory("ros2webots_rosbot") + "/resource/rosbot.urdf"; 20 | loadURDF(); 21 | publisher_ = this->create_publisher("robot_description_rosbot", 10); 22 | subscriberOdomData_ = this->create_subscription("/webots/rosbot/odometry", 10, std::bind(&RosbotDescriptionPublisher::subscriberOdom_callback, this, std::placeholders::_1)); 23 | tf_broadcaster_ = std::make_shared(this); 24 | timer_ = this->create_wall_timer(50ms, std::bind(&RosbotDescriptionPublisher::publishRosBotDescription, this)); 25 | timer2_ = this->create_wall_timer(50ms, std::bind(&RosbotDescriptionPublisher::publish_transforms, this)); 26 | timer3_ = this->create_wall_timer(50ms, std::bind(&RosbotDescriptionPublisher::publish_transform_laser_lds01, this)); 27 | } 28 | std::string getURDF() { 29 | return urdf_path_; 30 | } 31 | 32 | 33 | private: 34 | // Create a publisher for the robot description topic 35 | void publishRosBotDescription() { 36 | auto message = std_msgs::msg::String(); 37 | message.data = urdf_path_; 38 | // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); 39 | publisher_->publish(message); 40 | } 41 | 42 | rclcpp::Publisher::SharedPtr publisher_; 43 | rclcpp::Subscription::SharedPtr subscriberOdomData_; 44 | rclcpp::TimerBase::SharedPtr timer_; 45 | rclcpp::TimerBase::SharedPtr timer2_; 46 | rclcpp::TimerBase::SharedPtr timer3_; 47 | std::string urdf_path_; 48 | urdf::Model model; 49 | std::shared_ptr tf_broadcaster_; 50 | 51 | void loadURDF() { 52 | // Load the URDF file 53 | if (!model.initFile(urdf_path_)) { 54 | RCLCPP_ERROR(this->get_logger(), "Failed to load URDF file"); 55 | return; 56 | } 57 | 58 | // Access model information (e.g., joints, links, etc.) 59 | // RCLCPP_INFO(this->get_logger(), "Robot name: %s", model.getName().c_str()); 60 | } 61 | void publish_transforms() 62 | { 63 | // Iterate through all joints in the URDF 64 | for (const auto& joint : model.joints_) { 65 | // Get joint properties 66 | const std::string& parent_link_name = joint.second->parent_link_name; 67 | const std::string& child_link_name = joint.second->child_link_name; 68 | const urdf::Pose& pose = joint.second->parent_to_joint_origin_transform; 69 | 70 | // Create a transform message 71 | geometry_msgs::msg::TransformStamped transform_stamped; 72 | transform_stamped.header.stamp = this->get_clock()->now(); 73 | transform_stamped.header.frame_id = parent_link_name; 74 | transform_stamped.child_frame_id = child_link_name; 75 | transform_stamped.transform.translation.x = pose.position.x; 76 | transform_stamped.transform.translation.y = pose.position.y; 77 | transform_stamped.transform.translation.z = pose.position.z; 78 | transform_stamped.transform.rotation.x = pose.rotation.x; 79 | transform_stamped.transform.rotation.y = pose.rotation.y; 80 | transform_stamped.transform.rotation.z = pose.rotation.z; 81 | transform_stamped.transform.rotation.w = pose.rotation.w; 82 | 83 | // Broadcast the transform 84 | tf_broadcaster_->sendTransform(transform_stamped); 85 | // RCLCPP_INFO(this->get_logger(), "TF of parent link %s to child link %s has been published.", parent_link_name.c_str(), child_link_name.c_str()); 86 | } 87 | } 88 | 89 | void publish_transform_laser_lds01 () { 90 | const std::string& parent_link_name = "laser"; 91 | const std::string& child_link_name = "LDS-01"; 92 | geometry_msgs::msg::TransformStamped transform_stamped_LDS01toLaser; 93 | transform_stamped_LDS01toLaser.header.stamp = this->get_clock()->now(); 94 | transform_stamped_LDS01toLaser.header.frame_id = parent_link_name; 95 | transform_stamped_LDS01toLaser.child_frame_id = child_link_name; 96 | transform_stamped_LDS01toLaser.transform.translation.x = 0; 97 | transform_stamped_LDS01toLaser.transform.translation.y = 0; 98 | transform_stamped_LDS01toLaser.transform.translation.z = 0; 99 | transform_stamped_LDS01toLaser.transform.rotation.x = 0; 100 | transform_stamped_LDS01toLaser.transform.rotation.y = 0; 101 | transform_stamped_LDS01toLaser.transform.rotation.z = 0; 102 | transform_stamped_LDS01toLaser.transform.rotation.w = 0; 103 | tf_broadcaster_->sendTransform(transform_stamped_LDS01toLaser); 104 | } 105 | 106 | // Dynamic base_link frame w.r.t fixed odom frame in point-cloud visualization 107 | 108 | void subscriberOdom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { 109 | 110 | const std::string& parent_link_name = "odom_frame"; 111 | const std::string& child_link_name = "base_link"; 112 | geometry_msgs::msg::TransformStamped transform_stamped_BaseLinktoOdom; 113 | transform_stamped_BaseLinktoOdom.header.stamp = this->get_clock()->now(); 114 | transform_stamped_BaseLinktoOdom.header.frame_id = parent_link_name; 115 | transform_stamped_BaseLinktoOdom.child_frame_id = child_link_name; 116 | transform_stamped_BaseLinktoOdom.transform.translation.x = -1 * msg->pose.pose.position.x; 117 | transform_stamped_BaseLinktoOdom.transform.translation.y = -1 * msg->pose.pose.position.y; 118 | transform_stamped_BaseLinktoOdom.transform.translation.z = -1 * msg->pose.pose.position.z; 119 | transform_stamped_BaseLinktoOdom.transform.rotation.x = 0; 120 | transform_stamped_BaseLinktoOdom.transform.rotation.y = 0; 121 | transform_stamped_BaseLinktoOdom.transform.rotation.z = std::sin(msg->pose.pose.orientation.z/2); 122 | transform_stamped_BaseLinktoOdom.transform.rotation.w = std::cos(msg->pose.pose.orientation.z/2); 123 | tf_broadcaster_->sendTransform(transform_stamped_BaseLinktoOdom); 124 | } 125 | }; 126 | 127 | int main(int argc, char **argv) { 128 | rclcpp::init(argc, argv); 129 | auto node = std::make_shared(); 130 | node->declare_parameter("rosbot_robot_description", node->getURDF()); 131 | node->set_parameter(rclcpp::Parameter("rosbot_robot_description", node->getURDF())); 132 | auto parameter_value = node->get_parameter("rosbot_robot_description"); 133 | rclcpp::spin(node); 134 | rclcpp::shutdown(); 135 | return 0; 136 | } -------------------------------------------------------------------------------- /ros2webots_rosbot/src/rosbot_way_point_tracker_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "rclcpp/rclcpp.hpp" 6 | #include "geometry_msgs/msg/twist.hpp" 7 | #include "nav_msgs/msg/odometry.hpp" 8 | 9 | #include "ros2webots_go_to_point_controller_model_cg.h" 10 | 11 | using namespace std::chrono_literals; 12 | 13 | 14 | 15 | class ROSBotWayPointTracker : public rclcpp::Node { 16 | public: 17 | ROSBotWayPointTracker() : Node("rosbot_way_point_tracker") { 18 | publisher_ = create_publisher("/webots/rosbot/command_vel", 10); 19 | OdomDataSubscriber_ = this->create_subscription("/webots/rosbot/odometry", 10, 20 | std::bind(&ROSBotWayPointTracker::OdomDataSubscriber_callback, this, std::placeholders::_1)); 21 | 22 | wayPointTrackerControllerObject.initialize(); 23 | } 24 | 25 | private: 26 | 27 | rclcpp::Publisher::SharedPtr publisher_; 28 | rclcpp::Subscription::SharedPtr OdomDataSubscriber_; 29 | rclcpp::Logger logger_ = this->get_logger(); 30 | 31 | // Actual position data 32 | nav_msgs::msg::Odometry ReceivedActualLocalizationData_; 33 | 34 | // Desiered position data 35 | // TO DO --> Later we have to received them from a mission (path) planner 36 | double x_des = 2.; double y_des = 1.; double yaw_des = 0.; 37 | std::vector x_des_vec = {0.0, 0.6, 1.4, 2.2, 3.2, 2.0, 0.2, 0.0}; 38 | std::vector y_des_vec = {0.0, -0.45, 0.45, -0.45, 0.45,-0.45, -0.45, 0.0}; 39 | int des_point_index = 0; 40 | 41 | 42 | // Way point tracker SDK class object 43 | ros2webots_go_to_point_controller_model_cg wayPointTrackerControllerObject; 44 | ros2webots_go_to_point_controller_model_cg::ExtU_ros2webots_go_to_point_c_T controller_input; 45 | ros2webots_go_to_point_controller_model_cg::ExtY_ros2webots_go_to_point_c_T controller_output; 46 | 47 | // Controller command to the ROSBot driver 48 | geometry_msgs::msg::Twist twistCommand; 49 | 50 | void OdomDataSubscriber_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { 51 | ReceivedActualLocalizationData_ = *msg; 52 | // Set controller inputs 53 | controller_input.x_act = ReceivedActualLocalizationData_.pose.pose.position.x; 54 | controller_input.y_act = ReceivedActualLocalizationData_.pose.pose.position.y; 55 | controller_input.yaw_act = ReceivedActualLocalizationData_.pose.pose.orientation.z; 56 | if(des_point_index>x_des_vec.size()){des_point_index = 0;} 57 | controller_input.x_des = x_des_vec[des_point_index]; // TO DO --> Path planner node 58 | controller_input.y_des = y_des_vec[des_point_index]; // TO DO --> Path planner node 59 | controller_input.yaw_des = yaw_des; // TO DO --> Path planner node 60 | 61 | wayPointTrackerControllerObject.setExternalInputs(&controller_input); 62 | 63 | // Step the controller 64 | wayPointTrackerControllerObject.step(); 65 | 66 | // Get controller calculations 67 | controller_output = wayPointTrackerControllerObject.getExternalOutputs(); 68 | 69 | // Control reaching to the next way point 70 | if(controller_output.controller_state == 0) {des_point_index++;} 71 | 72 | // Assign twist command 73 | twistCommand.linear.x = controller_output.command_velocity; 74 | twistCommand.angular.z = controller_output.command_steering_angle; 75 | // Info: controller_output.controller_state == 0 --> Reached to the point, == 1 --> Not reached to the point 76 | 77 | // Publish the control command for the ROSBot 78 | publisher_->publish(twistCommand); 79 | 80 | } 81 | }; 82 | 83 | int main(int argc, char **argv) { 84 | rclcpp::init(argc, argv); 85 | auto node = std::make_shared(); 86 | rclcpp::spin(node); 87 | rclcpp::shutdown(); 88 | return 0; 89 | } 90 | -------------------------------------------------------------------------------- /ros2webots_rosbot/worlds/.world_roboticknowledge.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_rosbot/worlds/.world_roboticknowledge.jpg -------------------------------------------------------------------------------- /ros2webots_rosbot/worlds/.world_roboticknowledge.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2023b 2 | perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000262fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000002620000004100ffffff00000003000007800000003efc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000780000000000000000000000780000003ce00000004000000040000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff0000000100000002000000fc000006820100000002010000000101 4 | sceneTreePerspectives: 000000ff00000001000000030000001e000000c0000000fa0100000002010000000201 5 | minimizedPerspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000262fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000002620000004100ffffff00000003000007800000003efc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000038e00000004000000040000000100000008fc00000000 6 | maximizedDockId: -1 7 | centralWidgetVisible: 1 8 | orthographicViewHeight: 1 9 | textFiles: -1 10 | renderingDevicePerspectives: rosbot_robot:camera depth;1;0.895833;0;0 11 | renderingDevicePerspectives: rosbot_robot:camera rgb;1;0.895833;0;0 12 | -------------------------------------------------------------------------------- /ros2webots_rosbot/worlds/world_roboticknowledge.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2023b utf8 2 | 3 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto" 4 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" 5 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/RectangleArena.proto" 6 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/plants/protos/PottedTree.proto" 7 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/tables/protos/Table.proto" 8 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/Parquetry.proto" 9 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/BrushedAluminium.proto" 10 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/husarion/rosbot/protos/Rosbot.proto" 11 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/traffic/protos/TrafficCone.proto" 12 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/balls/protos/SoccerBall.proto" 13 | 14 | WorldInfo { 15 | } 16 | Viewpoint { 17 | orientation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.0944 18 | position -0.22097335940147445 -1.2722682925136632 5.428593795119409 19 | follow "rosbot_robot" 20 | followType "None" 21 | } 22 | TexturedBackground { 23 | } 24 | TexturedBackgroundLight { 25 | } 26 | RectangleArena { 27 | translation 0.01 0 0 28 | floorSize 5 5 29 | floorAppearance Parquetry { 30 | type "light strip" 31 | colorOverride 1 1 0.498039 32 | IBLStrength 10 33 | } 34 | wallHeight 0.3 35 | wallAppearance BrushedAluminium { 36 | colorOverride 0.666667 0.333333 0 37 | IBLStrength 22 38 | } 39 | } 40 | SoccerBall { 41 | translation -1.5 -1.75 0.11 42 | } 43 | SoccerBall { 44 | translation -0.7 -1.75 0.11 45 | name "soccer ball(1)" 46 | } 47 | SoccerBall { 48 | translation 0.1 -1.75 0.11 49 | name "soccer ball(2)" 50 | } 51 | SoccerBall { 52 | translation 0.9 -1.75 0.11 53 | name "soccer ball(3)" 54 | } 55 | TrafficCone { 56 | translation -0.74 -0.46 0 57 | } 58 | TrafficCone { 59 | translation -0.74 1.44 0 60 | name "traffic cone(3)" 61 | } 62 | TrafficCone { 63 | translation 1.9 -0.46 0 64 | name "traffic cone(1)" 65 | } 66 | TrafficCone { 67 | translation 1.9 1.44 0 68 | name "traffic cone(2)" 69 | } 70 | Rosbot { 71 | translation -2.24 -1.76 0 72 | name "rosbot_robot" 73 | controller "" 74 | } 75 | Table { 76 | translation -1.6 1.5 0 77 | } 78 | PottedTree { 79 | translation 1.86 -1.12 0 80 | } 81 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/launch/test_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='webots_ros2_dev_1', 8 | namespace='sim_node_run_1', 9 | executable='sim_node_ros2webots', 10 | name='sim' 11 | ), 12 | ]) -------------------------------------------------------------------------------- /ros2webots_turtlebot/launch/turtlebot_world_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.substitutions.path_join_substitution import PathJoinSubstitution 4 | from webots_ros2_driver.webots_launcher import WebotsLauncher 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch.substitutions import LaunchConfiguration 7 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 8 | import launch 9 | from webots_ros2_driver.webots_controller import WebotsController 10 | 11 | def generate_launch_description(): 12 | 13 | package_dir = get_package_share_directory('ros2webots_turtlebot') 14 | world = LaunchConfiguration('world') 15 | 16 | # start webots 17 | webots = WebotsLauncher ( 18 | world=PathJoinSubstitution([package_dir, 'worlds', world]), 19 | ros2_supervisor=True 20 | ) 21 | 22 | # add robot controller 23 | robot_description_path = PathJoinSubstitution([package_dir, 'resource', 'turtleBotComponent.urdf']) 24 | turtleBot_driver = WebotsController( 25 | robot_name='TurtleBot3Burger_robot', 26 | parameters=[{'robot_description':robot_description_path} 27 | ], 28 | respawn=True 29 | ) 30 | 31 | return LaunchDescription([ 32 | DeclareLaunchArgument( 33 | 'world', 34 | default_value='world_roboticknowledge.wbt', 35 | description='Choose one of the world files from `/webots_ros2_epuck/world` directory' 36 | ), 37 | webots, 38 | webots._supervisor, 39 | turtleBot_driver, 40 | 41 | 42 | # This action will kill all nodes once the Webots simulation has exited 43 | launch.actions.RegisterEventHandler( 44 | event_handler=launch.event_handlers.OnProcessExit( 45 | target_action=webots, 46 | on_exit=[ 47 | launch.actions.EmitEvent(event=launch.events.Shutdown()) 48 | ], 49 | ) 50 | ) 51 | ]) -------------------------------------------------------------------------------- /ros2webots_turtlebot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2webots_turtlebot 5 | 0.0.1 6 | ros2webots_rosbot is a ros2 package that runs simulation of a Turtlebot in Webots. 7 | Mohammad Javad Zallaghi 8 | Apache-2.0 9 | 10 | rclpy 11 | geometry_msgs 12 | 13 | ament_copyright 14 | ament_flake8 15 | ament_pep257 16 | python3-pytest 17 | 18 | 19 | ament_python 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/protos/TurtleBot3Burger.proto: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2023b utf8 2 | # license: Apache License 2.0 3 | # license url: https://www.apache.org/licenses/LICENSE-2.0 4 | # documentation url: https://webots.cloud/run?url=https://github.com/cyberbotics/webots/blob/released/projects/robots/robotis/turtlebot/protos/TurtleBot3Burger.proto 5 | # keywords: robot/wheeled 6 | # Burger model of the third version of the TurtleBot robot. 7 | 8 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/devices/robotis/protos/RobotisLds01.proto" 9 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/TireRubber.proto" 10 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/Pcb.proto" 11 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/RoughPolymer.proto" 12 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/MattePaint.proto" 13 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/BrushedAluminium.proto" 14 | 15 | PROTO TurtleBot3Burger [ 16 | field SFVec3f translation 0 0 0 # Is `Pose.translation`. 17 | field SFRotation rotation 0 1 0 0 # Is `Pose.rotation`. 18 | field SFString name "TurtleBot3Burger" # Is `Solid.name`. 19 | field SFString controller "" # Is `Robot.controller`. 20 | field MFString controllerArgs [] # Is `Robot.controllerArgs`. 21 | field SFString customData "" # Is `Robot.customData`. 22 | field SFBool supervisor FALSE # Is `Robot.supervisor`. 23 | field SFBool synchronization TRUE # Is `Robot.synchronization`. 24 | field MFNode extensionSlot [ RobotisLds01 { } ] # Extends the robot with new nodes in the extension slot. 25 | ] 26 | { 27 | Robot { 28 | translation IS translation 29 | rotation IS rotation 30 | name IS name 31 | controller IS controller 32 | controllerArgs IS controllerArgs 33 | customData IS customData 34 | supervisor IS supervisor 35 | synchronization IS synchronization 36 | children [ 37 | Pose { 38 | translation -0.03 0 0.153 39 | children IS extensionSlot 40 | } 41 | DEF RIGHT_JOINT HingeJoint { 42 | jointParameters HingeJointParameters { 43 | axis 0 1 0 44 | anchor 0 -0.08 0.033 45 | } 46 | device [ 47 | RotationalMotor { 48 | name "right wheel motor" 49 | consumptionFactor -0.001 50 | maxVelocity 6.67 51 | } 52 | PositionSensor { 53 | name "right wheel sensor" 54 | resolution 0.00628 55 | } 56 | ] 57 | endPoint Solid { 58 | translation 0 -0.08 0.033 59 | rotation 0 -1 0 1.570796 60 | children [ 61 | DEF RIGHT_WHEEL Pose { 62 | rotation 1 0 0 -1.5708003061004252 63 | children [ 64 | DEF WHEEL_SHAPE Group { 65 | children [ 66 | Shape { 67 | appearance PBRAppearance { 68 | baseColor 0 0 0 69 | roughness 1 70 | metalness 0 71 | } 72 | geometry Mesh { 73 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/wheel_shape.obj" 74 | } 75 | } 76 | Shape { 77 | appearance PBRAppearance { 78 | baseColor 0 0 0 79 | roughness 1 80 | metalness 0 81 | } 82 | geometry Mesh { 83 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/inner_wheel.obj" 84 | } 85 | } 86 | Shape { 87 | appearance TireRubber { 88 | textureTransform TextureTransform { 89 | scale 1.7 0.76 90 | translation 0 0.164 91 | } 92 | type "bike" 93 | } 94 | geometry Mesh { 95 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/tire.obj" 96 | } 97 | } 98 | Shape { 99 | appearance PBRAppearance { 100 | } 101 | geometry Mesh { 102 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/wheel_support.obj" 103 | } 104 | } 105 | ] 106 | } 107 | ] 108 | } 109 | ] 110 | name "right wheel" 111 | boundingObject Pose { 112 | rotation 1 0 0 -1.570796 113 | children [ 114 | Cylinder { 115 | height 0.018 116 | radius 0.033 117 | } 118 | ] 119 | } 120 | physics Physics { 121 | density -1 122 | mass 0.028499 123 | centerOfMass [ 124 | 0 0 0 125 | ] 126 | } 127 | } 128 | } 129 | DEF LEFT_JOINT HingeJoint { 130 | jointParameters HingeJointParameters { 131 | axis 0 1 0 132 | anchor 0 0.08 0.033 133 | } 134 | device [ 135 | RotationalMotor { 136 | name "left wheel motor" 137 | consumptionFactor -0.001 138 | maxVelocity 6.67 139 | } 140 | PositionSensor { 141 | name "left wheel sensor" 142 | resolution 0.00628 143 | } 144 | ] 145 | endPoint Solid { 146 | translation 0 0.08 0.033 147 | rotation 0.707105 0 0.707109 -3.141588 148 | children [ 149 | DEF RIGHT_WHEEL Pose { 150 | rotation 1 0 0 -1.5708003061004252 151 | children [ 152 | USE WHEEL_SHAPE 153 | ] 154 | } 155 | ] 156 | name "left wheel" 157 | boundingObject Pose { 158 | rotation 1 0 0 -1.570796 159 | children [ 160 | Cylinder { 161 | height 0.018 162 | radius 0.033 163 | } 164 | ] 165 | } 166 | physics Physics { 167 | density -1 168 | mass 0.028499 169 | centerOfMass [ 170 | 0 0 0 171 | ] 172 | } 173 | } 174 | } 175 | DEF CASTER_JOINT BallJoint { 176 | jointParameters BallJointParameters { 177 | anchor -0.081 0 0.004 178 | } 179 | endPoint Solid { 180 | translation -0.081 0 0.004 181 | children [ 182 | Shape { 183 | appearance PBRAppearance { 184 | } 185 | geometry DEF CASTER_SPHERE Sphere { 186 | radius 0.004 187 | subdivision 2 188 | } 189 | } 190 | ] 191 | boundingObject USE CASTER_SPHERE 192 | physics Physics { 193 | } 194 | } 195 | } 196 | DEF BODY Pose { 197 | translation 0 0 0.01 198 | children [ 199 | DEF MOTORS Shape { 200 | appearance PBRAppearance { 201 | baseColor 0 0 0 202 | roughness 0.8 203 | metalness 0 204 | } 205 | geometry Mesh { 206 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/motors.obj" 207 | } 208 | } 209 | DEF CARDS Shape { 210 | appearance Pcb { 211 | } 212 | geometry Mesh { 213 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/cards.obj" 214 | } 215 | } 216 | DEF HOLDERS Shape { 217 | appearance DEF MAIN_APP RoughPolymer { 218 | baseColor 0 0 0 219 | textureTransform TextureTransform { 220 | scale 3 3 221 | } 222 | } 223 | geometry Mesh { 224 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/holders.obj" 225 | } 226 | } 227 | DEF BODY_FLOOR_0 Shape { 228 | appearance USE MAIN_APP 229 | geometry Mesh { 230 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/body_floor.obj" 231 | } 232 | } 233 | DEF BODY_FLOOR_1 Pose { 234 | translation 0 0 0.0376 235 | children [ 236 | USE BODY_FLOOR_0 237 | ] 238 | } 239 | DEF BODY_FLOOR_2 Pose { 240 | translation 0 0 0.085 241 | children [ 242 | USE BODY_FLOOR_0 243 | ] 244 | } 245 | DEF BODY_FLOOR_3 Pose { 246 | translation 0 0 0.1325 247 | children [ 248 | USE BODY_FLOOR_0 249 | ] 250 | } 251 | DEF BATTERY Shape { 252 | appearance MattePaint { 253 | baseColor 0.054902 0.121569 0.356863 254 | } 255 | geometry Mesh { 256 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/battery.obj" 257 | } 258 | } 259 | DEF SCREWS Shape { 260 | appearance PBRAppearance { 261 | } 262 | geometry Mesh { 263 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/screws.obj" 264 | } 265 | } 266 | DEF BARS Shape { 267 | appearance BrushedAluminium { 268 | textureTransform TextureTransform { 269 | scale 3 2 270 | } 271 | } 272 | geometry Mesh { 273 | url "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/meshes/bars.obj" 274 | } 275 | } 276 | ] 277 | } 278 | Accelerometer { 279 | translation -0.032 0 0.078 280 | } 281 | Gyro { 282 | translation -0.032 0 0.078 283 | } 284 | Compass { 285 | translation -0.032 0 0.078 286 | } 287 | ] 288 | boundingObject Group { 289 | children [ 290 | Pose { 291 | translation -0.032 0 0.08 292 | children [ 293 | Box { 294 | size 0.097 0.137 0.1425 295 | } 296 | ] 297 | } 298 | Pose { 299 | translation -0.032 0 0.08 300 | children [ 301 | Box { 302 | size 0.135 0.095 0.143 303 | } 304 | ] 305 | } 306 | ] 307 | } 308 | physics Physics { 309 | density -1 310 | mass 0.825735 311 | centerOfMass [ 312 | -0.035 0 0.03 313 | ] 314 | } 315 | } 316 | } 317 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/protos/icons/TurtleBot3Burger.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_turtlebot/protos/icons/TurtleBot3Burger.png -------------------------------------------------------------------------------- /ros2webots_turtlebot/resource/TurtleBot3Burger.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/resource/ros2webots_turtlebot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_turtlebot/resource/ros2webots_turtlebot -------------------------------------------------------------------------------- /ros2webots_turtlebot/resource/turtleBotComponent.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 5 11 | /webots/turtlebot/lidar 12 | true 13 | LDS-01 14 | 15 | 16 | 17 | 18 | 19 | 20 | true 21 | 5 22 | /webots/turtlebot/compass 23 | true 24 | compass 25 | 26 | 27 | 28 | 29 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/ros2webots_turtlebot/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_turtlebot/ros2webots_turtlebot/__init__.py -------------------------------------------------------------------------------- /ros2webots_turtlebot/ros2webots_turtlebot/turtlebot_robot_driver.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | 3 | from geometry_msgs.msg import Twist 4 | 5 | HALF_DISTANCE_BETWEEN_WHEELS = 0.045 6 | WHEEL_RADIUS = 0.025 7 | 8 | class TurtleBotWebotsDriver(): 9 | def init(self, webots_node, properties): 10 | self.__robot = webots_node.robot 11 | 12 | self.__left_motor = self.__robot.getDevice('left wheel motor') 13 | self.__right_motor = self.__robot.getDevice('right wheel motor') 14 | 15 | self.__left_motor.setPosition(float('inf')) 16 | self.__left_motor.setVelocity(0) 17 | 18 | self.__right_motor.setPosition(float('inf')) 19 | self.__right_motor.setVelocity(0) 20 | 21 | self.__target_twist = Twist() 22 | 23 | rclpy.init(args=None) 24 | 25 | self.__node = rclpy.create_node('webots_turtle_bot_driver_node') 26 | self.__node.create_subscription(Twist, '/webots/turtlebot/command_vel', self.__cmd_vel_callback, 10) 27 | 28 | def __cmd_vel_callback(self, twist): 29 | self.__target_twist = twist 30 | 31 | def step(self): 32 | rclpy.spin_once(self.__node, timeout_sec=0) 33 | 34 | forward_speed = self.__target_twist.linear.x 35 | angular_speed = self.__target_twist.angular.z 36 | 37 | command_motor_left = (forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS 38 | command_motor_right = (forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / WHEEL_RADIUS 39 | 40 | self.__left_motor.setVelocity(command_motor_left) 41 | self.__right_motor.setVelocity(command_motor_right) -------------------------------------------------------------------------------- /ros2webots_turtlebot/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/ros2webots_turtlebot 3 | [install] 4 | install_scripts=$base/lib/ros2webots_turtlebot 5 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/setup.py: -------------------------------------------------------------------------------- 1 | # Lunch file import 2 | import os 3 | from glob import glob 4 | 5 | from setuptools import find_packages, setup 6 | 7 | package_name = 'ros2webots_turtlebot' 8 | data_files = [] 9 | data_files.append(('share/ament_index/resource_index/packages', [ 10 | 'resource/' + package_name 11 | ])) 12 | data_files.append(('share/' + package_name + '/launch', [ 13 | 'launch/test_launch.py', 14 | 'launch/turtlebot_world_launch.py', 15 | ])) 16 | data_files.append(('share/' + package_name + '/worlds', [ 17 | 'worlds/world_roboticknowledge.wbt', 18 | ])) 19 | # data_files.append(('share/' + package_name + '/protos', [ 20 | # 'protos/LegoTallInterval.proto', 21 | # ])) 22 | data_files.append(('share/' + package_name + '/resource', [ 23 | 'resource/TurtleBot3Burger.urdf', 24 | 'resource/turtleBotComponent.urdf', 25 | ])) 26 | data_files.append(('share/' + package_name, [ 27 | 'package.xml' 28 | ])) 29 | 30 | setup( 31 | name=package_name, 32 | version='0.0.0', 33 | packages=find_packages(exclude=['test']), 34 | data_files=data_files, 35 | install_requires=['setuptools'], 36 | zip_safe=True, 37 | maintainer='mjavadzallaghi', 38 | maintainer_email='mohammadjavadzallaghi@gmail.com', 39 | description='TODO: Package description', 40 | license='Apache-2.0', 41 | tests_require=['pytest'], 42 | entry_points={ 43 | 'console_scripts': [ 44 | ], 45 | }, 46 | ) 47 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/worlds/.world_roboticknowledge.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MJavadZallaghi/ros2webots/5a29c38fb8c97066abee961e577fca9a91fef266/ros2webots_turtlebot/worlds/.world_roboticknowledge.jpg -------------------------------------------------------------------------------- /ros2webots_turtlebot/worlds/.world_roboticknowledge.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2023b 2 | perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000262fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002620000008600ffffff000000030000077c00000161fc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000077c0000006900ffffff000005400000026200000004000000040000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff000000010000000200000169000003d50100000002010000000101 4 | sceneTreePerspectives: 000000ff00000001000000030000001e00000225000000fa0100000002010000000201 5 | maximizedDockId: -1 6 | centralWidgetVisible: 1 7 | orthographicViewHeight: 1 8 | textFiles: 0 "protos/TurtleBot3Burger.proto" 9 | globalOptionalRendering: LidarPointClouds 10 | consoles: Console:All:All 11 | -------------------------------------------------------------------------------- /ros2webots_turtlebot/worlds/world_roboticknowledge.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2023b utf8 2 | 3 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto" 4 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" 5 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/RectangleArena.proto" 6 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/turtlebot/protos/TurtleBot3Burger.proto" 7 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/plants/protos/PottedTree.proto" 8 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/tables/protos/Table.proto" 9 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/Parquetry.proto" 10 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/BrushedAluminium.proto" 11 | 12 | WorldInfo { 13 | } 14 | Viewpoint { 15 | orientation -0.26112552194043803 -0.09466842178053833 0.9606515245961543 3.752417594678755 16 | position 4.954843715464183 3.7908288287558847 3.477095723475976 17 | } 18 | TexturedBackground { 19 | } 20 | TexturedBackgroundLight { 21 | } 22 | RectangleArena { 23 | floorSize 5 5 24 | floorAppearance Parquetry { 25 | type "light strip" 26 | colorOverride 1 1 0.498039 27 | IBLStrength 10 28 | } 29 | wallHeight 0.3 30 | wallAppearance BrushedAluminium { 31 | colorOverride 0.666667 0.333333 0 32 | IBLStrength 22 33 | } 34 | } 35 | Table { 36 | translation -1.6 1.5 0 37 | } 38 | PottedTree { 39 | translation 1.86 -1.12 0 40 | } 41 | TurtleBot3Burger { 42 | translation 4.5464847164833344e-10 -5.214934642157418e-10 -0.0002477013487637847 43 | rotation -0.0021195456650272724 -0.9999977537491604 4.775707089984546e-06 0.004640846749703647 44 | name "TurtleBot3Burger_robot" 45 | controller "" 46 | } 47 | --------------------------------------------------------------------------------