Merge branch 'issue/linear_velocity_leak' into 'master'

bug fix: linear velocity leak See merge request autowarefoundation/autoware.ai/common!64
autowarefoundation · Nov 26, 2019 · e91b3e0 · e91b3e0
2 parents c0fc751 + 8ed21f5
commit e91b3e0
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/emergency_handler/include/emergency_handler/emergency_handler.h b/emergency_handler/include/emergency_handler/emergency_handler.h
@@ -68,7 +68,7 @@ class EmergencyHandler
   std::mutex priority_mutex_;
   std::mutex error_status_mutex_;
 
-  int priority_;
+  int priority_, latest_priority_;
   void updatePriority(const bool status_ok);
   ros::Time callback_time_;
 

diff --git a/emergency_handler/src/emergency_handler.cpp b/emergency_handler/src/emergency_handler.cpp
@@ -55,6 +55,7 @@ EmergencyHandler::EmergencyHandler(const ros::NodeHandle& nh, const ros::NodeHan
   EmergencyHandler::setupPublisher();
 
   priority_ = priority_table.no_error;
+  latest_priority_ = priority_table.no_error;
   callback_time_ = ros::Time::now();
   is_system_status_received_ = false;
 
@@ -206,7 +207,7 @@ void EmergencyHandler::run(void)
     {
       if ((ros::Time::now() - start_time).toSec() >= 10.0)
       {
-        priority_ = priority_table.no_error;
+        priority_ = latest_priority_;
         is_urgent = false;
       }
     }
@@ -234,7 +235,8 @@ void EmergencyHandler::run(void)
 void EmergencyHandler::wrapFunc(FilterFunc func, std::shared_ptr<SystemStatus> const status)
 {
   std::lock_guard<std::mutex> lock(priority_mutex_);
-  priority_ = std::min(func(status), priority_);
+  latest_priority_ = func(status);
+  priority_ = std::min(latest_priority_, priority_);
   callback_time_ = ros::Time::now();
   is_system_status_received_ = true;
 }