Fix race condition in gimbal controller

PX4 · Apr 3, 2020 · a928906 · a928906
1 parent e4f6bff
commit a928906
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
diff --git a/include/gazebo_gimbal_controller_plugin.hh b/include/gazebo_gimbal_controller_plugin.hh
@@ -23,6 +23,7 @@
 
 #include <string>
 #include <vector>
+#include <mutex>
 
 #include <gazebo/common/PID.hh>
 #include <gazebo/common/Plugin.hh>
@@ -91,6 +92,7 @@ namespace gazebo
     private: void OnRollStringMsg(ConstGzStringPtr &_msg);
     private: void OnYawStringMsg(ConstGzStringPtr &_msg);
 #endif
+    private: std::mutex cmd_mutex;
 
     private: sdf::ElementPtr sdf;
 

diff --git a/src/gazebo_gimbal_controller_plugin.cpp b/src/gazebo_gimbal_controller_plugin.cpp
@@ -401,6 +401,7 @@ void GimbalControllerPlugin::Init()
 
 void GimbalControllerPlugin::ImuCallback(ImuPtr& imu_message)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
   this->lastImuYaw = ignition::math::Quaterniond(imu_message->orientation().w(),
 						 imu_message->orientation().x(),
 						 imu_message->orientation().y(),
@@ -412,41 +413,47 @@ void GimbalControllerPlugin::ImuCallback(ImuPtr& imu_message)
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnPitchStringMsg(ConstAnyPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "pitch command received " << _msg->double_value() << std::endl;
   this->pitchCommand = _msg->double_value();
 }
 
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnRollStringMsg(ConstAnyPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "roll command received " << _msg->double_value() << std::endl;
   this->rollCommand = _msg->double_value();
 }
 
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnYawStringMsg(ConstAnyPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "yaw command received " << _msg->double_value() << std::endl;
   this->yawCommand = _msg->double_value();
 }
 #else
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnPitchStringMsg(ConstGzStringPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "pitch command received " << _msg->data() << std::endl;
   this->pitchCommand = atof(_msg->data().c_str());
 }
 
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnRollStringMsg(ConstGzStringPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "roll command received " << _msg->data() << std::endl;
   this->rollCommand = atof(_msg->data().c_str());
 }
 
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnYawStringMsg(ConstGzStringPtr &_msg)
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
 //  gzdbg << "yaw command received " << _msg->data() << std::endl;
   this->yawCommand = atof(_msg->data().c_str());
 }
@@ -455,6 +462,8 @@ void GimbalControllerPlugin::OnYawStringMsg(ConstGzStringPtr &_msg)
 /////////////////////////////////////////////////
 void GimbalControllerPlugin::OnUpdate()
 {
+  const std::lock_guard<std::mutex> lock(cmd_mutex);
+
   if (!this->pitchJoint || !this->rollJoint || !this->yawJoint)
     return;