Small cleanup

PX4 · Oct 25, 2019 · c05a9f6 · c05a9f6
1 parent fb3d083
commit c05a9f6
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 9 deletions.
diff --git a/local_planner/include/local_planner/local_planner_nodelet.h b/local_planner/include/local_planner/local_planner_nodelet.h
@@ -53,7 +53,6 @@ class LocalPlanner;
 class WaypointGenerator;
 
 struct cameraData {
-
   std::string topic_;
   ros::Subscriber pointcloud_sub_;
 
@@ -63,15 +62,12 @@ struct cameraData {
   pcl::PointCloud<pcl::PointXYZ> transformed_cloud_;
   bool transformed_;
 
-
   bool transform_registered_ = false;
   std::unique_ptr<std::mutex> camera_mutex_;
 
-  std::unique_ptr<std::mutex> camera_cv_mutex_;
   std::unique_ptr<std::condition_variable> camera_cv_;
   std::thread transform_thread_;
   FOV fov_fcu_frame_;
-
 };
 
 class LocalPlannerNodelet : public nodelet::Nodelet {

diff --git a/local_planner/src/nodes/local_planner_nodelet.cpp b/local_planner/src/nodes/local_planner_nodelet.cpp
@@ -19,15 +19,27 @@ LocalPlannerNodelet::LocalPlannerNodelet() : tf_buffer_(5.f), spin_dt_(0.1) {}
 
 LocalPlannerNodelet::~LocalPlannerNodelet() {
   should_exit_ = true;
-  data_ready_cv_.notify_all();
+  {
+    std::lock_guard<std::mutex> guard(data_ready_mutex_);
+    data_ready_cv_.notify_all();
+  }
 
-  if (worker.joinable()) worker.join();
-  if (worker_tf_listener.joinable()) worker_tf_listener.join();
+  {
+    std::lock_guard<std::mutex> guard(buffered_transforms_mutex_);
+    tf_buffer_cv_.notify_all();
+  }
 
   for (size_t i = 0; i < cameras_.size(); ++i) {
+    {
+      std::lock_guard<std::mutex> guard(*cameras_[i].camera_mutex_);
+      cameras_[i].camera_cv_->notify_all();
+    }
     if (cameras_[i].transform_thread_.joinable()) cameras_[i].transform_thread_.join();
   }
 
+  if (worker.joinable()) worker.join();
+  if (worker_tf_listener.joinable()) worker_tf_listener.join();
+
   if (server_ != nullptr) delete server_;
   if (tf_listener_ != nullptr) delete tf_listener_;
 }
@@ -126,7 +138,6 @@ void LocalPlannerNodelet::initializeCameraSubscribers(std::vector<std::string>&
 
   for (size_t i = 0; i < camera_topics.size(); i++) {
     cameras_[i].camera_mutex_.reset(new std::mutex);
-    cameras_[i].camera_cv_mutex_.reset(new std::mutex);
     cameras_[i].camera_cv_.reset(new std::condition_variable);
 
     cameras_[i].received_ = false;
@@ -524,12 +535,16 @@ void LocalPlannerNodelet::pointCloudTransformThread(int index) {
       }
     }
 
+    if (should_exit_) {
+      break;
+    }
+
     if (waiting_on_transform) {
       std::unique_lock<std::mutex> lck(buffered_transforms_mutex_);
       tf_buffer_cv_.wait_for(lck, std::chrono::milliseconds(5000));
     }
     if (waiting_on_cloud) {
-      std::unique_lock<std::mutex> lck(*(cameras_[index].camera_cv_mutex_));
+      std::unique_lock<std::mutex> lck(*(cameras_[index].camera_mutex_));
       cameras_[index].camera_cv_->wait_for(lck, std::chrono::milliseconds(5000));
     }
   }