Add mapping support (#11)

* Added teleop support to ease mapping.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Use online async mapping with SLAM Toolbox.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Update the documentation to reflect the mapping process.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

Author: leander-dsouza

README.md

@@ -63,7 +63,7 @@ Tutorial
 * Open a new terminal and run the teleoperation node:
 
    ```bash
-   ros2 run key_teleop_ros key_teleop_ros
+   ros2 run key_teleop_ros key_drive
    ```
 
    Use the arrow keys to control the robot's movement.
@@ -74,6 +74,40 @@ Tutorial
 
    https://github.com/user-attachments/assets/cd9df2df-2087-4dc8-8ce1-d5b526d30321
 
+### 3. Mapping
+
+* Launch the robot in Gazebo:
+
+   ```bash
+   ros2 launch atreus gazebo.launch.py
+   ```
+
+* Open another terminal and run the mapping node:
+
+   ```bash
+   ros2 launch atreus slam_toolbox.launch.py
+   ```
+
+   This will start the mapping process using **SLAM Toolbox**.
+
+* Open a new terminal and run the teleoperation node:
+
+   ```bash
+   ros2 run key_teleop_ros key_drive
+   ```
+
+   This will allow you to map the environment by driving the robot around.
+
+   https://github.com/user-attachments/assets/b92657ef-2855-4d6b-8c25-e220f16a3816
+
+
+* In order to save the map, open a new terminal and run the following command:
+
+   ```bash
+   ros2 run nav2_map_server map_saver_cli -f my_map
+   ```
+
+   This will save the map to the current directory.
 
 
 ###### 💾 EOF

config/mapping/slam_toolbox/slam_toolbox_params.yaml

@@ -1,9 +1,75 @@
 slam_toolbox:
   ros__parameters:
 
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
     odom_frame: odom
     map_frame: map
     base_frame: base_link
-    scan_topic: /scan
+    scan_topic: scan
+    use_map_saver: true
+    mode: localization
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    #map_file_name: test_steve
+    # map_start_pose: [0.0, 0.0, 0.0]
+    #map_start_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    min_laser_range: 0.1 #for rastering images
+    max_laser_range: 16.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
 
-    max_laser_range: 16.
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true