From 70853f30ffb16ce0000d8badfd9f221c710a0fc8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= <bjoern.ellensohn@gmail.com>
Date: Tue, 22 Aug 2023 12:01:43 +0200
Subject: [PATCH] added slam_toolbox localization option

---
 config/mapper_params_localization.yaml      | 67 +++++++++++++++++++++
 launch/robot_mapping_localization.launch.py | 51 ++++++++++++++++
 2 files changed, 118 insertions(+)
 create mode 100644 config/mapper_params_localization.yaml
 create mode 100644 launch/robot_mapping_localization.launch.py

diff --git a/config/mapper_params_localization.yaml b/config/mapper_params_localization.yaml
new file mode 100644
index 0000000..bf4da51
--- /dev/null
+++ b/config/mapper_params_localization.yaml
@@ -0,0 +1,67 @@
+slam_toolbox:
+  ros__parameters:
+    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_footprint
+    scan_topic: /scan
+    mode: mapping #localization
+
+    # if you'd like to start localizing on bringup in a map and pose
+    #map_file_name: test_steve
+    #map_start_pose: [5.0, 1.0, 0.0]
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 8.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 3
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 3
+    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
+    loop_search_maximum_distance: 3.0
+
+    # 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
\ No newline at end of file
diff --git a/launch/robot_mapping_localization.launch.py b/launch/robot_mapping_localization.launch.py
new file mode 100644
index 0000000..cbbc878
--- /dev/null
+++ b/launch/robot_mapping_localization.launch.py
@@ -0,0 +1,51 @@
+# Copyright 2022 Factor Robotics
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import LifecycleNode
+from launch_ros.descriptions import ParameterValue
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    use_sim_time = False
+    slam_params_file = PathJoinSubstitution(
+                [
+                    FindPackageShare("cps_rmp220_support"),
+                    "config",
+                    "mapper_params_localization.yaml"
+                ]
+            )
+    namespace = "/rmp"
+    localization_node = Node(
+        package="slam_toolbox",
+        executable="localization_slam_toolbox_node",
+        name='slam_toolbox',
+        output='screen',
+        parameters=[
+            slam_params_file,
+            {'use_sim_time': use_sim_time}
+        ],
+        #namespace = namespace,
+        #remappings=[('/scan', 'scan'), ('/map', 'map')],
+    )
+
+    return LaunchDescription([
+        localization_node
+    ])
\ No newline at end of file