Merge pull request #3 from leggedrobotics/fix/multithreading

Fix/multithreading

Author: mktk1117

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -7,9 +7,7 @@
 
 // STL
 #include <iostream>
-
-// Boost
-#include <boost/thread/recursive_mutex.hpp>
+#include <mutex>
 
 // Eigen
 #include <Eigen/Dense>
@@ -55,7 +53,7 @@ class ElevationMappingNode {
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud);
-  void publishAsPointCloud();
+  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);
@@ -66,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_;
@@ -97,7 +96,6 @@ class ElevationMappingNode {
   std::string mapFrameId_;
   std::string correctedMapFrameId_;
   std::string baseFrameId_;
-  grid_map::GridMap gridMap_;
 
   // map topics info
   std::vector<std::vector<std::string>> map_topics_;
@@ -116,20 +114,24 @@ class ElevationMappingNode {
   Eigen::Vector3d lowpassPosition_;
   Eigen::Vector4d lowpassOrientation_;
 
-  boost::recursive_mutex mapMutex_;
+  std::mutex mapMutex_;  // protects gridMap_
+  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_;
-  bool isGridmapUpdated_;
   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);
@@ -184,25 +188,29 @@ void ElevationMappingNode::publishMapOfIndex(int index) {
   }
   grid_map_msgs::GridMap msg;
   std::vector<std::string> layers;
-  for (const auto& layer : map_layers_[index]) {
-    const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end();
-    if (is_layer_in_all && gridMap_.exists(layer)) {
-      layers.push_back(layer);
-    } else if (map_.exists_layer(layer)) {
-      // if there are layers which is not in the syncing layer.
-      boost::recursive_mutex::scoped_lock scopedLockForGridMap(mapMutex_);
-      ElevationMappingWrapper::RowMatrixXf map_data;
-      map_.get_layer_data(layer, map_data);
-      gridMap_.add(layer, map_data);
-      layers.push_back(layer);
+
+  {  // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in
+     // map_layers_all_
+    std::lock_guard<std::mutex> lock(mapMutex_);
+    for (const auto& layer : map_layers_[index]) {
+      const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end();
+      if (is_layer_in_all && gridMap_.exists(layer)) {
+        layers.push_back(layer);
+      } else if (map_.exists_layer(layer)) {
+        // if there are layers which is not in the syncing layer.
+        ElevationMappingWrapper::RowMatrixXf map_data;
+        map_.get_layer_data(layer, map_data);
+        gridMap_.add(layer, map_data);
+        layers.push_back(layer);
+      }
     }
+    if (layers.empty()) {
+      return;
+    }
+
+    grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg);
   }
-  if (layers.empty()) {
-    return;
-  }
-  boost::recursive_mutex::scoped_lock scopedLockForGridMap(mapMutex_);
-  grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg);
-  scopedLockForGridMap.unlock();
+
   msg.basic_layers = map_basic_layers_[index];
   mapPubs_[index].publish(msg);
 }
@@ -226,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_++;
 }
@@ -259,18 +276,22 @@ 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() {
+void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const {
   sensor_msgs::PointCloud2 msg;
-  grid_map::GridMapRosConverter::toPointCloud(gridMap_, "elevation", msg);
+  grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg);
   pointPub_.publish(msg);
 }
 
@@ -302,7 +323,7 @@ bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request
   grid_map::Index index;
   grid_map::GridMap subMap;
   {
-    boost::recursive_mutex::scoped_lock scopedLockForGridMap(mapMutex_);
+    std::lock_guard<std::mutex> lock(mapMutex_);
     subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess);
   }
   const auto& length = subMap.getLength();
@@ -453,14 +474,14 @@ void ElevationMappingNode::publishStatistics(const ros::TimerEvent&) {
 
 void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) {
   std::vector<std::string> layers(map_layers_all_.begin(), map_layers_all_.end());
-  boost::recursive_mutex::scoped_lock scopedLockForGridMap(mapMutex_);
+  std::lock_guard<std::mutex> lock(mapMutex_);
   map_.get_grid_map(gridMap_, layers);
   gridMap_.setTimestamp(ros::Time::now().toNSec());
   alivePub_.publish(std_msgs::Empty());
 
   // Mostly debug purpose
   if (enablePointCloudPublishing_) {
-    publishAsPointCloud();
+    publishAsPointCloud(gridMap_);
   }
   if (enableNormalArrowPublishing_) {
     publishNormalAsArrow(gridMap_);
@@ -515,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"];
@@ -545,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();
@@ -576,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