protect shared error state. make counter atomic. make configurable bool atomic

Author: rubengrandia

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -53,7 +53,7 @@ class ElevationMappingNode {
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud);
-  void publishAsPointCloud(const grid_map::GridMap& map);
+  void publishAsPointCloud(const grid_map::GridMap& map) const;
   bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response);
   bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response);
   bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response);
@@ -64,16 +64,17 @@ class ElevationMappingNode {
   void updateVariance(const ros::TimerEvent&);
   void updateTime(const ros::TimerEvent&);
   void updateGridMap(const ros::TimerEvent&);
-  void publishNormalAsArrow(const grid_map::GridMap& map);
+  void publishNormalAsArrow(const grid_map::GridMap& map) const;
   void initializeWithTF();
   void publishMapToOdom(double error);
   void publishStatistics(const ros::TimerEvent&);
   void publishMapOfIndex(int index);
 
-  visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id);
+  visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const;
   ros::NodeHandle nh_;
   std::vector<ros::Subscriber> pointcloudSubs_;
   std::vector<ros::Publisher> mapPubs_;
+  tf::TransformBroadcaster tfBroadcaster_;
   ros::Publisher alivePub_;
   ros::Publisher pointPub_;
   ros::Publisher normalPub_;
@@ -117,17 +118,20 @@ class ElevationMappingNode {
   grid_map::GridMap gridMap_;
   std::atomic_bool isGridmapUpdated_;  // needs to be atomic (read is not protected by mapMutex_)
 
+  std::mutex errorMutex_; // protects positionError_, and orientationError_
   double positionError_;
   double orientationError_;
+
   double positionAlpha_;
   double orientationAlpha_;
+
   double recordableFps_;
-  bool enablePointCloudPublishing_;
+  std::atomic_bool enablePointCloudPublishing_;
   bool enableNormalArrowPublishing_;
   bool enableDriftCorrectedTFPublishing_;
   bool useInitializerAtStart_;
   double initializeTfGridSize_;
-  int pointCloudProcessCounter_;
+  std::atomic_int pointCloudProcessCounter_;
 };
 
 }  // namespace elevation_mapping_cupy

elevation_mapping_cupy/src/elevation_mapping_ros.cpp

@@ -36,6 +36,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
   std::vector<std::string> pointcloud_topics;
   std::vector<std::string> map_topics;
   double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps;
+  bool enablePointCloudPublishing(false);
 
   nh.param<std::vector<std::string>>("pointcloud_topics", pointcloud_topics, {"points"});
   nh.getParam("publishers", publishers);
@@ -55,10 +56,13 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
   nh.param<double>("initialize_tf_grid_size", initializeTfGridSize_, 0.5);
   nh.param<double>("map_acquire_fps", updateGridMapFps, 5.0);
   nh.param<double>("publish_statistics_fps", publishStatisticsFps, 1.0);
-  nh.param<bool>("enable_pointcloud_publishing", enablePointCloudPublishing_, false);
+  nh.param<bool>("enable_pointcloud_publishing", enablePointCloudPublishing, false);
   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);
+
+  enablePointCloudPublishing_ = enablePointCloudPublishing;
+
   for (const auto& pointcloud_topic : pointcloud_topics) {
     ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, &ElevationMappingNode::pointcloudCallback, this);
     pointcloudSubs_.push_back(sub);
@@ -230,16 +234,25 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cl
     ROS_ERROR("%s", ex.what());
     return;
   }
-  map_.input(pointCloud, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError_, orientationError_);
+
+  double positionError{0.0};
+  double orientationError{0.0};
+  {
+    std::lock_guard<std::mutex> lock(errorMutex_);
+    positionError = positionError_;
+    orientationError = orientationError_;
+  }
+
+  map_.input(pointCloud, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, orientationError);
 
   if (enableDriftCorrectedTFPublishing_) {
     publishMapToOdom(map_.get_additive_mean_error());
   }
 
   ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast<int>(pointCloud->size()),
                      (ros::Time::now() - start).toSec());
-  ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError_);
-  ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError_);
+  ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError);
+  ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError);
   // This is used for publishing as statistics.
   pointCloudProcessCounter_++;
 }
@@ -263,16 +276,20 @@ void ElevationMappingNode::updatePose(const ros::TimerEvent&) {
                               transformTf.getRotation().w());
   lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_;
   lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_;
-  positionError_ = (position3 - lowpassPosition_).norm();
-  orientationError_ = (orientation - lowpassOrientation_).norm();
+  {
+    std::lock_guard<std::mutex> lock(errorMutex_);
+    positionError_ = (position3 - lowpassPosition_).norm();
+    orientationError_ = (orientation - lowpassOrientation_).norm();
+  }
+
   if (useInitializerAtStart_) {
     ROS_INFO("Clearing map with initializer.");
     initializeWithTF();
     useInitializerAtStart_ = false;
   }
 }
 
-void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) {
+void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const {
   sensor_msgs::PointCloud2 msg;
   grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg);
   pointPub_.publish(msg);
@@ -519,7 +536,7 @@ bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request
   return true;
 }
 
-void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) {
+void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const {
   auto startTime = ros::Time::now();
 
   const auto& normalX = map["normal_x"];
@@ -549,7 +566,7 @@ void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) {
 }
 
 visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end,
-                                                                     const int id) {
+                                                                     const int id) const {
   visualization_msgs::Marker marker;
   marker.header.frame_id = mapFrameId_;
   marker.header.stamp = ros::Time::now();
@@ -580,13 +597,12 @@ visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen
 }
 
 void ElevationMappingNode::publishMapToOdom(double error) {
-  static tf::TransformBroadcaster br;
   tf::Transform transform;
   transform.setOrigin(tf::Vector3(0.0, 0.0, error));
   tf::Quaternion q;
   q.setRPY(0, 0, 0);
   transform.setRotation(q);
-  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_));
+  tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_));
 }
 
 }  // namespace elevation_mapping_cupy