fix thread handling of node_status_publisher. (#2058)

autowarefoundation · Mar 6, 2019 · 00d8f41 · 00d8f41
1 parent 6a5c720
commit 00d8f41
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/...rc/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h b/...rc/system/autoware_health_checker/include/autoware_health_checker/node_status_publisher.h
@@ -108,6 +108,8 @@ class NodeStatusPublisher {
   void publishStatus();
   bool node_activated_;
   std::mutex mtx_;
+  boost::thread publish_thread_;
+  bool ros_ok_;
 };
 }
-#endif // NODE_STATUS_PUBLISHER_H_INCLUDED
+#endif // NODE_STATUS_PUBLISHER_H_INCLUDED
diff --git a/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp b/ros/src/system/autoware_health_checker/src/node_status_publisher.cpp
@@ -23,17 +23,21 @@ namespace autoware_health_checker {
 NodeStatusPublisher::NodeStatusPublisher(ros::NodeHandle nh,
                                          ros::NodeHandle pnh) {
   node_activated_ = false;
+  ros_ok_ = true;
   nh_ = nh;
   pnh_ = pnh;
   status_pub_ =
       nh_.advertise<autoware_system_msgs::NodeStatus>("node_status", 10);
 }
 
-NodeStatusPublisher::~NodeStatusPublisher() {}
+NodeStatusPublisher::~NodeStatusPublisher() {
+  ros_ok_ = false;
+  publish_thread_.join();
+}
 
 void NodeStatusPublisher::publishStatus() {
   ros::Rate rate = ros::Rate(autoware_health_checker::UPDATE_RATE);
-  while (ros::ok()) {
+  while (ros_ok_) {
     mtx_.lock();
     autoware_system_msgs::NodeStatus status;
     status.node_activated = node_activated_;
@@ -70,8 +74,7 @@ void NodeStatusPublisher::publishStatus() {
 }
 
 void NodeStatusPublisher::ENABLE() {
-  boost::thread publish_thread(
-      boost::bind(&NodeStatusPublisher::publishStatus, this));
+  publish_thread_ = boost::thread(boost::bind(&NodeStatusPublisher::publishStatus, this));
   return;
 }
 
@@ -219,4 +222,4 @@ std::string NodeStatusPublisher::doubeToJson(double value) {
   write_json(ss, pt);
   return ss.str();
 }
-}
+}