Fix hreading issues in PSM and servo (moveit#257)

AndyZe · Sep 6, 2020 · b63538c · b63538c
1 parent 9afdc30
commit b63538c
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 5 deletions.
diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp
@@ -76,7 +76,7 @@ class ServoFixture : public ::testing::Test
   void SetUp() override
   {
     executor_->add_node(node_);
-    executor_task_fut_ = std::async(std::launch::async, [this]() { this->executor_->spin(); });
+    executor_thread_ = std::thread([this]() { this->executor_->spin(); });
   }
 
   ServoFixture()
@@ -104,6 +104,8 @@ class ServoFixture : public ::testing::Test
       client_servo_stop_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
     }
     executor_->cancel();
+    if (executor_thread_.joinable())
+      executor_thread_.join();
   }
 
   // Set up for callbacks (so they aren't run for EVERY test)
@@ -434,7 +436,7 @@ class ServoFixture : public ::testing::Test
   rclcpp::Node::SharedPtr node_;
   rclcpp::Executor::SharedPtr executor_;
   moveit_servo::ServoParametersPtr parameters_;
-  std::future<void> executor_task_fut_;
+  std::thread executor_thread_;
 
   // ROS interfaces
   // Service Clients

diff --git a/...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
@@ -488,7 +488,7 @@ class PlanningSceneMonitor : private boost::noncopyable
   std::unique_ptr<boost::thread> publish_planning_scene_;
   double publish_planning_scene_frequency_;
   SceneUpdateType publish_update_types_;
-  SceneUpdateType new_scene_update_;
+  std::atomic<SceneUpdateType> new_scene_update_;
   boost::condition_variable_any new_scene_update_condition_;
 
   // subscribe to various sources of data

diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -1188,7 +1188,8 @@ void PlanningSceneMonitor::stopStateMonitor()
     attached_collision_object_subscriber_.reset();
 
   // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
-  state_update_timer_.reset();
+  if (state_update_timer_)
+    state_update_timer_->cancel();
   {
     std::unique_lock<std::mutex> lock(state_pending_mutex_);
     state_update_pending_ = false;
@@ -1292,7 +1293,8 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz)
     // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
     // ROS original: state_update_timer_.stop();
     // TODO: re-enable WallTimer stop()
-    state_update_timer_.reset();
+    if (state_update_timer_)
+      state_update_timer_->cancel();
     std::unique_lock<std::mutex> lock(state_pending_mutex_);
     dt_state_update_ = std::chrono::duration<double>(0.0);
     if (state_update_pending_)