Humble update to 2.6.7 (#666)

* fix mismatched id between rviz marker and node in pose graph (#635)

* Corrected spelling mistakes in README.md (#646)

* added support for ROS 2 Components (#652)

* allow slam_params_file passed as launch arg (prior was hard coded) (#655)

* allow slam_params_file passed as launch arg (prior was hard coded)

* removed modifier comment

* Merge Maps: Transformation of box points has been fixed. (#661)

Co-authored-by: b2ed55dcda6ed1fb7822c27124846addcb174b73 <ozanberk.kaya@arcelik.com>

* Ceres solver: check parameters exist before removing them [ros2] (#659)

* Ceres solver: check parameters exist before removing them

* minor update logger

* bump to 2.6.7 for release

---------

Co-authored-by: gene.su <47352191+gene-su@users.noreply.github.com>
Co-authored-by: Chad Faragher <wyckster@hotmail.com>
Co-authored-by: Antonio Brandi <34162460+AntoBrandi@users.noreply.github.com>
Co-authored-by: slowrunner <slowrunner@users.noreply.github.com>
Co-authored-by: Ozan Berk Kaya <oenzan94@gmail.com>
Co-authored-by: b2ed55dcda6ed1fb7822c27124846addcb174b73 <ozanberk.kaya@arcelik.com>
Co-authored-by: PhamNhatTan <baoden93@gmail.com>

CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(rviz_ogre_vendor REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(interactive_markers REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
+find_package(rclcpp_components REQUIRED)
 
 #karto_sdk lib
 set(BUILD_SHARED_LIBS ON)
@@ -58,6 +59,7 @@ set(dependencies
   rviz_rendering
   interactive_markers
   Qt5
+  rclcpp_components
 )
 
 set(libraries
@@ -187,6 +189,12 @@ target_link_libraries(merge_maps_kinematic kartoSlamToolbox toolbox_common)
 #  target_link_libraries(lifelong_metrics_test lifelong_slam_toolbox)
 #endif()
 
+rclcpp_components_register_nodes(async_slam_toolbox "slam_toolbox::AsynchronousSlamToolbox")
+rclcpp_components_register_nodes(sync_slam_toolbox "slam_toolbox::SynchronousSlamToolbox")
+rclcpp_components_register_nodes(localization_slam_toolbox "slam_toolbox::LocalizationSlamToolbox")
+rclcpp_components_register_nodes(lifelong_slam_toolbox "slam_toolbox::LifelongSlamToolbox")
+rclcpp_components_register_nodes(map_and_localization_slam_toolbox "slam_toolbox::MapAndLocalizationSlamToolbox")
+
 #### Install
 install(TARGETS async_slam_toolbox_node
                 sync_slam_toolbox_node

README.md

@@ -24,7 +24,7 @@ You can find this work [here](https://joss.theoj.org/papers/10.21105/joss.02783)
 
 # Introduction
 
-Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained whil at Samsung Research, and largely in his free time. 
+Slam Toolbox is a set of tools and capabilities for 2D SLAM built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101) while at [Simbe Robotics](https://www.simberobotics.com/), maintained while at Samsung Research, and largely in his free time. 
 
 This project contains the ability to do most everything any other available SLAM library, both free and paid, and more. This includes:
 - Ordinary point-and-shoot 2D SLAM mobile robotics folks expect (start, map, save pgm file) with some nice built in utilities like saving maps
@@ -41,7 +41,7 @@ This project contains the ability to do most everything any other available SLAM
 
 For running on live production robots, I recommend using the snap: slam-toolbox, it has optimizations in it that make it about 10x faster. You need the deb/source install for the other developer level tools that don't need to be on the robot (rviz plugins, etc).
 
-This package has been benchmarked mapping building at 5x+ realtime up to about 30,000 sqft and 3x realtime up to about 60,000 sqft. with the largest area (I'm aware of) used was a 200,000 sq.ft. building in synchronous mode (e.i. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. 
+This package has been benchmarked mapping building at 5x+ real-time up to about 30,000 sq. ft. and 3x real-time up to about 60,000 sq. ft. with the largest area (I'm aware of) used was a 200,000 sq. ft. building in synchronous mode (i.e. processing all scans, regardless of lag), and *much* larger spaces in asynchronous mode. 
 
 The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/) in Oakland, California. Thanks to [Silicon Valley Robotics](https://svrobo.org/) & Circuit Launch for being a testbed for some of this work.
 
@@ -76,8 +76,8 @@ LifeLong mapping is the concept of being able to map a space, completely or part
 
 Our lifelong mapping consists of a few key steps
 - Serialization and Deserialization to store and reload map information
-- KD-Tree search matching to locate the robot in its position on reinitalization
-- pose-graph optimizition based SLAM with 2D scan matching abstraction 
+- KD-Tree search matching to locate the robot in its position on reinitialization
+- pose-graph optimization based SLAM with 2D scan matching abstraction 
 
 This will allow the user to create and update existing maps, then serialize the data for use in other mapping sessions, something sorely lacking from most SLAM implementations and nearly all planar SLAM implementations. Other good libraries that do this include RTab-Map and Cartoprapher, though they themselves have their own quirks that make them (in my opinion) unusable for production robotics applications. This library provides the mechanics to save not only the data, but the pose graph, and associated metadata to work with. This has been used to create maps by merging techniques (taking 2 or more serialized objects and creating 1 globally consistent one) as well as continuous mapping techniques (updating 1, same, serialized map object over time and refining it). The major benefit of this over RTab-Map or Cartoprapher is the maturity of the underlying (but heavily modified) `open_karto` library the project is based on. The scan matcher of Karto is well known as an extremely good matcher for 2D laser scans and modified versions of Karto can be found in companies across the world. 
 
@@ -86,21 +86,21 @@ Slam Toolbox supports all the major modes:
 - Starting at any particular node - select a node ID to start near
 - Starting in any particular area - indicate current pose in the map frame to start at, like AMCL 
 
-In the RVIZ interface (see section below) you'll be able to re-localize in a map or continue mapping graphically or programatically using ROS services. 
+In the RVIZ interface (see section below) you'll be able to re-localize in a map or continue mapping graphically or programmatically using ROS services. 
 
 On time of writing: there a **highly** experimental implementation of what I call "true lifelong" mapping that does support the method for removing nodes over time as well as adding nodes, this results in a true ability to map for life since the computation is bounded by removing extraneous or outdated information. Its recommended to run the non-full LifeLong mapping mode in the cloud for the increased computational burdens if you'd like to be continuously refining a map. However a real and desperately needed application of this is to have multi-session mapping to update just a section of the map or map half an area at a time to create a full (and then static) map for AMCL or Slam Toolbox localization mode, which this will handle in spades. The immediate plan is to create a mode within LifeLong mapping to decay old nodes to bound the computation and allow it to run on the edge by refining the experimental node. Continuing mapping (lifelong) should be used to build a complete map then switch to the pose-graph deformation localization mode until node decay is implemented, and **you should not see any substantial performance impacts**. 
 
 
 # Localization
 
-<!-- map refind local area localization Gif here-->
+<!-- map refined local area localization Gif here-->
 
 Localization mode consists of 3 things:
 - Loads existing serialized map into the node
 - Maintains a rolling buffer of recent scans in the pose-graph
 - After expiring from the buffer scans are removed and the underlying map is not affected
 
-Localization methods on image map files has been around for years and works relatively well. There has not been a great deal of work in academia to refine these algorithms to a degree that satesfies me. However SLAM is a rich and well benchmarked topic. The inspiration of this work was the concept of "Can we make localization, SLAM again?" such that we can take advantage of all the nice things about SLAM for localization, but remove the unbounded computational increase. 
+Localization methods on image map files has been around for years and works relatively well. There has not been a great deal of work in academia to refine these algorithms to a degree that satisfies me. However SLAM is a rich and well benchmarked topic. The inspiration of this work was the concept of "Can we make localization, SLAM again?" such that we can take advantage of all the nice things about SLAM for localization, but remove the unbounded computational increase. 
 
 To enable, set `mode: localization` in the configuration file to allow for the Ceres plugin to set itself correctly to be able to quickly add *and remove* nodes and constraints from the pose graph, but isn't strictly required, but a performance optimization. The localization mode will automatically load your pose graph, take the first scan and match it against the local area to further refine your estimated position, and start localizing. 
 
@@ -156,11 +156,11 @@ If you're a weirdo like me and you want to see how I came up with the settings I
 
 ![ceres_solver_comparison](https://user-images.githubusercontent.com/14944147/41576505-a6802d76-733c-11e8-8eca-334da2c8bd50.png)
 
-The data sets present solve time vs number of nodes in the pose graph on a large dataset, as that is not open source, but suffice to say that the settings I recommend work well. I think anyone would be hardset in a normal application to exceed or find that another solver type is better (that super low curve on the bottom one, yeah, that's it). Benchmark on a low power 7th gen i7 machine.
+The data sets present solve time vs number of nodes in the pose graph on a large dataset, as that is not open source, but suffice to say that the settings I recommend work well. I think anyone would be hard set in a normal application to exceed or find that another solver type is better (that super low curve on the bottom one, yeah, that's it). Benchmark on a low power 7th gen i7 machine.
 
 It can map _very_ large spaces with reasonable CPU and memory consumption. My default settings increase O(N) on number of elements in the pose graph. I recommend from extensive testing to use the `SPARSE_NORMAL_CHOLESKY` solver with Ceres and the `SCHUR_JACOBI` preconditioner. Using `LM` at the trust region strategy is comparable to the dogleg subspace strategy, but `LM` is much better supported so why argue with it. 
 
-You can get away without a loss function if your odometry is good (ie likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server.
+You can get away without a loss function if your odometry is good (i.e. likelihood for outliers is extremely low). If you have an abnormal application or expect wheel slippage, I might recommend a `HuberLoss` function, which is a really good catch-all loss function if you're looking for a place to start. All these options and more are available from the ROS parameter server.
 
 # API
 
@@ -204,7 +204,7 @@ The following settings and options are exposed to you. My default configuration
 
 `ceres_preconditioner` - The preconditioner to use with that solver. Options: `JACOBI`, `IDENTITY` (none), `SCHUR_JACOBI`. Defaults to `JACOBI`.
 
-`ceres_trust_strategy` - The trust region strategy. Line searach strategies are not exposed because they perform poorly for this use. Options: `LEVENBERG_MARQUARDT`, `DOGLEG`. Default: `LEVENBERG_MARQUARDT`.
+`ceres_trust_strategy` - The trust region strategy. Line search strategies are not exposed because they perform poorly for this use. Options: `LEVENBERG_MARQUARDT`, `DOGLEG`. Default: `LEVENBERG_MARQUARDT`.
 
 `ceres_dogleg_type` - The dogleg strategy to use if the trust strategy is `DOGLEG`. Options: `TRADITIONAL_DOGLEG`, `SUBSPACE_DOGLEG`. Default: `TRADITIONAL_DOGLEG`
 
@@ -220,7 +220,7 @@ The following settings and options are exposed to you. My default configuration
 
 `base_frame` - Base frame
 
-`scan_topic` - scan topic, *absolute* path, ei `/scan` not `scan`
+`scan_topic` - scan topic, *absolute* path, i.e. `/scan` not `scan`
 
 `scan_queue_size` - The number of scan messages to queue up before throwing away old ones. Should always be set to 1 in async mode
 
@@ -248,7 +248,7 @@ The following settings and options are exposed to you. My default configuration
 
 `resolution` - Resolution of the 2D occupancy map to generate
 
-`max_laser_range` - Maximum laser range to use for 2D occupancy map rastering
+`max_laser_range` - Maximum laser range to use for 2D occupancy map rasterizing
 
 `minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode
 
@@ -294,7 +294,7 @@ The following settings and options are exposed to you. My default configuration
 
 `correlation_search_space_smear_deviation` - Amount of multimodal smearing to smooth out responses
 
-`loop_search_space_dimension` - Size of the search grid over the loop closure algorith
+`loop_search_space_dimension` - Size of the search grid over the loop closure algorithm
 
 `loop_search_space_resolution` - Search grid resolution to do loop closure over
 
@@ -343,7 +343,7 @@ In order to do some operations quickly for continued mapping and localization, I
 
 ## Brief incursion into snaps
 
-Snap are completely isolated containerized packages that one can run through the Canonical organization on a large number of Linux distributions. They're similar to Docker containers but it doesn't share the kernel or any of the libraries, and rather has everything internal as essentially a seperate partitioned operating system based on Ubuntu Core. 
+Snap are completely isolated containerized packages that one can run through the Canonical organization on a large number of Linux distributions. They're similar to Docker containers but it doesn't share the kernel or any of the libraries, and rather has everything internal as essentially a separate partitioned operating system based on Ubuntu Core. 
 
 We package up slam toolbox in this way for a nice multiple-on speed up in execution from a couple of pretty nuanced reasons in this particular project, but generally speaking you shouldn't expect a speedup from a snap. 
 

launch/localization_launch.py

@@ -1,17 +1,41 @@
+
+import os
+
 from launch import LaunchDescription
-import launch_ros.actions
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    return LaunchDescription([
-        launch_ros.actions.Node(
-          parameters=[
-            get_package_share_directory("slam_toolbox") + '/config/mapper_params_localization.yaml'
-          ],
-          package='slam_toolbox',
-          executable='localization_slam_toolbox_node',
-          name='slam_toolbox',
-          output='screen'
-        )
-    ])
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    slam_params_file = LaunchConfiguration('slam_params_file')
+
+    declare_use_sim_time_argument = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation/Gazebo clock')
+    declare_slam_params_file_cmd = DeclareLaunchArgument(
+        'slam_params_file',
+        default_value=os.path.join(get_package_share_directory("slam_toolbox"),
+                                   'config', 'mapper_params_localization.yaml'),
+        description='Full path to the ROS2 parameters file to use for the slam_toolbox node')
+
+    start_localization_slam_toolbox_node = Node(
+        parameters=[
+          slam_params_file,
+          {'use_sim_time': use_sim_time}
+        ],
+        package='slam_toolbox',
+        executable='localization_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen')
+
+    ld = LaunchDescription()
+
+    ld.add_action(declare_use_sim_time_argument)
+    ld.add_action(declare_slam_params_file_cmd)
+    ld.add_action(start_localization_slam_toolbox_node)
+
+    return ld

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>slam_toolbox</name>
-  <version>2.6.6</version>
+  <version>2.6.7</version>
   <description>
      This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
   </description>

solvers/ceres_solver.cpp

@@ -354,6 +354,24 @@ void CeresSolver::RemoveNode(kt_int32s id)
   boost::mutex::scoped_lock lock(nodes_mutex_);
   GraphIterator nodeit = nodes_->find(id);
   if (nodeit != nodes_->end()) {
+    if (problem_->HasParameterBlock(&nodeit->second(0)) &&
+        problem_->HasParameterBlock(&nodeit->second(1)) &&
+        problem_->HasParameterBlock(&nodeit->second(2)))
+    {
+      problem_->RemoveParameterBlock(&nodeit->second(0));
+      problem_->RemoveParameterBlock(&nodeit->second(1));
+      problem_->RemoveParameterBlock(&nodeit->second(2));
+      RCLCPP_DEBUG(
+        logger_,
+        "RemoveNode: Removed node id %d" ,nodeit->first);
+    }
+    else
+    {
+      RCLCPP_DEBUG(
+        logger_,
+        "RemoveNode: Missing parameter blocks for "
+        "node id %d", nodeit->first);
+    }
     nodes_->erase(nodeit);
   } else {
     RCLCPP_ERROR(node_->get_logger(), "RemoveNode: Failed to find node matching id %i",

src/experimental/slam_toolbox_lifelong.cpp

@@ -443,3 +443,10 @@ double LifelongSlamToolbox::computeReadingOverlapRatio(
 }
 
 }  // namespace slam_toolbox
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::LifelongSlamToolbox)

src/experimental/slam_toolbox_map_and_localization.cpp

@@ -175,3 +175,10 @@ LocalizedRangeScan * MapAndLocalizationSlamToolbox::addScan(
 }
 
 }  // namespace slam_toolbox
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::MapAndLocalizationSlamToolbox)

src/loop_closure_assistant.cpp

@@ -90,7 +90,7 @@ void LoopClosureAssistant::processInteractiveFeedback(const
     return;
   }
 
-  const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1;
+  const int id = std::stoi(feedback->marker_name, nullptr, 10);
 
   // was depressed, something moved, and now released
   if (feedback->event_type ==

src/merge_maps_kinematic.cpp

@@ -207,11 +207,18 @@ void MergeMapsKinematic::transformScan(
   BoundingBox2 bbox = (*iter)->GetBoundingBox();
   const Vector2<kt_double> bbox_min_corr =
     applyCorrection(bbox.GetMinimum(), submap_correction);
-  bbox.SetMinimum(bbox_min_corr);
   const Vector2<kt_double> bbox_max_corr =
     applyCorrection(bbox.GetMaximum(), submap_correction);
-  bbox.SetMaximum(bbox_max_corr);
-  (*iter)->SetBoundingBox(bbox);
+  Vector2<kt_double> bbox_min_right_corr{bbox.GetMaximum().GetX(),bbox.GetMinimum().GetY()};
+  bbox_min_right_corr = applyCorrection(bbox_min_right_corr, submap_correction);
+  Vector2<kt_double> bbox_max_left_corr{bbox.GetMinimum().GetX(),bbox.GetMaximum().GetY()};
+  bbox_max_left_corr = applyCorrection(bbox_max_left_corr, submap_correction);
+  BoundingBox2 transformed_bbox;
+  transformed_bbox.Add(bbox_min_corr);
+  transformed_bbox.Add(bbox_max_corr);
+  transformed_bbox.Add(bbox_min_right_corr);
+  transformed_bbox.Add(bbox_max_left_corr);
+  (*iter)->SetBoundingBox(transformed_bbox);
 
   // TRANSFORM UNFILTERED POINTS USED
   PointVectorDouble UPR_vec = (*iter)->GetPointReadings();

src/slam_toolbox_async.cpp

@@ -73,3 +73,10 @@ bool AsynchronousSlamToolbox::deserializePoseGraphCallback(
 }
 
 }  // namespace slam_toolbox
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::AsynchronousSlamToolbox)

src/slam_toolbox_localization.cpp

@@ -239,3 +239,10 @@ void LocalizationSlamToolbox::localizePoseCallback(
 }
 
 }  // namespace slam_toolbox
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(slam_toolbox::LocalizationSlamToolbox)