Initial pose reset noetic (#337)

* first skeleton for clearing observation buffer of localization measurements * clearing last and running scans for reset of state in localization mode
SteveMacenski · Mar 2, 2021 · 709e11e · 709e11e
1 parent 54d735f
commit 709e11e
Show file tree

Hide file tree

Showing 6 changed files with 97 additions and 0 deletions.
diff --git a/slam_toolbox/include/slam_toolbox/slam_mapper.hpp b/slam_toolbox/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_;
 };

diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp
@@ -20,6 +20,7 @@
 #define SLAM_TOOLBOX_SLAM_TOOLBOX_LOCALIZATION_H_
 
 #include "slam_toolbox/slam_toolbox_common.hpp"
+#include "std_srvs/Empty.h"
 
 namespace slam_toolbox
 {
@@ -38,6 +39,9 @@ class LocalizationSlamToolbox : public SlamToolbox
   void localizePoseCallback(
     const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
 
+  bool clearLocalizationBuffer(
+    std_srvs::Empty::Request& req,
+    std_srvs::Empty::Response& resp);
   virtual bool serializePoseGraphCallback(
     slam_toolbox_msgs::SerializePoseGraph::Request& req,
     slam_toolbox_msgs::SerializePoseGraph::Response& resp) override final;
@@ -50,6 +54,7 @@ class LocalizationSlamToolbox : public SlamToolbox
     karto::Pose2& karto_pose) override final;
 
   ros::Subscriber localization_pose_sub_;
+  ros::ServiceServer clear_localization_;
 };
 
 }

diff --git a/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h b/slam_toolbox/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -1585,6 +1585,18 @@ namespace karto
      */
     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
@@ -1951,6 +1963,7 @@ namespace karto
     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.

diff --git a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp
@@ -106,6 +106,15 @@ namespace karto
       return m_pLastScan;
     }
 
+    /**
+     * Clears last scan
+     * @param deviceId
+     */
+    inline void ClearLastScan()
+    {
+      m_pLastScan = NULL;
+    }
+
     /**
      * Sets the last scan
      * @param pScan
@@ -287,6 +296,24 @@ namespace karto
     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
@@ -2883,6 +2910,33 @@ namespace karto
     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

diff --git a/slam_toolbox/src/slam_mapper.cpp b/slam_toolbox/src/slam_mapper.cpp
@@ -51,6 +51,13 @@ void SMapper::setMapper(karto::Mapper* mapper)
   mapper_.reset(mapper);
 }
 
+/*****************************************************************************/
+void SMapper::clearLocalizationBuffer()
+/*****************************************************************************/
+{
+  mapper_->ClearLocalizationBuffer();
+}
+
 /*****************************************************************************/
 karto::OccupancyGrid* SMapper::getOccupancyGrid(const double& resolution)
 /*****************************************************************************/

diff --git a/slam_toolbox/src/slam_toolbox_localization.cpp b/slam_toolbox/src/slam_toolbox_localization.cpp
@@ -29,6 +29,9 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(ros::NodeHandle& nh)
   processor_type_ = PROCESS_LOCALIZATION;
   localization_pose_sub_ = nh.subscribe("/initialpose", 1,
     &LocalizationSlamToolbox::localizePoseCallback, this);
+  clear_localization_ = nh.advertiseService(
+    "clear_localization_buffer",
+    &LocalizationSlamToolbox::clearLocalizationBuffer, this);
 
   std::string filename;
   geometry_msgs::Pose2D pose;
@@ -58,6 +61,17 @@ LocalizationSlamToolbox::LocalizationSlamToolbox(ros::NodeHandle& nh)
   return;
 }
 
+/*****************************************************************************/
+bool LocalizationSlamToolbox::clearLocalizationBuffer(
+  std_srvs::Empty::Request& req,
+  std_srvs::Empty::Response& resp)
+/*****************************************************************************/
+{
+  ROS_INFO("LocalizationSlamToolbox: Clearing localization buffer.");
+  smapper_->clearLocalizationBuffer();
+  return true;
+}
+
 /*****************************************************************************/
 bool LocalizationSlamToolbox::serializePoseGraphCallback(
   slam_toolbox_msgs::SerializePoseGraph::Request& req,
@@ -205,6 +219,8 @@ void LocalizationSlamToolbox::localizePoseCallback(const
 
   first_measurement_ = true;
 
+  smapper_->clearLocalizationBuffer();
+
   ROS_INFO("LocalizePoseCallback: Localizing to: (%0.2f %0.2f), theta=%0.2f",
     msg->pose.pose.position.x, msg->pose.pose.position.y,
     tf2::getYaw(msg->pose.pose.orientation));