Added a param called alwaysClearWithInitializer.

leggedrobotics · Mar 25, 2024 · d33feca · d33feca
1 parent 6b25b7e
commit d33feca
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 0 deletions.
diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp
@@ -184,6 +184,7 @@ class ElevationMappingNode {
   bool enableDriftCorrectedTFPublishing_;
   bool useInitializerAtStart_;
   double initializeTfGridSize_;
+  bool alwaysClearWithInitializer_;
   std::atomic_int pointCloudProcessCounter_;
 };
 

diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp
@@ -68,6 +68,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
   nh.param<bool>("enable_normal_arrow_publishing", enableNormalArrowPublishing_, false);
   nh.param<bool>("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_, false);
   nh.param<bool>("use_initializer_at_start", useInitializerAtStart_, false);
+  nh.param<bool>("always_clear_with_initializer", alwaysClearWithInitializer_, false);
 
   enablePointCloudPublishing_ = enablePointCloudPublishing;
 
@@ -541,6 +542,9 @@ bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request
 bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
   ROS_INFO("Clearing map.");
   map_.clear();
+  if (alwaysClearWithInitializer_) {
+    initializeWithTF();
+  }
   return true;
 }