├── LICENSE ├── README.md ├── cad_mobile_robot.gif ├── mobile_robot_gazebo.gif ├── navigation.gif ├── slam.gif └── src ├── robot_description ├── CMakeLists.txt ├── LICENSE ├── launch │ ├── display.launch.xml │ └── gazebo.launch.xml ├── meshes │ ├── base_link.dae │ ├── camera.dae │ ├── motor.dae │ ├── rplidar.dae │ ├── ubase_link.dae │ └── wheel.dae ├── package.xml ├── rviz │ ├── gazebo_config.rviz │ └── urdf_config.rviz └── urdf │ ├── camera.xacro │ ├── common_properties.xacro │ ├── lidar.xacro │ ├── mobile_robot.urdf.xacro │ ├── mobile_robot_base.xacro │ └── mobile_robot_gazebo.xacro ├── robot_patrol ├── LICENSE ├── package.xml ├── resource │ └── robot_patrol ├── robot_patrol │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-310.pyc │ │ └── house_patrol.cpython-310.pyc │ └── house_patrol.py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── robot_simulation ├── CMakeLists.txt ├── LICENSE ├── config ├── ekf.yaml └── nav2_config.rviz ├── launch ├── autonomous_navigation.launch.py ├── house_sim.launch.py └── house_slam.launch.py ├── maps ├── house_map.pgm └── house_map.yaml ├── package.xml ├── params └── nav2_params.yaml └── worlds └── house.world /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Alexandros Rafael Aposporis 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HomeBot-ROS2-Navigation 2 | HomeBot-ROS2-Navigation: A ROS2-powered robotic platform for autonomous indoor navigation and mapping. 3 | 4 | ![HomeBot Image](mobile_robot_gazebo.gif) 5 | 6 | ## Project Overview 7 | This repository contains a suite of ROS2 packages for the HomeBot, an autonomous robot designed for indoor navigation and mapping. The provided image showcases the HomeBot's physical design, which the following packages bring to life: 8 | 9 | - `robot_description`: Defines the robot's physical parameters, including its URDF files, visual meshes, and essential configurations for simulation purposes. 10 | - `robot_simulation`: Contains the necessary configurations and launch files for simulating the HomeBot in a household environment, using tools like Gazebo and RViz for SLAM and navigation testing. 11 | - `robot_patrol`: Implements a patrolling behavior, directing the robot to navigate autonomously through a series of predefined waypoints based on the generated map data. 12 | 13 | These packages represent the core components of the HomeBot's functionality, demonstrating practical applications of ROS2 in robotic indoor navigation. 14 | 15 | ## Getting Started 16 | ### Prerequisites 17 | **ROS2 (tested with ROS2 Humble)** - [Documentation](https://docs.ros.org/en/humble/index.html) 18 | 19 | **Python 3.10** 20 | 21 | **Nav2** - [Github](https://github.com/ros-planning/navigation2) and [Documentation](https://navigation.ros.org/) 22 | 23 | **Slam_toolbox** - [Github](https://github.com/SteveMacenski/slam_toolbox) 24 | 25 | ## Installation 26 | 27 | 1. **Create a ROS 2 workspace (if one does not already exist):** 28 | 29 | ```sh 30 | mkdir ~/your_workspace-name_ws 31 | cd ~/your_workspace-name_ws 32 | 33 | 2. **Clone this repository into your workspace:** 34 | 35 | ```sh 36 | git clone https://github.com/aimechengineer/HomeBot-ROS2-Navigation.git src 37 | 38 | 3. **Build the workspace:** 39 | ```sh 40 | cd ~/your_workspace-name_ws 41 | colcon build --symlink-install 42 | 43 | 4. **Source the setup script:** 44 | ```sh 45 | source ~/your_workspace-name_ws/install/setup.bash 46 | 47 | ## Additional Steps for Gazebo Simulation 48 | If Gazebo does not launch the simulation correctly, use the following commands to set up the robot_description package in the Gazebo models directory, keeping only the meshes directory: 49 | 50 | 1. **Copy the entire robot_description package to the Gazebo models directory** 51 | 52 | ```sh 53 | cp -r path_to_robot_description_package ~/.gazebo/models/robot_description 54 | Replace **path_to_robot_description_package** with the actual path to your **robot_description** package. 55 | 56 | 2. **Remove all contents from the copied robot_description package except for the meshes directory** 57 | 58 | Navigate to the robot_description directory in the Gazebo models directory: 59 | 60 | cd ~/.gazebo/models/robot_description 61 | 62 | Then, remove all directories except for meshes: 63 | 64 | rm -rf !(meshes) 65 | This process will ensure that only the meshes directory is retained in the robot_description package within the Gazebo models directory, which should help resolve any simulation launch issues. 66 | 67 | ## Usage 68 | ### robot_description 69 | **Display HomeBot in RViz:** 70 | 71 | ros2 launch robot_description display.launch.xml 72 | 73 | **Display HomeBot in Gazebo and Rviz:** 74 | 75 | ros2 launch robot_description gazebo.launch.xml 76 | 77 | ### robot_simulation and robot_patrol 78 | #### SLAM Process 79 | Perform SLAM and generate a map of the environment: 80 | 81 | 1. **Launch the SLAM simulation:** 82 | Start the SLAM Process: 83 | 84 | ros2 launch robot_simulation house_slam.launch.py 85 | 86 | 3. **Control HomeBot:** 87 | Use teleop to manually control HomeBot during SLAM: 88 | 89 | ros2 run teleop_twist_keyboard teleop_twist_keyboard 90 | 91 | ![HomeBot Image](slam.gif) 92 | 93 | #### Autonomous Navigation 94 | Navigate autonomously in the environment post-SLAM: 95 | 1. **Launch Gazebo and RViz with Nav2 Configuration**: 96 | To initialize the simulation environment in Gazebo along with RViz configured for Nav2, use the following launch command: 97 | 98 | ros2 launch robot_simulation house_sim.launch.py 99 | 100 | 3. **Start Navigation:** 101 | Launch the navigation stack: 102 | 103 | ros2 launch robot_simulation autonomous_navigation.launch.py 104 | 105 | Once the 2D Pose Estimate is set in RViz, HomeBot can autonomously navigate the environment. Alternatively, you can start the patrol mode: 106 | 107 | ros2 run robot_patrol robot_patrol 108 | In patrol mode, HomeBot will navigate to predefined locations autonomously. 109 | 110 | ![HomeBot Image](navigation.gif) 111 | 112 | ## License 113 | 114 | This project is licensed under the MIT License - see the ![LICENSE](LICENSE) file for details. 115 | -------------------------------------------------------------------------------- /cad_mobile_robot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/cad_mobile_robot.gif -------------------------------------------------------------------------------- /mobile_robot_gazebo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/mobile_robot_gazebo.gif -------------------------------------------------------------------------------- /navigation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/navigation.gif -------------------------------------------------------------------------------- /slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/slam.gif -------------------------------------------------------------------------------- /src/robot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_description) 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(xacro REQUIRED) 11 | find_package(urdf REQUIRED) 12 | find_package(gazebo_ros REQUIRED) 13 | 14 | install( 15 | DIRECTORY launch urdf meshes rviz 16 | DESTINATION share/${PROJECT_NAME} 17 | ) 18 | 19 | ament_package() 20 | -------------------------------------------------------------------------------- /src/robot_description/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Alexandros Rafael Aposporis 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/robot_description/launch/display.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 17 | -------------------------------------------------------------------------------- /src/robot_description/launch/gazebo.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 20 | -------------------------------------------------------------------------------- /src/robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_description 5 | 0.0.0 6 | 7 | The 'robot_description' package provides a comprehensive model of our custom robot, 8 | essential for simulation and visualization in ROS2 environments. It includes a detailed 9 | URDF (Unified Robot Description Format) file that specifies the robot's physical 10 | structure, joint configurations, and kinematic properties. Accompanying this URDF 11 | are Collada (.dae) format meshes derived from CAD files, offering a realistic visual 12 | representation of the robot. Additionally, this package encompasses RViz configuration 13 | files for effective visualization and debugging, alongside launch files for easy 14 | integration and testing within ROS2 frameworks. This package is pivotal for the robot's 15 | simulation and serves as a foundation for further development in autonomous navigation 16 | and SLAM (Simultaneous Localization and Mapping). 17 | 18 | 19 | Alexandros Rafael Aposporis - aimechengineer 20 | MIT 21 | 22 | ament_cmake 23 | joint_state_publisher 24 | robot_state_publisher 25 | gazebo_ros 26 | xacro 27 | rviz 28 | robot_localization 29 | 30 | 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/robot_description/rviz/gazebo_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 355 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Class: rviz_default_plugins/TF 51 | Enabled: true 52 | Frame Timeout: 15 53 | Frames: 54 | All Enabled: true 55 | base_footprint: 56 | Value: true 57 | base_link: 58 | Value: true 59 | camera_link: 60 | Value: true 61 | camera_link_optical: 62 | Value: true 63 | imu_link: 64 | Value: true 65 | laser_frame: 66 | Value: true 67 | left_motor_link: 68 | Value: true 69 | left_wheel_link: 70 | Value: true 71 | lidar_link: 72 | Value: true 73 | odom: 74 | Value: true 75 | right_motor_link: 76 | Value: true 77 | right_wheel_link: 78 | Value: true 79 | Marker Scale: 1 80 | Name: TF 81 | Show Arrows: true 82 | Show Axes: true 83 | Show Names: false 84 | Tree: 85 | base_footprint: 86 | base_link: 87 | camera_link: 88 | camera_link_optical: 89 | {} 90 | imu_link: 91 | {} 92 | left_motor_link: 93 | {} 94 | lidar_link: 95 | laser_frame: 96 | {} 97 | right_motor_link: 98 | {} 99 | left_wheel_link: 100 | {} 101 | right_wheel_link: 102 | {} 103 | odom: 104 | {} 105 | Update Interval: 0 106 | Value: true 107 | - Alpha: 1 108 | Class: rviz_default_plugins/RobotModel 109 | Collision Enabled: false 110 | Description File: "" 111 | Description Source: Topic 112 | Description Topic: 113 | Depth: 5 114 | Durability Policy: Volatile 115 | History Policy: Keep Last 116 | Reliability Policy: Reliable 117 | Value: /robot_description 118 | Enabled: true 119 | Links: 120 | All Links Enabled: true 121 | Expand Joint Details: false 122 | Expand Link Details: false 123 | Expand Tree: false 124 | Link Tree Style: Links in Alphabetic Order 125 | base_footprint: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | base_link: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | camera_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | camera_link_optical: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | imu_link: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | laser_frame: 148 | Alpha: 1 149 | Show Axes: false 150 | Show Trail: false 151 | left_motor_link: 152 | Alpha: 1 153 | Show Axes: false 154 | Show Trail: false 155 | Value: true 156 | left_wheel_link: 157 | Alpha: 1 158 | Show Axes: false 159 | Show Trail: false 160 | Value: true 161 | lidar_link: 162 | Alpha: 1 163 | Show Axes: false 164 | Show Trail: false 165 | Value: true 166 | right_motor_link: 167 | Alpha: 1 168 | Show Axes: false 169 | Show Trail: false 170 | Value: true 171 | right_wheel_link: 172 | Alpha: 1 173 | Show Axes: false 174 | Show Trail: false 175 | Value: true 176 | Mass Properties: 177 | Inertia: false 178 | Mass: false 179 | Name: RobotModel 180 | TF Prefix: "" 181 | Update Interval: 0 182 | Value: true 183 | Visual Enabled: true 184 | - Alpha: 1 185 | Autocompute Intensity Bounds: true 186 | Autocompute Value Bounds: 187 | Max Value: 10 188 | Min Value: -10 189 | Value: true 190 | Axis: Z 191 | Channel Name: intensity 192 | Class: rviz_default_plugins/LaserScan 193 | Color: 255; 255; 255 194 | Color Transformer: Intensity 195 | Decay Time: 0 196 | Enabled: true 197 | Invert Rainbow: false 198 | Max Color: 255; 255; 255 199 | Max Intensity: -999999 200 | Min Color: 0; 0; 0 201 | Min Intensity: 999999 202 | Name: LaserScan 203 | Position Transformer: XYZ 204 | Selectable: true 205 | Size (Pixels): 3 206 | Size (m): 0.009999999776482582 207 | Style: Flat Squares 208 | Topic: 209 | Depth: 5 210 | Durability Policy: Volatile 211 | Filter size: 10 212 | History Policy: Keep Last 213 | Reliability Policy: Reliable 214 | Value: /scan 215 | Use Fixed Frame: true 216 | Use rainbow: true 217 | Value: true 218 | - Class: rviz_default_plugins/Image 219 | Enabled: true 220 | Max Value: 1 221 | Median window: 5 222 | Min Value: 0 223 | Name: Image 224 | Normalize Range: true 225 | Topic: 226 | Depth: 5 227 | Durability Policy: Volatile 228 | History Policy: Keep Last 229 | Reliability Policy: Reliable 230 | Value: /camera_sensor/image_raw 231 | Value: true 232 | Enabled: true 233 | Global Options: 234 | Background Color: 48; 48; 48 235 | Fixed Frame: base_footprint 236 | Frame Rate: 30 237 | Name: root 238 | Tools: 239 | - Class: rviz_default_plugins/Interact 240 | Hide Inactive Objects: true 241 | - Class: rviz_default_plugins/MoveCamera 242 | - Class: rviz_default_plugins/Select 243 | - Class: rviz_default_plugins/FocusCamera 244 | - Class: rviz_default_plugins/Measure 245 | Line color: 128; 128; 0 246 | - Class: rviz_default_plugins/SetInitialPose 247 | Covariance x: 0.25 248 | Covariance y: 0.25 249 | Covariance yaw: 0.06853891909122467 250 | Topic: 251 | Depth: 5 252 | Durability Policy: Volatile 253 | History Policy: Keep Last 254 | Reliability Policy: Reliable 255 | Value: /initialpose 256 | - Class: rviz_default_plugins/SetGoal 257 | Topic: 258 | Depth: 5 259 | Durability Policy: Volatile 260 | History Policy: Keep Last 261 | Reliability Policy: Reliable 262 | Value: /goal_pose 263 | - Class: rviz_default_plugins/PublishPoint 264 | Single click: true 265 | Topic: 266 | Depth: 5 267 | Durability Policy: Volatile 268 | History Policy: Keep Last 269 | Reliability Policy: Reliable 270 | Value: /clicked_point 271 | Transformation: 272 | Current: 273 | Class: rviz_default_plugins/TF 274 | Value: true 275 | Views: 276 | Current: 277 | Class: rviz_default_plugins/Orbit 278 | Distance: 4.634582042694092 279 | Enable Stereo Rendering: 280 | Stereo Eye Separation: 0.05999999865889549 281 | Stereo Focal Distance: 1 282 | Swap Stereo Eyes: false 283 | Value: false 284 | Focal Point: 285 | X: -0.2596261203289032 286 | Y: 0.11999500542879105 287 | Z: 0.3547627031803131 288 | Focal Shape Fixed Size: true 289 | Focal Shape Size: 0.05000000074505806 290 | Invert Z Axis: false 291 | Name: Current View 292 | Near Clip Distance: 0.009999999776482582 293 | Pitch: 0.3153699040412903 294 | Target Frame: 295 | Value: Orbit (rviz_default_plugins) 296 | Yaw: 0.2613753080368042 297 | Saved: ~ 298 | Window Geometry: 299 | Displays: 300 | collapsed: false 301 | Height: 846 302 | Hide Left Dock: false 303 | Hide Right Dock: false 304 | Image: 305 | collapsed: false 306 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001ee000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000231000000bc0000002800ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 307 | Selection: 308 | collapsed: false 309 | Time: 310 | collapsed: false 311 | Tool Properties: 312 | collapsed: false 313 | Views: 314 | collapsed: false 315 | Width: 1200 316 | X: 720 317 | Y: 52 318 | -------------------------------------------------------------------------------- /src/robot_description/rviz/urdf_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | Splitter Ratio: 0.5 11 | Tree Height: 549 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz_default_plugins/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | All Enabled: true 56 | base_footprint: 57 | Value: true 58 | base_link: 59 | Value: true 60 | Marker Scale: 1 61 | Name: TF 62 | Show Arrows: true 63 | Show Axes: true 64 | Show Names: false 65 | Tree: 66 | base_footprint: 67 | base_link: 68 | {} 69 | Update Interval: 0 70 | Value: true 71 | - Alpha: 1 72 | Class: rviz_default_plugins/RobotModel 73 | Collision Enabled: false 74 | Description File: "" 75 | Description Source: Topic 76 | Description Topic: 77 | Depth: 5 78 | Durability Policy: Volatile 79 | History Policy: Keep Last 80 | Reliability Policy: Reliable 81 | Value: /robot_description 82 | Enabled: true 83 | Links: 84 | All Links Enabled: true 85 | Expand Joint Details: false 86 | Expand Link Details: false 87 | Expand Tree: false 88 | Link Tree Style: Links in Alphabetic Order 89 | base_footprint: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | base_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | Mass Properties: 99 | Inertia: false 100 | Mass: false 101 | Name: RobotModel 102 | TF Prefix: "" 103 | Update Interval: 0 104 | Value: true 105 | Visual Enabled: true 106 | Enabled: true 107 | Global Options: 108 | Background Color: 48; 48; 48 109 | Fixed Frame: base_footprint 110 | Frame Rate: 30 111 | Name: root 112 | Tools: 113 | - Class: rviz_default_plugins/Interact 114 | Hide Inactive Objects: true 115 | - Class: rviz_default_plugins/MoveCamera 116 | - Class: rviz_default_plugins/Select 117 | - Class: rviz_default_plugins/FocusCamera 118 | - Class: rviz_default_plugins/Measure 119 | Line color: 128; 128; 0 120 | - Class: rviz_default_plugins/SetInitialPose 121 | Covariance x: 0.25 122 | Covariance y: 0.25 123 | Covariance yaw: 0.06853891909122467 124 | Topic: 125 | Depth: 5 126 | Durability Policy: Volatile 127 | History Policy: Keep Last 128 | Reliability Policy: Reliable 129 | Value: /initialpose 130 | - Class: rviz_default_plugins/SetGoal 131 | Topic: 132 | Depth: 5 133 | Durability Policy: Volatile 134 | History Policy: Keep Last 135 | Reliability Policy: Reliable 136 | Value: /goal_pose 137 | - Class: rviz_default_plugins/PublishPoint 138 | Single click: true 139 | Topic: 140 | Depth: 5 141 | Durability Policy: Volatile 142 | History Policy: Keep Last 143 | Reliability Policy: Reliable 144 | Value: /clicked_point 145 | Transformation: 146 | Current: 147 | Class: rviz_default_plugins/TF 148 | Value: true 149 | Views: 150 | Current: 151 | Class: rviz_default_plugins/Orbit 152 | Distance: 4.634582042694092 153 | Enable Stereo Rendering: 154 | Stereo Eye Separation: 0.05999999865889549 155 | Stereo Focal Distance: 1 156 | Swap Stereo Eyes: false 157 | Value: false 158 | Focal Point: 159 | X: -0.2596261203289032 160 | Y: 0.11999500542879105 161 | Z: 0.3547627031803131 162 | Focal Shape Fixed Size: true 163 | Focal Shape Size: 0.05000000074505806 164 | Invert Z Axis: false 165 | Name: Current View 166 | Near Clip Distance: 0.009999999776482582 167 | Pitch: 0.3153699040412903 168 | Target Frame: 169 | Value: Orbit (rviz_default_plugins) 170 | Yaw: 0.2613753080368042 171 | Saved: ~ 172 | Window Geometry: 173 | Displays: 174 | collapsed: false 175 | Height: 846 176 | Hide Left Dock: false 177 | Hide Right Dock: false 178 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 179 | Selection: 180 | collapsed: false 181 | Time: 182 | collapsed: false 183 | Tool Properties: 184 | collapsed: false 185 | Views: 186 | collapsed: false 187 | Width: 1200 188 | X: 720 189 | Y: 52 190 | -------------------------------------------------------------------------------- /src/robot_description/urdf/camera.xacro: -------------------------------------------------------------------------------- 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 | Gazebo/Gray 41 | 42 | 0 0 0 0 0 0 43 | true 44 | 10.0 45 | 46 | ${60*pi/180} 47 | 48 | 640 49 | 480 50 | 51 | 52 | 0.1 53 | 300 54 | 55 | 56 | 57 | camera_link_optical 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /src/robot_description/urdf/common_properties.xacro: -------------------------------------------------------------------------------- 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 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/robot_description/urdf/lidar.xacro: -------------------------------------------------------------------------------- 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 | 0 0 0 0 0 0 40 | true 41 | 10 42 | 43 | 44 | 45 | 720 46 | -3.14 47 | 3.14 48 | 49 | 50 | 51 | 0.1 52 | 12 53 | 54 | 55 | 56 | 57 | ~/out:=scan 58 | 59 | sensor_msgs/LaserScan 60 | laser_frame 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/robot_description/urdf/mobile_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/robot_description/urdf/mobile_robot_base.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 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 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 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 | -------------------------------------------------------------------------------- /src/robot_description/urdf/mobile_robot_gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Gazebo/Black 6 | 7 | 8 | 9 | 10 | 11 | Gazebo/Red 12 | 13 | 14 | 15 | Gazebo/Red 16 | 17 | 18 | 19 | Gazebo/Grey 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | Gazebo/Grey 28 | 29 | 30 | 31 | 32 | 33 | 34 | left_motor_wheel_joint 35 | right_motor_wheel_joint 36 | 37 | ${scale_num*0.057504} 38 | ${0.072*scale_num} 39 | 40 | 20 41 | 1.0 42 | 43 | odom 44 | base_footprint 45 | 46 | true 47 | true 48 | true 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /src/robot_patrol/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Alexandros Rafael Aposporis 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/robot_patrol/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_patrol 5 | 0.0.0 6 | 7 | The 'robot_patrol' package embodies the practical application of autonomous navigation in a household setting, bringing to life the concept of a robot reliably patrolling a defined area. At its core is a Python-based node that seamlessly blends with ROS2's navigation framework. This node acts as a commander, guiding the robot along a series of predetermined waypoints, effectively 'patrolling' the house. 8 | This package showcases an elegant integration of custom path planning with ROS2's sophisticated navigation capabilities. It's not just about moving from point A to point B; it's about doing so with precision, efficiency, and adaptability. The waypoints are strategically chosen to ensure comprehensive coverage of the environment, demonstrating the robot's ability to navigate complex indoor spaces. 9 | Moreover, the 'robot_patrol' package serves as a testament to the flexibility of ROS2 systems in accommodating custom logic and commands, extending beyond standard navigation tasks. This flexibility is crucial for designing robots capable of performing specific missions, such as surveillance, routine inspections, or even interactive guides in domestic settings. 10 | In essence, 'robot_patrol' is where the theoretical aspects of robotics meet practical application, highlighting a significant step towards autonomous, intelligent robotic systems in everyday environments. 11 | 12 | 13 | Alexandros Rafael Aposporis - aimechengineer 14 | MIT 15 | 16 | ament_copyright 17 | ament_flake8 18 | ament_pep257 19 | python3-pytest 20 | nav2_simple_commander 21 | geometry_msgs 22 | tf2_ros 23 | rclpy 24 | 25 | 26 | ament_python 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/robot_patrol/resource/robot_patrol: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/src/robot_patrol/resource/robot_patrol -------------------------------------------------------------------------------- /src/robot_patrol/robot_patrol/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/src/robot_patrol/robot_patrol/__init__.py -------------------------------------------------------------------------------- /src/robot_patrol/robot_patrol/__pycache__/__init__.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/src/robot_patrol/robot_patrol/__pycache__/__init__.cpython-310.pyc -------------------------------------------------------------------------------- /src/robot_patrol/robot_patrol/__pycache__/house_patrol.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/src/robot_patrol/robot_patrol/__pycache__/house_patrol.cpython-310.pyc -------------------------------------------------------------------------------- /src/robot_patrol/robot_patrol/house_patrol.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | from nav2_simple_commander.robot_navigator import BasicNavigator 5 | from geometry_msgs.msg import PoseStamped 6 | import tf_transformations 7 | import numpy as np 8 | 9 | class HousePatrolNode(Node): 10 | def __init__(self): 11 | super().__init__('house_patrol') 12 | self.navigator = BasicNavigator() 13 | 14 | # Set initial pose 15 | initial_pose = self.create_pose_stamped(0.0, 0.0, 0.0) 16 | self.navigator.setInitialPose(initial_pose) 17 | 18 | # Wait for Nav2 to be active 19 | self.navigator.waitUntilNav2Active() 20 | 21 | # Set waypoints and start navigation 22 | waypoints = [ 23 | self.create_pose_stamped(4.0, 2.5, np.radians(0)), 24 | self.create_pose_stamped(2.5, 2.5, np.radians(90)), 25 | self.create_pose_stamped(2.5, 7.0, np.radians(-130.60)), 26 | self.create_pose_stamped(-3.5, 0.0, np.radians(11.31)), 27 | self.create_pose_stamped(9.0, 2.5, np.radians(155.56)), 28 | self.create_pose_stamped(3.5, 5.0, np.radians(-125.00)), 29 | self.create_pose_stamped(0.0, 0.0, 0.0) # Assuming the robot stops facing the original direction 30 | ] 31 | self.follow_waypoints(waypoints) 32 | 33 | def create_pose_stamped(self, position_x, position_y, orientation_z): 34 | q_x, q_y, q_z, q_w = tf_transformations.quaternion_from_euler(0.0, 0.0, orientation_z) 35 | pose = PoseStamped() 36 | pose.header.frame_id = 'map' 37 | pose.header.stamp = self.navigator.get_clock().now().to_msg() 38 | pose.pose.position.x = position_x 39 | pose.pose.position.y = position_y 40 | pose.pose.position.z = 0.0 41 | pose.pose.orientation.x = q_x 42 | pose.pose.orientation.y = q_y 43 | pose.pose.orientation.z = q_z 44 | pose.pose.orientation.w = q_w 45 | return pose 46 | 47 | def follow_waypoints(self, waypoints): 48 | self.navigator.followWaypoints(waypoints) 49 | while not self.navigator.isTaskComplete(): 50 | feedback = self.navigator.getFeedback() 51 | self.get_logger().info('Navigation Feedback: %s' % feedback) 52 | result = self.navigator.getResult() 53 | self.get_logger().info('Navigation Result: %s' % result) 54 | 55 | def main(args=None): 56 | rclpy.init(args=args) 57 | node = HousePatrolNode() 58 | try: 59 | rclpy.spin(node) 60 | except KeyboardInterrupt: 61 | pass 62 | finally: 63 | node.destroy_node() 64 | rclpy.shutdown() 65 | 66 | if __name__ == '__main__': 67 | main() 68 | -------------------------------------------------------------------------------- /src/robot_patrol/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/robot_patrol 3 | [install] 4 | install_scripts=$base/lib/robot_patrol 5 | -------------------------------------------------------------------------------- /src/robot_patrol/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = 'robot_patrol' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=find_packages(exclude=['test']), 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='eeengineer', 17 | maintainer_email='aposporisalexander@gmail.com', 18 | description='TODO: Package description', 19 | license='TODO: License declaration', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'robot_patrol = robot_patrol.house_patrol:main', 24 | ], 25 | }, 26 | ) 27 | -------------------------------------------------------------------------------- /src/robot_patrol/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 | -------------------------------------------------------------------------------- /src/robot_patrol/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 | -------------------------------------------------------------------------------- /src/robot_patrol/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 | -------------------------------------------------------------------------------- /src/robot_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_simulation) 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 | 11 | 12 | install( 13 | DIRECTORY launch config worlds maps params 14 | DESTINATION share/${PROJECT_NAME} 15 | ) 16 | 17 | 18 | ament_package() 19 | -------------------------------------------------------------------------------- /src/robot_simulation/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Alexandros Rafael Aposporis 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/robot_simulation/config/ekf.yaml: -------------------------------------------------------------------------------- 1 | ### ekf config file ### 2 | ekf_filter_node: 3 | ros__parameters: 4 | 5 | use_sim_time: true 6 | 7 | 8 | # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin 9 | # computation until it receives at least one message from one of the inputs. It will then run continuously at the 10 | # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. 11 | frequency: 30.0 12 | 13 | # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict 14 | # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the 15 | # filter will generate new output. Defaults to 1 / frequency if not specified. 16 | sensor_timeout: 0.1 17 | 18 | # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is 19 | # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar 20 | # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected 21 | # by, for example, an IMU. Defaults to false if unspecified. 22 | two_d_mode: true 23 | 24 | # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for 25 | # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if 26 | # unspecified. 27 | transform_time_offset: 0.0 28 | 29 | # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 30 | # Defaults to 0.0 if unspecified. 31 | transform_timeout: 0.0 32 | 33 | # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is 34 | # unhappy with any settings or data. 35 | print_diagnostics: true 36 | 37 | # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by 38 | # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious 39 | # effects on the performance of the node. Defaults to false if unspecified. 40 | debug: false 41 | 42 | # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. 43 | debug_out_file: /path/to/debug/file.txt 44 | 45 | # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. 46 | publish_tf: true 47 | 48 | # Whether to publish the acceleration state. Defaults to false if unspecified. 49 | publish_acceleration: false 50 | 51 | # If the filter sees a jump back in time, the filter is reset (convenient for testing with rosbags!) 52 | reset_on_time_jump: true 53 | 54 | # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and 55 | # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. 56 | # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be 57 | # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom 58 | # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your 59 | # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based 60 | # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. 61 | # ekf_localization_node and ukf_localization_node are not concerned with the earth frame. 62 | # Here is how to use the following settings: 63 | # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. 64 | # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of 65 | # odom_frame. 66 | # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set 67 | # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. 68 | # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates 69 | # from landmark observations) then: 70 | # 3a. Set your "world_frame" to your map_frame value 71 | # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state 72 | # estimation node from robot_localization! However, that instance should *not* fuse the global data. 73 | map_frame: map # Defaults to "map" if unspecified 74 | odom_frame: odom # Defaults to "odom" if unspecified 75 | base_link_frame: base_footprint # Defaults to "base_link" if unspecified 76 | world_frame: odom # Defaults to the value of odom_frame if unspecified 77 | 78 | # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, 79 | # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, 80 | # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, 81 | # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no 82 | # default values, and must be specified. 83 | odom0: wheel/odometry 84 | 85 | # Each sensor reading updates some or all of the filter's state. These options give you greater control over which 86 | # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only 87 | # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the 88 | # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types 89 | # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message 90 | # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false 91 | # if unspecified, effectively making this parameter required for each sensor. 92 | odom0_config: [true, true, true, 93 | false, false, false, 94 | true, true, true, 95 | false, false, true, 96 | false, false, false] 97 | 98 | # [x_pos , y_pos , z_pos, 99 | # roll , pitch , yaw, 100 | # x_vel , y_vel , z_vel, 101 | # roll_vel, pitch_vel, yaw_vel, 102 | # x_accel , y_accel , z_accel] 103 | 104 | # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase 105 | # the size of the subscription queue so that more measurements are fused. 106 | odom0_queue_size: 2 107 | 108 | # [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result 109 | # of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's 110 | # algorithm. 111 | odom0_nodelay: false 112 | 113 | # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- 114 | # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they 115 | # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also 116 | # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't 117 | # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose 118 | # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then 119 | # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true 120 | # for twist measurements has no effect. 121 | odom0_differential: false 122 | 123 | # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" 124 | # for all future measurements. While you can achieve the same effect with the differential paremeter, the key 125 | # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before 126 | # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. 127 | odom0_relative: false 128 | 129 | # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to 130 | # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to 131 | # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not 132 | # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. 133 | # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying 134 | # the thresholds. 135 | odom0_pose_rejection_threshold: 5.0 136 | odom0_twist_rejection_threshold: 1.0 137 | 138 | imu0: imu/data 139 | imu0_config: [false, false, false, 140 | true, true, true, 141 | false, false, false, 142 | true, true, true, 143 | true, true, true] 144 | 145 | # [x_pos , y_pos , z_pos, 146 | # roll , pitch , yaw, 147 | # x_vel , y_vel , z_vel, 148 | # roll_vel, pitch_vel, yaw_vel, 149 | # x_accel , y_accel , z_accel] 150 | 151 | imu0_nodelay: false 152 | imu0_differential: false 153 | imu0_relative: true 154 | imu0_queue_size: 7 155 | imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names 156 | imu0_twist_rejection_threshold: 0.8 # 157 | imu0_linear_acceleration_rejection_threshold: 0.8 # 158 | 159 | # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set 160 | # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. 161 | imu0_remove_gravitational_acceleration: true 162 | 163 | # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no 164 | # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During 165 | # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be 166 | # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When 167 | # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially 168 | # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance 169 | # for the velocity variable in question, or decrease the variance of the variable in question in the measurement 170 | # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we 171 | # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during 172 | # predicition. Note that if an acceleration measurement for the variable in question is available from one of the 173 | # inputs, the control term will be ignored. 174 | # Whether or not we use the control input during predicition. Defaults to false. 175 | use_control: false 176 | # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to 177 | # false. 178 | stamped_control: false 179 | # The last issued control command will be used in prediction for this period. Defaults to 0.2. 180 | control_timeout: 0.2 181 | # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. 182 | control_config: [true, false, false, false, false, true] 183 | # Places limits on how large the acceleration term will be. Should match your robot's kinematics. 184 | acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] 185 | # Acceleration and deceleration limits are not always the same for robots. 186 | deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] 187 | # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these 188 | # gains 189 | acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] 190 | # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these 191 | # gains 192 | deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] 193 | # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is 194 | # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each 195 | # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. 196 | # However, if users find that a given variable is slow to converge, one approach is to increase the 197 | # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error 198 | # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are 199 | # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if 200 | # unspecified. 201 | process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 202 | 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 203 | 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 204 | 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 205 | 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 206 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 207 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 208 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 209 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 210 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 211 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 212 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 213 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 214 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 215 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] 216 | # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal 217 | # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in 218 | # question. Users should take care not to use large values for variables that will not be measured directly. The values 219 | # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below 220 | #if unspecified. 221 | initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 222 | 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 223 | 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 224 | 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 225 | 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 226 | 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 227 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 228 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 229 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 230 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 231 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 232 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 233 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 234 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 235 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] 236 | -------------------------------------------------------------------------------- /src/robot_simulation/config/nav2_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /TF1 11 | - /TF1/Frames1 12 | - /TF1/Tree1 13 | - /LaserScan1 14 | - /Map1 15 | - /Global Planner1 16 | - /Global Planner1/Global Costmap1 17 | - /Global Planner1/Path1 18 | - /Controller1 19 | - /Controller1/Local Costmap1 20 | - /Controller1/Local Plan1 21 | - /Controller1/Polygon1 22 | - /MarkerArray1 23 | - /RobotModel1 24 | - /RobotModel1/Status1 25 | Splitter Ratio: 0.5833333134651184 26 | Tree Height: 481 27 | - Class: rviz_common/Selection 28 | Name: Selection 29 | - Class: rviz_common/Tool Properties 30 | Expanded: 31 | - /Publish Point1 32 | Name: Tool Properties 33 | Splitter Ratio: 0.5886790156364441 34 | - Class: rviz_common/Views 35 | Expanded: 36 | - /Current View1 37 | Name: Views 38 | Splitter Ratio: 0.5 39 | - Class: nav2_rviz_plugins/Navigation 2 40 | Name: Navigation 2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz_default_plugins/Grid 47 | Color: 160; 160; 164 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.029999999329447746 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: 0 58 | Plane: XY 59 | Plane Cell Count: 10 60 | Reference Frame: 61 | Value: true 62 | - Class: rviz_default_plugins/TF 63 | Enabled: true 64 | Frame Timeout: 15 65 | Frames: 66 | All Enabled: true 67 | base_footprint: 68 | Value: true 69 | base_link: 70 | Value: true 71 | drivewhl_l_link: 72 | Value: true 73 | drivewhl_r_link: 74 | Value: true 75 | front_caster: 76 | Value: true 77 | gps_link: 78 | Value: true 79 | imu_link: 80 | Value: true 81 | lidar_link: 82 | Value: true 83 | map: 84 | Value: true 85 | odom: 86 | Value: true 87 | Marker Scale: 1 88 | Name: TF 89 | Show Arrows: true 90 | Show Axes: true 91 | Show Names: false 92 | Tree: 93 | map: 94 | odom: 95 | base_footprint: 96 | base_link: 97 | drivewhl_l_link: 98 | {} 99 | drivewhl_r_link: 100 | {} 101 | front_caster: 102 | {} 103 | gps_link: 104 | {} 105 | imu_link: 106 | {} 107 | lidar_link: 108 | {} 109 | Update Interval: 0 110 | Value: true 111 | - Alpha: 1 112 | Autocompute Intensity Bounds: true 113 | Autocompute Value Bounds: 114 | Max Value: 10 115 | Min Value: -10 116 | Value: true 117 | Axis: Z 118 | Channel Name: intensity 119 | Class: rviz_default_plugins/LaserScan 120 | Color: 255; 255; 255 121 | Color Transformer: Intensity 122 | Decay Time: 0 123 | Enabled: true 124 | Invert Rainbow: false 125 | Max Color: 255; 255; 255 126 | Max Intensity: -999999 127 | Min Color: 0; 0; 0 128 | Min Intensity: 999999 129 | Name: LaserScan 130 | Position Transformer: XYZ 131 | Selectable: true 132 | Size (Pixels): 3 133 | Size (m): 0.009999999776482582 134 | Style: Flat Squares 135 | Topic: 136 | Depth: 5 137 | Durability Policy: Volatile 138 | History Policy: Keep Last 139 | Reliability Policy: Best Effort 140 | Value: /scan 141 | Use Fixed Frame: true 142 | Use rainbow: true 143 | Value: true 144 | - Alpha: 1 145 | Class: rviz_default_plugins/Map 146 | Color Scheme: map 147 | Draw Behind: true 148 | Enabled: true 149 | Name: Map 150 | Topic: 151 | Depth: 1 152 | Durability Policy: Transient Local 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /map 156 | Update Topic: 157 | Depth: 5 158 | Durability Policy: Volatile 159 | History Policy: Keep Last 160 | Reliability Policy: Reliable 161 | Value: /map_updates 162 | Use Timestamp: false 163 | Value: true 164 | - Class: rviz_common/Group 165 | Displays: 166 | - Alpha: 0.30000001192092896 167 | Class: rviz_default_plugins/Map 168 | Color Scheme: costmap 169 | Draw Behind: false 170 | Enabled: true 171 | Name: Global Costmap 172 | Topic: 173 | Depth: 1 174 | Durability Policy: Transient Local 175 | History Policy: Keep Last 176 | Reliability Policy: Reliable 177 | Value: /global_costmap/costmap 178 | Update Topic: 179 | Depth: 5 180 | Durability Policy: Volatile 181 | History Policy: Keep Last 182 | Reliability Policy: Reliable 183 | Value: /global_costmap/costmap_updates 184 | Use Timestamp: false 185 | Value: true 186 | - Alpha: 1 187 | Buffer Length: 1 188 | Class: rviz_default_plugins/Path 189 | Color: 255; 0; 0 190 | Enabled: true 191 | Head Diameter: 0.019999999552965164 192 | Head Length: 0.019999999552965164 193 | Length: 0.30000001192092896 194 | Line Style: Lines 195 | Line Width: 0.029999999329447746 196 | Name: Path 197 | Offset: 198 | X: 0 199 | Y: 0 200 | Z: 0 201 | Pose Color: 255; 85; 255 202 | Pose Style: Arrows 203 | Radius: 0.029999999329447746 204 | Shaft Diameter: 0.004999999888241291 205 | Shaft Length: 0.019999999552965164 206 | Topic: 207 | Depth: 5 208 | Durability Policy: Volatile 209 | History Policy: Keep Last 210 | Reliability Policy: Reliable 211 | Value: /plan 212 | Value: true 213 | Enabled: true 214 | Name: Global Planner 215 | - Class: rviz_common/Group 216 | Displays: 217 | - Alpha: 0.699999988079071 218 | Class: rviz_default_plugins/Map 219 | Color Scheme: costmap 220 | Draw Behind: false 221 | Enabled: true 222 | Name: Local Costmap 223 | Topic: 224 | Depth: 1 225 | Durability Policy: Transient Local 226 | History Policy: Keep Last 227 | Reliability Policy: Reliable 228 | Value: /local_costmap/costmap 229 | Update Topic: 230 | Depth: 5 231 | Durability Policy: Volatile 232 | History Policy: Keep Last 233 | Reliability Policy: Reliable 234 | Value: /local_costmap/costmap_updates 235 | Use Timestamp: false 236 | Value: true 237 | - Alpha: 1 238 | Buffer Length: 1 239 | Class: rviz_default_plugins/Path 240 | Color: 0; 12; 255 241 | Enabled: true 242 | Head Diameter: 0.30000001192092896 243 | Head Length: 0.20000000298023224 244 | Length: 0.30000001192092896 245 | Line Style: Lines 246 | Line Width: 0.029999999329447746 247 | Name: Local Plan 248 | Offset: 249 | X: 0 250 | Y: 0 251 | Z: 0 252 | Pose Color: 255; 85; 255 253 | Pose Style: None 254 | Radius: 0.029999999329447746 255 | Shaft Diameter: 0.10000000149011612 256 | Shaft Length: 0.10000000149011612 257 | Topic: 258 | Depth: 5 259 | Durability Policy: Volatile 260 | History Policy: Keep Last 261 | Reliability Policy: Reliable 262 | Value: /local_plan 263 | Value: true 264 | - Alpha: 1 265 | Class: rviz_default_plugins/Polygon 266 | Color: 25; 255; 0 267 | Enabled: true 268 | Name: Polygon 269 | Topic: 270 | Depth: 5 271 | Durability Policy: Volatile 272 | History Policy: Keep Last 273 | Reliability Policy: Reliable 274 | Value: /local_costmap/published_footprint 275 | Value: true 276 | Enabled: true 277 | Name: Controller 278 | - Class: rviz_default_plugins/MarkerArray 279 | Enabled: true 280 | Name: MarkerArray 281 | Namespaces: 282 | {} 283 | Topic: 284 | Depth: 5 285 | Durability Policy: Volatile 286 | History Policy: Keep Last 287 | Reliability Policy: Reliable 288 | Value: /waypoints 289 | Value: true 290 | - Alpha: 1 291 | Class: rviz_default_plugins/RobotModel 292 | Collision Enabled: false 293 | Description File: /home/focalfossa/dev_ws/src/basic_mobile_robot/models/basic_mobile_bot_v2.urdf 294 | Description Source: Topic 295 | Description Topic: 296 | Depth: 5 297 | Durability Policy: Volatile 298 | History Policy: Keep Last 299 | Reliability Policy: Reliable 300 | Value: /robot_description 301 | Enabled: true 302 | Links: 303 | All Links Enabled: true 304 | Expand Joint Details: false 305 | Expand Link Details: false 306 | Expand Tree: false 307 | Link Tree Style: Links in Alphabetic Order 308 | base_footprint: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | base_link: 313 | Alpha: 1 314 | Show Axes: false 315 | Show Trail: false 316 | Value: true 317 | drivewhl_l_link: 318 | Alpha: 1 319 | Show Axes: false 320 | Show Trail: false 321 | Value: true 322 | drivewhl_r_link: 323 | Alpha: 1 324 | Show Axes: false 325 | Show Trail: false 326 | Value: true 327 | front_caster: 328 | Alpha: 1 329 | Show Axes: false 330 | Show Trail: false 331 | Value: true 332 | gps_link: 333 | Alpha: 1 334 | Show Axes: false 335 | Show Trail: false 336 | imu_link: 337 | Alpha: 1 338 | Show Axes: false 339 | Show Trail: false 340 | lidar_link: 341 | Alpha: 1 342 | Show Axes: false 343 | Show Trail: false 344 | Value: true 345 | Name: RobotModel 346 | TF Prefix: "" 347 | Update Interval: 0 348 | Value: true 349 | Visual Enabled: true 350 | Enabled: true 351 | Global Options: 352 | Background Color: 48; 48; 48 353 | Fixed Frame: map 354 | Frame Rate: 30 355 | Name: root 356 | Tools: 357 | - Class: rviz_default_plugins/MoveCamera 358 | - Class: rviz_default_plugins/Select 359 | - Class: rviz_default_plugins/FocusCamera 360 | - Class: rviz_default_plugins/Measure 361 | Line color: 128; 128; 0 362 | - Class: rviz_default_plugins/SetInitialPose 363 | Topic: 364 | Depth: 5 365 | Durability Policy: Volatile 366 | History Policy: Keep Last 367 | Reliability Policy: Reliable 368 | Value: /initialpose 369 | - Class: rviz_default_plugins/PublishPoint 370 | Single click: true 371 | Topic: 372 | Depth: 5 373 | Durability Policy: Volatile 374 | History Policy: Keep Last 375 | Reliability Policy: Reliable 376 | Value: /clicked_point 377 | - Class: nav2_rviz_plugins/GoalTool 378 | Transformation: 379 | Current: 380 | Class: rviz_default_plugins/TF 381 | Value: true 382 | Views: 383 | Current: 384 | Angle: -4.714995861053467 385 | Class: rviz_default_plugins/TopDownOrtho 386 | Enable Stereo Rendering: 387 | Stereo Eye Separation: 0.05999999865889549 388 | Stereo Focal Distance: 1 389 | Swap Stereo Eyes: false 390 | Value: false 391 | Invert Z Axis: false 392 | Name: Current View 393 | Near Clip Distance: 0.009999999776482582 394 | Scale: 50.432029724121094 395 | Target Frame: 396 | Value: TopDownOrtho (rviz_default_plugins) 397 | X: 0.05480484664440155 398 | Y: 0.43397775292396545 399 | Saved: ~ 400 | Window Geometry: 401 | Displays: 402 | collapsed: false 403 | Height: 896 404 | Hide Left Dock: false 405 | Hide Right Dock: true 406 | Navigation 2: 407 | collapsed: false 408 | QMainWindow State: 000000ff00000000fd00000004000000000000025f00000326fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000021e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000026100000102000000b700fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001a30000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 409 | Selection: 410 | collapsed: false 411 | Tool Properties: 412 | collapsed: false 413 | Views: 414 | collapsed: true 415 | Width: 1032 416 | X: 888 417 | Y: 48 -------------------------------------------------------------------------------- /src/robot_simulation/launch/autonomous_navigation.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.launch_description_sources import PythonLaunchDescriptionSource 5 | from launch_ros.substitutions import FindPackageShare 6 | from launch.substitutions import LaunchConfiguration 7 | 8 | def generate_launch_description(): 9 | # Find the nav2_bringup and robot_simulation packages 10 | nav2_bringup_share_dir = FindPackageShare(package='nav2_bringup').find('nav2_bringup') 11 | pkg_robot_simulation = FindPackageShare(package='robot_simulation').find('robot_simulation') 12 | 13 | # Localization and navigation launch file paths 14 | localization_launch_file = os.path.join(nav2_bringup_share_dir, 'launch', 'localization_launch.py') 15 | navigation_launch_file = os.path.join(nav2_bringup_share_dir, 'launch', 'navigation_launch.py') 16 | 17 | # Path to the custom nav2_params.yaml file 18 | nav2_params_file_path = os.path.join(pkg_robot_simulation, 'params', 'nav2_params.yaml') 19 | 20 | # Declare launch arguments 21 | declare_map_file_path_cmd = DeclareLaunchArgument( 22 | name='map_file_path', 23 | default_value='./src/robot_simulation/maps/house_map.yaml', 24 | description='Path to the map file') 25 | 26 | declare_use_sim_time_cmd = DeclareLaunchArgument( 27 | name='use_sim_time', 28 | default_value='true', 29 | description='Use simulation time if true') 30 | 31 | # Include the localization launch file with custom nav2 parameters 32 | start_localization_cmd = IncludeLaunchDescription( 33 | PythonLaunchDescriptionSource(localization_launch_file), 34 | launch_arguments={ 35 | 'map': LaunchConfiguration('map_file_path'), 36 | 'params_file': nav2_params_file_path 37 | }.items() 38 | ) 39 | 40 | # Include the navigation launch file with custom nav2 parameters 41 | start_navigation_cmd = IncludeLaunchDescription( 42 | PythonLaunchDescriptionSource(navigation_launch_file), 43 | launch_arguments={ 44 | 'map_subscribe_transient_local': 'true', 45 | 'params_file': nav2_params_file_path 46 | }.items() 47 | ) 48 | 49 | # Create the launch description and populate 50 | ld = LaunchDescription() 51 | 52 | # Declare the launch options 53 | ld.add_action(declare_map_file_path_cmd) 54 | ld.add_action(declare_use_sim_time_cmd) 55 | 56 | # Add actions 57 | ld.add_action(start_localization_cmd) 58 | ld.add_action(start_navigation_cmd) 59 | 60 | return ld 61 | -------------------------------------------------------------------------------- /src/robot_simulation/launch/house_sim.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition, UnlessCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | 10 | 11 | 12 | 13 | def generate_launch_description(): 14 | # Define package paths 15 | pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') 16 | pkg_robot_description = FindPackageShare(package='robot_description').find('robot_description') 17 | pkg_robot_simulation= FindPackageShare(package='robot_simulation').find('robot_simulation') 18 | 19 | # Define file and folder paths 20 | default_model_path = os.path.join(pkg_robot_description, 'urdf/mobile_robot.urdf.xacro') 21 | robot_localization_file_path = os.path.join(pkg_robot_simulation, 'config/ekf.yaml') 22 | default_rviz_config_path = os.path.join(pkg_robot_simulation, 'config/nav2_config.rviz') 23 | world_file_name = 'house.world' 24 | world_path = os.path.join(pkg_robot_simulation, 'worlds', world_file_name) 25 | 26 | # Declare launch configuration variables 27 | headless = LaunchConfiguration('headless') 28 | model = LaunchConfiguration('model') 29 | namespace = LaunchConfiguration('namespace') 30 | rviz_config_file = LaunchConfiguration('rviz_config_file') 31 | use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') 32 | use_rviz = LaunchConfiguration('use_rviz') 33 | use_sim_time = LaunchConfiguration('use_sim_time') 34 | use_simulator = LaunchConfiguration('use_simulator') 35 | world = LaunchConfiguration('world') 36 | 37 | # Define TF remappings 38 | remappings = [('/tf', 'tf'), 39 | ('/tf_static', 'tf_static')] 40 | 41 | static_tf_pub_map_to_odom = Node( 42 | package='tf2_ros', 43 | executable='static_transform_publisher', 44 | name='static_tf_pub_map_to_odom', 45 | arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], # Replace with your transform values 46 | ) 47 | 48 | # Declare the launch arguments 49 | declare_namespace_cmd = DeclareLaunchArgument( 50 | name='namespace', 51 | default_value='', 52 | description='Top-level namespace') 53 | 54 | declare_model_path_cmd = DeclareLaunchArgument( 55 | name='model', 56 | default_value=default_model_path, 57 | description='Absolute path to robot urdf file') 58 | 59 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 60 | name='rviz_config_file', 61 | default_value=default_rviz_config_path, 62 | description='Full path to the RVIZ config file to use') 63 | 64 | declare_simulator_cmd = DeclareLaunchArgument( 65 | name='headless', 66 | default_value='False', 67 | description='Whether to execute gzclient') 68 | 69 | declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 70 | name='use_robot_state_pub', 71 | default_value='True', 72 | description='Whether to start the robot state publisher') 73 | 74 | declare_use_rviz_cmd = DeclareLaunchArgument( 75 | name='use_rviz', 76 | default_value='True', 77 | description='Whether to start RVIZ') 78 | 79 | declare_use_sim_time_cmd = DeclareLaunchArgument( 80 | name='use_sim_time', 81 | default_value='True', 82 | description='Use simulation (Gazebo) clock if true') 83 | 84 | declare_use_simulator_cmd = DeclareLaunchArgument( 85 | name='use_simulator', 86 | default_value='True', 87 | description='Whether to start the simulator') 88 | 89 | declare_world_cmd = DeclareLaunchArgument( 90 | name='world', 91 | default_value=world_path, 92 | description='Full path to the world model file to load') 93 | 94 | # Start Gazebo server 95 | start_gazebo_server_cmd = IncludeLaunchDescription( 96 | PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), 97 | condition=IfCondition(use_simulator), 98 | launch_arguments={'world': world}.items()) 99 | 100 | # Start Gazebo client 101 | start_gazebo_client_cmd = IncludeLaunchDescription( 102 | PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), 103 | condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless]))) 104 | 105 | spawn_robot_cmd = Node( 106 | package='gazebo_ros', 107 | executable='spawn_entity.py', 108 | arguments=['-topic', 'robot_description', '-entity', 'my_robot'] 109 | ) 110 | 111 | # Start robot localization using an Extended Kalman filter 112 | start_robot_localization_cmd = Node( 113 | package='robot_localization', 114 | executable='ekf_node', 115 | name='ekf_filter_node', 116 | output='screen', 117 | parameters=[robot_localization_file_path, 118 | {'use_sim_time': use_sim_time}]) 119 | 120 | 121 | # Subscribe to the joint states of the robot, and publish the 3D pose of each link. 122 | start_robot_state_publisher_cmd = Node( 123 | condition=IfCondition(use_robot_state_pub), 124 | package='robot_state_publisher', 125 | executable='robot_state_publisher', 126 | namespace=namespace, 127 | parameters=[{'use_sim_time': use_sim_time, 128 | 'robot_description': Command(['xacro ', model])}], 129 | remappings=remappings, 130 | arguments=[default_model_path]) 131 | 132 | # Launch RViz 133 | start_rviz_cmd = Node( 134 | condition=IfCondition(use_rviz), 135 | package='rviz2', 136 | executable='rviz2', 137 | name='rviz2', 138 | output='screen', 139 | arguments=['-d', rviz_config_file]) 140 | 141 | 142 | # Create the launch description and populate 143 | ld = LaunchDescription() 144 | 145 | # Declare the launch options 146 | ld.add_action(declare_namespace_cmd) 147 | ld.add_action(declare_model_path_cmd) 148 | ld.add_action(declare_rviz_config_file_cmd) 149 | ld.add_action(declare_simulator_cmd) 150 | ld.add_action(declare_use_robot_state_pub_cmd) 151 | ld.add_action(declare_use_rviz_cmd) 152 | ld.add_action(declare_use_sim_time_cmd) 153 | ld.add_action(declare_use_simulator_cmd) 154 | ld.add_action(declare_world_cmd) 155 | 156 | # Add any actions 157 | ld.add_action(start_gazebo_server_cmd) 158 | ld.add_action(start_gazebo_client_cmd) 159 | ld.add_action(spawn_robot_cmd) 160 | ld.add_action(start_robot_localization_cmd) 161 | ld.add_action(start_robot_state_publisher_cmd) 162 | ld.add_action(start_rviz_cmd) 163 | ld.add_action(static_tf_pub_map_to_odom) 164 | 165 | return ld 166 | -------------------------------------------------------------------------------- /src/robot_simulation/launch/house_slam.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 4 | from launch.conditions import IfCondition, UnlessCondition 5 | from launch.launch_description_sources import PythonLaunchDescriptionSource 6 | from launch.substitutions import Command, LaunchConfiguration, PythonExpression 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | 10 | 11 | 12 | 13 | def generate_launch_description(): 14 | 15 | # Define package paths 16 | pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') 17 | pkg_robot_description = FindPackageShare(package='robot_description').find('robot_description') 18 | pkg_robot_simulation= FindPackageShare(package='robot_simulation').find('robot_simulation') 19 | 20 | # Define file and folder paths 21 | default_model_path = os.path.join(pkg_robot_description, 'urdf/mobile_robot.urdf.xacro') 22 | robot_localization_file_path = os.path.join(pkg_robot_simulation, 'config/ekf.yaml') 23 | default_rviz_config_path = os.path.join(pkg_robot_simulation, 'config/nav2_config.rviz') 24 | world_file_name = 'house.world' 25 | world_path = os.path.join(pkg_robot_simulation, 'worlds', world_file_name) 26 | 27 | 28 | 29 | # Launch configuration variables specific to simulation 30 | headless = LaunchConfiguration('headless') 31 | model = LaunchConfiguration('model') 32 | namespace = LaunchConfiguration('namespace') 33 | rviz_config_file = LaunchConfiguration('rviz_config_file') #gazebo_config.rviz 34 | use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') 35 | use_rviz = LaunchConfiguration('use_rviz') 36 | use_sim_time = LaunchConfiguration('use_sim_time') 37 | use_simulator = LaunchConfiguration('use_simulator') 38 | world = LaunchConfiguration('world') 39 | 40 | remappings = [('/tf', 'tf'), 41 | ('/tf_static', 'tf_static')] 42 | 43 | static_tf_pub_map_to_odom = Node( 44 | package='tf2_ros', 45 | executable='static_transform_publisher', 46 | name='static_tf_pub_map_to_odom', 47 | arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], # Replace with your transform values 48 | ) 49 | 50 | 51 | # Declare the launch arguments 52 | declare_namespace_cmd = DeclareLaunchArgument( 53 | name='namespace', 54 | default_value='', 55 | description='Top-level namespace') 56 | 57 | declare_use_namespace_cmd = DeclareLaunchArgument( 58 | name='use_namespace', 59 | default_value='False', 60 | description='Whether to apply a namespace to the navigation stack') 61 | 62 | 63 | declare_model_path_cmd = DeclareLaunchArgument( 64 | name='model', 65 | default_value=default_model_path, 66 | description='Absolute path to robot urdf file') 67 | 68 | declare_rviz_config_file_cmd = DeclareLaunchArgument( 69 | name='rviz_config_file', 70 | default_value=default_rviz_config_path, 71 | description='Full path to the RVIZ config file to use') 72 | 73 | declare_simulator_cmd = DeclareLaunchArgument( 74 | name='headless', 75 | default_value='False', 76 | description='Whether to execute gzclient') 77 | 78 | declare_slam_cmd = DeclareLaunchArgument( 79 | name='slam', 80 | default_value='True', 81 | description='Whether to run SLAM') 82 | 83 | declare_use_robot_state_pub_cmd = DeclareLaunchArgument( 84 | name='use_robot_state_pub', 85 | default_value='True', 86 | description='Whether to start the robot state publisher') 87 | 88 | declare_use_rviz_cmd = DeclareLaunchArgument( 89 | name='use_rviz', 90 | default_value='True', 91 | description='Whether to start RVIZ') 92 | 93 | declare_use_sim_time_cmd = DeclareLaunchArgument( 94 | name='use_sim_time', 95 | default_value='True', 96 | description='Use simulation (Gazebo) clock if true') 97 | 98 | declare_use_simulator_cmd = DeclareLaunchArgument( 99 | name='use_simulator', 100 | default_value='True', 101 | description='Whether to start the simulator') 102 | 103 | declare_world_cmd = DeclareLaunchArgument( 104 | name='world', 105 | default_value=world_path, 106 | description='Full path to the world model file to load') 107 | 108 | # Start Gazebo server 109 | start_gazebo_server_cmd = IncludeLaunchDescription( 110 | PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), 111 | condition=IfCondition(use_simulator), 112 | launch_arguments={'world': world}.items()) 113 | 114 | # Start Gazebo client 115 | start_gazebo_client_cmd = IncludeLaunchDescription( 116 | PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')), 117 | condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless]))) 118 | 119 | spawn_robot_cmd = Node( 120 | package='gazebo_ros', 121 | executable='spawn_entity.py', 122 | arguments=['-topic', 'robot_description', '-entity', 'my_robot'] 123 | ) 124 | 125 | # Start robot localization using an Extended Kalman filter 126 | start_robot_localization_cmd = Node( 127 | package='robot_localization', 128 | executable='ekf_node', 129 | name='ekf_filter_node', 130 | output='screen', 131 | parameters=[robot_localization_file_path, 132 | {'use_sim_time': use_sim_time}]) 133 | 134 | # Start SLAM 135 | start_slam_toolbox_cmd = Node( 136 | package='slam_toolbox', 137 | executable='sync_slam_toolbox_node', 138 | name='slam_toolbox', 139 | output='screen', 140 | parameters=[{'use_sim_time': use_sim_time}] 141 | ) 142 | 143 | 144 | # Subscribe to the joint states of the robot, and publish the 3D pose of each link. 145 | start_robot_state_publisher_cmd = Node( 146 | condition=IfCondition(use_robot_state_pub), 147 | package='robot_state_publisher', 148 | executable='robot_state_publisher', 149 | namespace=namespace, 150 | parameters=[{'use_sim_time': use_sim_time, 151 | 'robot_description': Command(['xacro ', model])}], 152 | remappings=remappings, 153 | arguments=[default_model_path]) 154 | 155 | # Launch RViz 156 | start_rviz_cmd = Node( 157 | condition=IfCondition(use_rviz), 158 | package='rviz2', 159 | executable='rviz2', 160 | name='rviz2', 161 | output='screen', 162 | arguments=['-d', rviz_config_file]) 163 | 164 | 165 | # Create the launch description and populate 166 | ld = LaunchDescription() 167 | 168 | # Declare the launch options 169 | ld.add_action(declare_namespace_cmd) 170 | ld.add_action(declare_use_namespace_cmd) 171 | ld.add_action(declare_model_path_cmd) 172 | ld.add_action(declare_rviz_config_file_cmd) 173 | ld.add_action(declare_simulator_cmd) 174 | ld.add_action(declare_slam_cmd) 175 | ld.add_action(declare_use_robot_state_pub_cmd) 176 | ld.add_action(declare_use_rviz_cmd) 177 | ld.add_action(declare_use_sim_time_cmd) 178 | ld.add_action(declare_use_simulator_cmd) 179 | ld.add_action(declare_world_cmd) 180 | 181 | # Add any actions 182 | ld.add_action(start_gazebo_server_cmd) 183 | ld.add_action(start_gazebo_client_cmd) 184 | ld.add_action(spawn_robot_cmd) 185 | ld.add_action(start_robot_localization_cmd) 186 | ld.add_action(start_robot_state_publisher_cmd) 187 | ld.add_action(start_slam_toolbox_cmd) 188 | ld.add_action(start_rviz_cmd) 189 | ld.add_action(static_tf_pub_map_to_odom) 190 | 191 | return ld -------------------------------------------------------------------------------- /src/robot_simulation/maps/house_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aimechengineer/HomeBot-ROS2-Navigation/885393b96aaace2f8553dab7bbe6fc78768b019f/src/robot_simulation/maps/house_map.pgm -------------------------------------------------------------------------------- /src/robot_simulation/maps/house_map.yaml: -------------------------------------------------------------------------------- 1 | image: house_map.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-4.78, -3.3, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /src/robot_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_simulation 5 | 0.0.0 6 | 7 | The 'robot_simulation' package facilitates advanced simulation capabilities for a custom robot in a household environment. 8 | It includes 'house_slam.launch.py' for conducting SLAM (Simultaneous Localization and Mapping), enabling the robot to generate 9 | and refine maps in a realistic house world. The 'house_sim.launch.py' provides a basic simulation setup with the robot in the 10 | household environment. Furthermore, the package supports autonomous navigation; following map generation, the 11 | 'autonomous_navigation.launch.py' leverages Nav2's capabilities for robust autonomous movement and localization within the 12 | simulated environment. Integration with RViz is also featured, offering real-time visualization of the robot's navigation and 13 | the environment map, enhancing both the development and demonstration aspects of robotic simulations. 14 | 15 | Alexandros Rafael Aposporis - aimechengineer 16 | MIT 17 | 18 | ament_cmake 19 | robot_description 20 | nav2_bringup 21 | slam_toolbox 22 | 23 | 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/robot_simulation/params/nav2_params.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: True 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | scan_topic: scan 41 | 42 | bt_navigator: 43 | ros__parameters: 44 | use_sim_time: True 45 | global_frame: map 46 | robot_base_frame: base_link 47 | odom_topic: /odom 48 | bt_loop_duration: 10 49 | default_server_timeout: 20 50 | # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: 51 | # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml 52 | # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml 53 | # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. 54 | plugin_lib_names: 55 | - nav2_compute_path_to_pose_action_bt_node 56 | - nav2_compute_path_through_poses_action_bt_node 57 | - nav2_smooth_path_action_bt_node 58 | - nav2_follow_path_action_bt_node 59 | - nav2_spin_action_bt_node 60 | - nav2_wait_action_bt_node 61 | - nav2_assisted_teleop_action_bt_node 62 | - nav2_back_up_action_bt_node 63 | - nav2_drive_on_heading_bt_node 64 | - nav2_clear_costmap_service_bt_node 65 | - nav2_is_stuck_condition_bt_node 66 | - nav2_goal_reached_condition_bt_node 67 | - nav2_goal_updated_condition_bt_node 68 | - nav2_globally_updated_goal_condition_bt_node 69 | - nav2_is_path_valid_condition_bt_node 70 | - nav2_initial_pose_received_condition_bt_node 71 | - nav2_reinitialize_global_localization_service_bt_node 72 | - nav2_rate_controller_bt_node 73 | - nav2_distance_controller_bt_node 74 | - nav2_speed_controller_bt_node 75 | - nav2_truncate_path_action_bt_node 76 | - nav2_truncate_path_local_action_bt_node 77 | - nav2_goal_updater_node_bt_node 78 | - nav2_recovery_node_bt_node 79 | - nav2_pipeline_sequence_bt_node 80 | - nav2_round_robin_node_bt_node 81 | - nav2_transform_available_condition_bt_node 82 | - nav2_time_expired_condition_bt_node 83 | - nav2_path_expiring_timer_condition 84 | - nav2_distance_traveled_condition_bt_node 85 | - nav2_single_trigger_bt_node 86 | - nav2_goal_updated_controller_bt_node 87 | - nav2_is_battery_low_condition_bt_node 88 | - nav2_navigate_through_poses_action_bt_node 89 | - nav2_navigate_to_pose_action_bt_node 90 | - nav2_remove_passed_goals_action_bt_node 91 | - nav2_planner_selector_bt_node 92 | - nav2_controller_selector_bt_node 93 | - nav2_goal_checker_selector_bt_node 94 | - nav2_controller_cancel_bt_node 95 | - nav2_path_longer_on_approach_bt_node 96 | - nav2_wait_cancel_bt_node 97 | - nav2_spin_cancel_bt_node 98 | - nav2_back_up_cancel_bt_node 99 | - nav2_assisted_teleop_cancel_bt_node 100 | - nav2_drive_on_heading_cancel_bt_node 101 | - nav2_is_battery_charging_condition_bt_node 102 | 103 | bt_navigator_navigate_through_poses_rclcpp_node: 104 | ros__parameters: 105 | use_sim_time: True 106 | 107 | bt_navigator_navigate_to_pose_rclcpp_node: 108 | ros__parameters: 109 | use_sim_time: True 110 | 111 | controller_server: 112 | ros__parameters: 113 | use_sim_time: True 114 | controller_frequency: 20.0 115 | min_x_velocity_threshold: 0.001 116 | min_y_velocity_threshold: 0.5 117 | min_theta_velocity_threshold: 0.001 118 | failure_tolerance: 0.3 119 | progress_checker_plugin: "progress_checker" 120 | goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" 121 | controller_plugins: ["FollowPath"] 122 | 123 | # Progress checker parameters 124 | progress_checker: 125 | plugin: "nav2_controller::SimpleProgressChecker" 126 | required_movement_radius: 0.5 127 | movement_time_allowance: 10.0 128 | # Goal checker parameters 129 | #precise_goal_checker: 130 | # plugin: "nav2_controller::SimpleGoalChecker" 131 | # xy_goal_tolerance: 0.25 132 | # yaw_goal_tolerance: 0.25 133 | # stateful: True 134 | general_goal_checker: 135 | stateful: True 136 | plugin: "nav2_controller::SimpleGoalChecker" 137 | xy_goal_tolerance: 0.25 138 | yaw_goal_tolerance: 0.25 139 | # DWB parameters 140 | FollowPath: 141 | plugin: "dwb_core::DWBLocalPlanner" 142 | debug_trajectory_details: True 143 | min_vel_x: 0.0 144 | min_vel_y: 0.0 145 | max_vel_x: 0.26 146 | max_vel_y: 0.0 147 | max_vel_theta: 1.0 148 | min_speed_xy: 0.0 149 | max_speed_xy: 0.26 150 | min_speed_theta: 0.0 151 | # Add high threshold velocity for turtlebot 3 issue. 152 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 153 | acc_lim_x: 2.5 154 | acc_lim_y: 0.0 155 | acc_lim_theta: 3.2 156 | decel_lim_x: -2.5 157 | decel_lim_y: 0.0 158 | decel_lim_theta: -3.2 159 | vx_samples: 20 160 | vy_samples: 5 161 | vtheta_samples: 20 162 | sim_time: 1.7 163 | linear_granularity: 0.05 164 | angular_granularity: 0.025 165 | transform_tolerance: 0.2 166 | xy_goal_tolerance: 0.25 167 | trans_stopped_velocity: 0.25 168 | short_circuit_trajectory_evaluation: True 169 | stateful: True 170 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 171 | BaseObstacle.scale: 0.02 172 | PathAlign.scale: 32.0 173 | PathAlign.forward_point_distance: 0.1 174 | GoalAlign.scale: 24.0 175 | GoalAlign.forward_point_distance: 0.1 176 | PathDist.scale: 32.0 177 | GoalDist.scale: 24.0 178 | RotateToGoal.scale: 32.0 179 | RotateToGoal.slowing_factor: 5.0 180 | RotateToGoal.lookahead_time: -1.0 181 | 182 | local_costmap: 183 | local_costmap: 184 | ros__parameters: 185 | update_frequency: 5.0 186 | publish_frequency: 2.0 187 | global_frame: odom 188 | robot_base_frame: base_link 189 | use_sim_time: True 190 | rolling_window: true 191 | width: 3 192 | height: 3 193 | resolution: 0.05 194 | robot_radius: 0.18 195 | plugins: ["voxel_layer", "inflation_layer"] 196 | inflation_layer: 197 | plugin: "nav2_costmap_2d::InflationLayer" 198 | cost_scaling_factor: 3.0 199 | inflation_radius: 0.4 200 | voxel_layer: 201 | plugin: "nav2_costmap_2d::VoxelLayer" 202 | enabled: True 203 | publish_voxel_map: True 204 | origin_z: 0.0 205 | z_resolution: 0.05 206 | z_voxels: 16 207 | max_obstacle_height: 2.0 208 | mark_threshold: 0 209 | observation_sources: scan 210 | scan: 211 | topic: /scan 212 | max_obstacle_height: 2.0 213 | clearing: True 214 | marking: True 215 | data_type: "LaserScan" 216 | raytrace_max_range: 3.0 217 | raytrace_min_range: 0.0 218 | obstacle_max_range: 2.5 219 | obstacle_min_range: 0.0 220 | static_layer: 221 | plugin: "nav2_costmap_2d::StaticLayer" 222 | map_subscribe_transient_local: True 223 | always_send_full_costmap: True 224 | 225 | global_costmap: 226 | global_costmap: 227 | ros__parameters: 228 | update_frequency: 1.0 229 | publish_frequency: 1.0 230 | global_frame: map 231 | robot_base_frame: base_link 232 | use_sim_time: True 233 | robot_radius: 0.18 234 | resolution: 0.05 235 | track_unknown_space: true 236 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 237 | obstacle_layer: 238 | plugin: "nav2_costmap_2d::ObstacleLayer" 239 | enabled: True 240 | observation_sources: scan 241 | scan: 242 | topic: /scan 243 | max_obstacle_height: 2.0 244 | clearing: True 245 | marking: True 246 | data_type: "LaserScan" 247 | raytrace_max_range: 3.0 248 | raytrace_min_range: 0.0 249 | obstacle_max_range: 2.5 250 | obstacle_min_range: 0.0 251 | static_layer: 252 | plugin: "nav2_costmap_2d::StaticLayer" 253 | map_subscribe_transient_local: True 254 | inflation_layer: 255 | plugin: "nav2_costmap_2d::InflationLayer" 256 | cost_scaling_factor: 3.0 257 | inflation_radius: 0.4 258 | always_send_full_costmap: True 259 | 260 | map_server: 261 | ros__parameters: 262 | use_sim_time: True 263 | # Overridden in launch by the "map" launch configuration or provided default value. 264 | # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. 265 | yaml_filename: "" 266 | 267 | map_saver: 268 | ros__parameters: 269 | use_sim_time: True 270 | save_map_timeout: 5.0 271 | free_thresh_default: 0.25 272 | occupied_thresh_default: 0.65 273 | map_subscribe_transient_local: True 274 | 275 | planner_server: 276 | ros__parameters: 277 | expected_planner_frequency: 20.0 278 | use_sim_time: True 279 | planner_plugins: ["GridBased"] 280 | GridBased: 281 | plugin: "nav2_navfn_planner/NavfnPlanner" 282 | tolerance: 0.5 283 | use_astar: false 284 | allow_unknown: true 285 | 286 | smoother_server: 287 | ros__parameters: 288 | use_sim_time: True 289 | smoother_plugins: ["simple_smoother"] 290 | simple_smoother: 291 | plugin: "nav2_smoother::SimpleSmoother" 292 | tolerance: 1.0e-10 293 | max_its: 1000 294 | do_refinement: True 295 | 296 | behavior_server: 297 | ros__parameters: 298 | costmap_topic: local_costmap/costmap_raw 299 | footprint_topic: local_costmap/published_footprint 300 | cycle_frequency: 10.0 301 | behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] 302 | spin: 303 | plugin: "nav2_behaviors/Spin" 304 | backup: 305 | plugin: "nav2_behaviors/BackUp" 306 | drive_on_heading: 307 | plugin: "nav2_behaviors/DriveOnHeading" 308 | wait: 309 | plugin: "nav2_behaviors/Wait" 310 | assisted_teleop: 311 | plugin: "nav2_behaviors/AssistedTeleop" 312 | global_frame: odom 313 | robot_base_frame: base_link 314 | transform_tolerance: 0.1 315 | use_sim_time: true 316 | simulate_ahead_time: 2.0 317 | max_rotational_vel: 1.0 318 | min_rotational_vel: 0.4 319 | rotational_acc_lim: 3.2 320 | 321 | robot_state_publisher: 322 | ros__parameters: 323 | use_sim_time: True 324 | 325 | waypoint_follower: 326 | ros__parameters: 327 | use_sim_time: True 328 | loop_rate: 20 329 | stop_on_failure: false 330 | waypoint_task_executor_plugin: "wait_at_waypoint" 331 | wait_at_waypoint: 332 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 333 | enabled: True 334 | waypoint_pause_duration: 200 335 | 336 | velocity_smoother: 337 | ros__parameters: 338 | use_sim_time: True 339 | smoothing_frequency: 20.0 340 | scale_velocities: False 341 | feedback: "OPEN_LOOP" 342 | max_velocity: [0.26, 0.0, 1.0] 343 | min_velocity: [-0.26, 0.0, -1.0] 344 | max_accel: [2.5, 0.0, 3.2] 345 | max_decel: [-2.5, 0.0, -3.2] 346 | odom_topic: "odom" 347 | odom_duration: 0.1 348 | deadband_velocity: [0.0, 0.0, 0.0] 349 | velocity_timeout: 1.0 --------------------------------------------------------------------------------