adding reset can buffer on relocalization update [ros2]

include/slam_toolbox/slam_mapper.hpp

@@ -56,6 +56,8 @@ class SMapper
   void setMapper(karto::Mapper * mapper);
   karto::Mapper * getMapper();
 
+  void clearLocalizationBuffer();
+
 protected:
   std::unique_ptr<karto::Mapper> mapper_;
 };

include/slam_toolbox/slam_toolbox_localization.hpp

@@ -21,6 +21,7 @@
 
 #include <memory>
 #include "slam_toolbox/slam_toolbox_common.hpp"
+#include "std_srvs/srv/empty.hpp"
 
 namespace slam_toolbox
 {
@@ -37,6 +38,10 @@ class LocalizationSlamToolbox : public SlamToolbox
     sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override;
   void localizePoseCallback(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  bool clearLocalizationBuffer(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+    std::shared_ptr<std_srvs::srv::Empty::Response> resp);
 
   bool serializePoseGraphCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
@@ -54,6 +59,7 @@ class LocalizationSlamToolbox : public SlamToolbox
 
   std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>>
   localization_pose_sub_;
+  std::shared_ptr<rclcpp::Service<std_srvs::srv::Empty> > clear_localization_;
 };
 
 }  // namespace slam_toolbox

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -1603,6 +1603,18 @@ class KARTO_EXPORT MapperSensorManager
    */
   void SetLastScan(LocalizedRangeScan * pScan);
 
+  /**
+   * Clears the laser scan of device
+   * @param pScan
+   */
+  void ClearLastScan(LocalizedRangeScan* pScan);
+
+  /**
+   * Clears the laser scan of device name
+   * @param pScan
+   */
+  void ClearLastScan(const Name& name);
+
   /**
    * Gets the scan with the given unique id
    * @param id
@@ -1965,6 +1977,7 @@ class KARTO_EXPORT Mapper : public Module
   kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan * pScan);
   kt_bool ProcessLocalization(LocalizedRangeScan * pScan);
   kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan> *);
+  void ClearLocalizationBuffer();
 
   /**
    * Returns all processed scans added to the mapper.

lib/karto_sdk/src/Mapper.cpp

@@ -112,6 +112,15 @@ class ScanManager
     return m_pLastScan;
   }
 
+  /**
+   * Clears last scan
+   * @param deviceId
+   */
+  inline void ClearLastScan()
+  {
+    m_pLastScan = NULL;
+  }
+
   /**
    * Sets the last scan
    * @param pScan
@@ -287,6 +296,24 @@ void MapperSensorManager::SetLastScan(LocalizedRangeScan * pScan)
   GetScanManager(pScan)->SetLastScan(pScan);
 }
 
+/**
+ * Clears the last scan of device of given scan
+ * @param pScan
+ */
+void MapperSensorManager::ClearLastScan(LocalizedRangeScan* pScan)
+{
+  GetScanManager(pScan)->ClearLastScan();
+}
+
+/**
+ * Clears the last scan of device name
+ * @param pScan
+ */
+void MapperSensorManager::ClearLastScan(const Name& name)
+{
+  GetScanManager(name)->ClearLastScan();
+}
+
 /**
  * Adds scan to scan vector of device that recorded scan
  * @param pScan
@@ -2819,6 +2846,33 @@ kt_bool Mapper::ProcessLocalization(LocalizedRangeScan * pScan)
   return true;
 }
 
+void Mapper::ClearLocalizationBuffer()
+{
+  while (!m_LocalizationScanVertices.empty())
+  {
+    LocalizationScanVertex& oldLSV = m_LocalizationScanVertices.front();
+    RemoveNodeFromGraph(oldLSV.vertex);
+    oldLSV.vertex->RemoveObject();
+    m_pMapperSensorManager->RemoveScan(oldLSV.scan);
+    if (oldLSV.scan)
+    {
+      delete oldLSV.scan;
+      oldLSV.scan = NULL;
+    }
+
+    m_LocalizationScanVertices.pop();
+  }
+
+  std::vector<Name> names = m_pMapperSensorManager->GetSensorNames();
+  for (uint i = 0; i != names.size(); i++)
+  {
+    m_pMapperSensorManager->ClearRunningScans(names[i]);
+    m_pMapperSensorManager->ClearLastScan(names[i]);
+  }
+
+  return;
+}
+
 kt_bool Mapper::RemoveNodeFromGraph(Vertex<LocalizedRangeScan> * vertex_to_remove)
 {
   // 1) delete edges in adjacent vertices, graph, and optimizer

src/slam_mapper.cpp

@@ -52,6 +52,13 @@ void SMapper::setMapper(karto::Mapper * mapper)
   mapper_.reset(mapper);
 }
 
+/*****************************************************************************/
+void SMapper::clearLocalizationBuffer()
+/*****************************************************************************/
+{
+  mapper_->ClearLocalizationBuffer();
+}
+
 /*****************************************************************************/
 karto::OccupancyGrid * SMapper::getOccupancyGrid(const double & resolution)
 /*****************************************************************************/

src/slam_toolbox_localization.cpp

@@ -34,6 +34,10 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(rclcpp::NodeOptions options)
     "/initialpose", 1,
     std::bind(&LocalizationSlamToolbox::localizePoseCallback,
     this, std::placeholders::_1));
+  clear_localization_ = this->create_service<std_srvs::srv::Empty>(
+    "clear_localization_buffer",
+    std::bind(&LocalizationSlamToolbox::clearLocalizationBuffer, this,
+    std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   // in localization mode, we cannot allow for interactive mode
   enable_interactive_mode_ = false;
@@ -68,6 +72,19 @@ void LocalizationSlamToolbox::loadPoseGraphByParams()
   }
 }
 
+/*****************************************************************************/
+bool LocalizationSlamToolbox::clearLocalizationBuffer(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<std_srvs::srv::Empty::Request> req,
+  std::shared_ptr<std_srvs::srv::Empty::Response> resp)
+/*****************************************************************************/
+{
+  RCLCPP_INFO(get_logger(),
+    "LocalizationSlamToolbox: Clearing localization buffer.");
+  smapper_->clearLocalizationBuffer();
+  return true;
+}
+
 /*****************************************************************************/
 bool LocalizationSlamToolbox::serializePoseGraphCallback(
   const std::shared_ptr<rmw_request_id_t> request_header,
@@ -203,6 +220,8 @@ void LocalizationSlamToolbox::localizePoseCallback(
 
   first_measurement_ = true;
 
+  smapper_->clearLocalizationBuffer();
+
   RCLCPP_INFO(get_logger(),
     "LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f",
     msg->pose.pose.position.x, msg->pose.pose.position.y,