added mapping and navigation

CMakeLists.txt

@@ -7,5 +7,11 @@ install(DIRECTORY launch meshes models urdf worlds rviz
     DESTINATION share/${PROJECT_NAME}/
 )
 
+install( DIRECTORY config/
+         DESTINATION share/${PROJECT_NAME}/config)
+
+
+install(PROGRAMS scripts/remapper.py
+        DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

Dockerfile

@@ -32,6 +32,15 @@ RUN apt-get install --no-install-recommends -yqqq \
     ros-$ROS_DISTRO-ros-gz-sim \
     ros-$ROS_DISTRO-ros-gz-interfaces \
     ros-$ROS_DISTRO-ros-gz-bridge
+
+
+# Install additional ROS packages
+RUN apt-get install --no-install-recommends -yqq \
+    ros-$ROS_DISTRO-rviz2 \
+    ros-$ROS_DISTRO-xacro \
+    ros-$ROS_DISTRO-teleop-twist-keyboard \
+    ros-$ROS_DISTRO-navigation2 \
+    ros-$ROS_DISTRO-nav2-bringup
 
 # Using shell to use bash commands like 'source'
 SHELL ["/bin/bash", "-c"]
@@ -42,4 +51,4 @@ ARG WORKSPACE=/root/ros2_ws
 # Add target workspace in environment
 ENV WORKSPACE=$WORKSPACE
 
-WORKDIR $WORKSPACE
+WORKDIR $WORKSPACE

README.md

@@ -238,6 +238,35 @@ ros2 launch stereo_image_proc stereo_image_proc.launch.py left_namespace:=bcr_bo
 
 **Warning:**  `gz-harmonic` cannot be installed alongside gazebo-classic (eg. gazebo11) since both use the `gz` command line tool.
 
+### Mapping with SLAM Toolbox
+
+SLAM Toolbox is an open-source package designed to map the environment using laser scans and odometry, generating a map for autonomous navigation.
+
+To start mapping:
+```bash
+ros2 launch bcr_bot mapping.launch.py
+```
+
+Use the teleop twist keyboard to control the robot and map the area:
+```bash
+ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:=/bcr_bot/cmd_vel
+```
+
+To save the map:
+```bash
+cd src/bcr_bot/config
+ros2 run nav2_map_server map_saver_cli -f bcr_map
+```
+
+### Using Nav2 with bcr_bot
+
+Nav2 is an open-source navigation package that enables a robot to navigate through an environment easily. It takes laser scan and odometry data, along with the map of the environment, as inputs.
+
+To run Nav2 on bcr_bot:
+```bash
+ros2 launch bcr_bot nav2.launch.py
+```
+
 
 ### Simulation and Visualization
 1. Gz Sim (Ignition Gazebo) (small_warehouse World):

config/bcr_map.yaml

@@ -0,0 +1,7 @@
+image: bcr_map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-14.3, -15.5, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

config/mapper_params_online_async.yaml

@@ -0,0 +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: /bcr_bot/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.0 #for rastering images
+    max_laser_range: 20.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    
+
+    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