Megabot — SolidWorks → ROS 2 Differential-Drive Robot with RTAB-Map SLAM & Nav2

A complete simulation-to-navigation pipeline for a differential-drive robot: designed in SolidWorks, exported as a URDF, simulated in Gazebo Ignition (Fortress), localized with an EKF (wheel odometry + IMU), mapped with RTAB-Map RGB-D SLAM, and driven autonomously with the Nav2 stack wrapped in a Behavior Tree.

Full runtime ROS graph — sensor bridge → EKF → RTAB-Map → TF → Nav2

---

Overview

This repository takes a robot from CAD to autonomous navigation:

1. CAD → URDF — the robot (mws.urdf) was modeled in SolidWorks and exported for ROS 2, including accurate link meshes (meshes/*.STL) for the base, wheels, casters, LiDAR mount, and IMU.
2. Simulation — spawned in Gazebo Ignition (Fortress) inside a depot.sdf warehouse world, with an RGB-D camera and IMU simulated via Gazebo sensor plugins and bridged to ROS 2.
3. State estimation — wheel odometry and IMU are fused through a robot_localization EKF to produce a smoothed /odom → base_footprint transform; an optional Madgwick filter is included for IMU orientation.
4. Mapping & localization — RTAB-Map builds a 2D occupancy grid from the RGB-D stream (planar SLAM, g2o backend, ORB feature loop closure) and can be re-run in localization-only mode against a saved map.
5. Navigation — the Nav2 stack (controller, planner, smoother, behavior server, BT navigator, waypoint follower) drives the robot, with a custom bt_ros2 behavior tree layer on top for higher-level mission logic (multi-goal patrol, interrupts, snapshot/inspection behaviors).

Left: live 2D occupancy grid in RViz. Right: RTAB-Map's internal view — RGB-D feature matching and loop-closure detection.

---

Repository layout

.
├── frames_2026-09-01_17.34.37.pdf   # RTAB-Map graph/frame export
├── rosgraph.png                     # ROS computation graph (see above)
├── rtabmap.png                      # RTAB-Map feature-matching view
├── rtabmap_rviz.png                 # Occupancy grid in RViz
└── src/
    ├── megabot/                     # Robot description, world, sim bring-up
    │   ├── config/                  #   ekf.yaml, madgwick.yaml
    │   ├── launch/                  #   gazebo_ignition.launch.py
    │   ├── meshes/                  #   SolidWorks-exported STL links
    │   ├── rviz/                    #   mws.rviz, mws1.rviz
    │   ├── urdf/                    #   mws.urdf
    │   └── worlds/                  #   depot.sdf
    ├── megabot_nav/                 # SLAM + Nav2 bring-up
    │   ├── launch/                  #   rtabmap.launch.py, navigation.launch.py, localization.launch.py
    │   ├── param/                   #   mws_params.yaml (Nav2 params)
    │   └── rviz/                    #   nav2 RViz configs
    └── bt_ros2/                     # Behavior Tree layer over Nav2 (ADLINK BT_ros2, vendored)
        ├── bt_xml/                  #   patrol, interrupt, snapshot, OpenVINO-triggered trees
        └── src/                     #   BT nodes (nav2 client, autodock, teleop, interrupt events)

---

Hardware / robot model

Drive	Differential drive (gz-sim-diff-drive-system)
Wheel separation	0.135 m
Wheel diameter	0.069 m
Max wheel torque	20 N·m
Sensors	RGB-D camera (rgbd_camera), IMU
Base frames	base_footprint → base_link → camera_link_optical, imu_link, lidar_link
Source geometry	SolidWorks assembly, exported to STL per link and wrapped in mws.urdf

---

Software architecture

Sensing → estimation → mapping → planning, roughly following this data flow:

Gazebo (depot.sdf)
   │  ros_gz_bridge
   ├── /camera/color/image_raw, /camera/depth/image_rect_raw, /camera/color/camera_info
   ├── /imu
   └── /odom/unfiltered  ──┐
                            ▼
                    robot_localization (EKF, 2D mode)
                            │
                          /odom  →  odom → base_footprint TF
                            ▼
                        RTAB-Map (RGB-D SLAM)
              ├── /map (2D occupancy grid, planar SLAM + g2o)
              ├── /rtabmap/mapData, /global_path
              └── point_cloud_xyz → obstacles_detection
                            ▼
                     depthimage_to_laserscan  →  /scan
                            ▼
                          Nav2 stack
        (controller_server, planner_server, smoother_server,
         behavior_server, bt_navigator, waypoint_follower)
                            ▼
                    bt_ros2 (mission-level Behavior Tree)
                       → sends Nav2 goals, handles
                         interrupts / snapshots

Key design choices baked into the launch/config files:

• RTAB-Map is configured for planar (2D) SLAM (Reg/Force3DoF, Optimizer/Slam2D) with a g2o backend and ORB features for both odometry correction and loop closure.
• The occupancy grid is generated directly from depth (Grid/FromDepth, Grid/Sensor: 1), not from a 3D point cloud, keeping it lightweight for Nav2's costmaps.
• A separate depthimage_to_laserscan node synthesizes a /scan topic from the depth image, so the Nav2 stack (built around 2D laser costmaps) works unchanged with an RGB-D-only sensor.
• rtabmap.launch.py supports both SLAM mode (localization:=false, default) and localization-only mode (localization:=true) against a previously built map — same launch file, one argument.
• The EKF (ekf.yaml) fuses only wheel-odometry linear velocity + yaw rate and IMU yaw rate, in 2D mode — deliberately excluding raw position/orientation from either source to avoid double-correcting drift.

---

Prerequisites

• ROS 2 Humble
• Gazebo Ignition Fortress (via ros_gz_sim, ros_gz_bridge)
• ROS 2 packages: robot_state_publisher, robot_localization, imu_filter_madgwick, depthimage_to_laserscan, rtabmap_ros (rtabmap_slam, rtabmap_util, rtabmap_viz), nav2_bringup and the full Nav2 stack, behaviortree_cpp_v3

Install ROS dependencies from the workspace root:

cd ~/megabot_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
source install/setup.bash

---

Usage

Run each command in its own sourced terminal, in order:

1. Simulation, robot spawn, and state estimation

ros2 launch megabot gazebo_ignition.launch.py

Starts Gazebo Ignition with the depot.sdf world, spawns the robot from robot_description, bridges camera/IMU/odom/clock/cmd_vel topics between Gazebo and ROS 2, publishes robot state and joint states, opens RViz, and runs the EKF that fuses wheel odometry with the IMU into /odom.

Useful args: use_sim_time:=true (default), publish_joints:=true|false

2. Mapping (or localization)

ros2 launch megabot_nav rtabmap.launch.py

Starts RTAB-Map RGB-D SLAM, rtabmap_viz for visual debugging, and the point-cloud → obstacle/ground detection nodes that feed Nav2's costmaps. Builds a new map by default.

Useful args:

• localization:=true — run against a previously saved map instead of building a new one
• use_sim_time:=true (default)

3. Autonomous navigation

ros2 launch megabot_nav navigation.launch.py

Brings up the full Nav2 stack (controller, planner, smoother, behavior server, BT navigator, waypoint follower) under a lifecycle manager, converts the depth image to a /scan laser topic for the costmaps, opens the Nav2 RViz view, and launches the bt_ros2 mission-level behavior tree on top so you can drive multi-goal patrol routes, respond to interrupts, or trigger snapshot/inspection behaviors (see src/bt_ros2/bt_xml/).

Useful args: params_file:=<path> (defaults to megabot_nav/param/mws_params.yaml), autostart:=true, use_composition:=false, namespace:=''

Send goals via RViz's "Nav2 Goal" tool, /goal_pose, or by editing one of the bt_xml trees to drive a fixed patrol route.

---

License

megabot / megabot_nav: Apache-2.0. bt_ros2: Apache-2.0, © ADLINK Technology / ChenYing Kuo, adapted for this project.