some humble improvements

openslam_gmapping/CMakeLists.txt

@@ -11,10 +11,17 @@ if(NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 14)
 endif()
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-    add_compile_options(-O3 -funroll-loops)
+# DEFAULT RELEASE
+if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
+  if (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
+  endif()
 endif()
 
+# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#     add_compile_options(-O3 -funroll-loops)
+# endif()
+
 add_compile_options(-fPIC)
 find_package(ament_cmake REQUIRED)
 

openslam_gmapping/gridfastslam/gridslamprocessor.cpp

@@ -17,7 +17,9 @@ const double m_distanceThresholdCheck = 20;
 
 using namespace std;
 
-  GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){
+
+  GridSlamProcessor::GridSlamProcessor()
+    :m_infoStream(cout){
 
     period_ = 5.0;
     m_obsSigmaGain=1;

slam_gmapping/CMakeLists.txt

@@ -11,11 +11,17 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 
+# DEFAULT RELEASE
+if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
+  if (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
+  endif()
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
-set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld -Wl,--disable-new-dtags")
-message(${CMAKE_SHARED_LINKER_FLAGS})
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -25,6 +31,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(openslam_gmapping REQUIRED)
@@ -40,24 +47,38 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-include_directories(include)
+# include_directories(include)
+
+add_library(slam_gmapping SHARED
+    src/slam_gmapping.cpp)
 
-add_executable(slam_gmapping src/slam_gmapping.cpp)
+target_include_directories(slam_gmapping
+PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
 
 ament_target_dependencies(slam_gmapping
-        rclcpp
-        tf2
-        tf2_ros
-        message_filters
-        sensor_msgs
-        nav_msgs
-        tf2_geometry_msgs
-        openslam_gmapping)
-
-install(TARGETS slam_gmapping
-        DESTINATION lib/${PROJECT_NAME})
-
-install(DIRECTORY launch
+    rclcpp
+    rclcpp_components
+    tf2
+    tf2_ros
+    message_filters
+    sensor_msgs
+    nav_msgs
+    tf2_geometry_msgs
+    openslam_gmapping)
+
+rclcpp_components_register_node(slam_gmapping 
+    PLUGIN "SlamGmapping" 
+    EXECUTABLE slam_gmapping_node)
+
+install(TARGETS slam_gmapping 
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY launch config
 	    DESTINATION share/${PROJECT_NAME})
 
 ament_package()

slam_gmapping/config/slam_gmapping.yaml

@@ -0,0 +1,49 @@
+slam_gmapping:
+  ros__parameters:
+    base_frame: "base_link"
+    odom_frame: "odom"      
+    map_frame:  "map"     
+    map_update_interval:  5.0 
+
+    # Default: autodetect. Set maxUrange and maxRange for manual settings
+    # maxUrange:          10.0
+    # maxRange:           10.0
+    minimumScore:         0
+
+    sigma:                0.05
+    kernelSize:           1 
+    lstep:                0.05
+    astep:                0.05
+    iterations:           5 
+    lsigma:               0.075
+    ogain:                3.0
+    lskip:                0
+
+    srr:                  0.1
+    srt:                  0.2
+    str:                  0.1
+    stt:                  0.2
+
+    linearUpdate:         1.0      
+    angularUpdate:        0.5 
+    temporalUpdate:       -1.0    
+
+    resampleThreshold:    0.5  
+    particles:            30
+
+    # Initial Map Size & Resolution
+    xmin:                -100.0
+    ymin:                -100.0
+    xmax:                 100.0
+    ymax:                 100.0
+    delta:                0.05
+
+    llsamplerange:        0.01           
+    llsamplestep:         0.01
+    lasamplerange:        0.005  
+    lasamplestep:         0.005
+
+    transform_publish_period:  0.05       
+    occ_thresh:           0.25
+
+    tf_delay:             0.1  

slam_gmapping/include/slam_gmapping/slam_gmapping.h

@@ -35,7 +35,7 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2/utils.h"
@@ -47,9 +47,9 @@
 #include "gmapping/sensor/sensor_range/rangesensor.h"
 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
 
-class SlamGmapping : public rclcpp::Node{
+class SlamGmapping : public rclcpp::Node {
 public:
-    SlamGmapping();
+    explicit SlamGmapping(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
     ~SlamGmapping() override;
 
     void init();
@@ -59,7 +59,6 @@ class SlamGmapping : public rclcpp::Node{
     void publishLoop(double transform_publish_period);
 
 private:
-    rclcpp::Node::SharedPtr node_;
     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr entropy_publisher_;
     rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr sst_;
     rclcpp::Publisher<nav_msgs::msg::MapMetaData>::SharedPtr sstm_;
@@ -111,6 +110,7 @@ class SlamGmapping : public rclcpp::Node{
     double computePoseEntropy();
 
     // Parameters used by GMapping
+    bool   autoRange_;
     double maxRange_;
     double maxUrange_;
     double maxrange_;

slam_gmapping/launch/slam_gmapping.launch.py

@@ -1,15 +1,28 @@
 from launch import LaunchDescription
+from ament_index_python.packages import get_package_share_directory
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+import os
+from launch.actions import DeclareLaunchArgument
 
 
 def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    use_sim_time_arg = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false'
+        )
+
     return LaunchDescription([
+        use_sim_time_arg,
         Node(
             package='slam_gmapping', 
-            executable='slam_gmapping',
+            executable='slam_gmapping_node',
+            name='slam_gmapping',
             output='screen',
-            parameters=[{'use_sim_time': use_sim_time}]
+            parameters=[
+                {'use_sim_time': use_sim_time},
+                os.path.join(get_package_share_directory("slam_gmapping"), 'config', 'slam_gmapping.yaml')
+            ]
         ),
     ])

slam_gmapping/package.xml

@@ -23,6 +23,7 @@
     <depend>tf2_geometry_msgs</depend>
     <depend>message_filters</depend>
     <depend>rclcpp</depend>
+    <depend>rclcpp_components</depend>
     <depend>sensor_msgs</depend>
     <depend>visualization_msgs</depend>
     <depend>openslam_gmapping</depend>