From 9cd1ae55ac152c7627b0290287a437144b65fd59 Mon Sep 17 00:00:00 2001
From: Docker <docker@ubuntu.local>
Date: Thu, 5 Apr 2018 17:12:00 -0700
Subject: [PATCH] fix race condition

---
 .../gazebo_plugins/gazebo_ros_triggered_camera.h      |  2 +-
 gazebo_plugins/src/gazebo_ros_triggered_camera.cpp    |  9 ++++++---
 gazebo_plugins/test/camera/triggered_camera.cpp       | 11 ++++++-----
 3 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h
index af737db4e..b99debba8 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h
@@ -66,7 +66,7 @@ namespace gazebo
 
     protected: void PreRender();
 
-    protected: bool triggered = false;
+    protected: int triggered = 0;
 
     protected: std::mutex mutex;
 
diff --git a/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp b/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
index 045d8a5fa..7ea48d073 100644
--- a/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
+++ b/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
@@ -97,8 +97,9 @@ void GazeboRosTriggeredCamera::OnNewFrame(const unsigned char *_image,
     this->PublishCameraInfo();
   }
   this->SetCameraEnabled(false);
+
   std::lock_guard<std::mutex> lock(this->mutex);
-  this->triggered = false;
+  this->triggered = std::max(this->triggered-1, 0);
 }
 
 void GazeboRosTriggeredCamera::TriggerCamera()
@@ -106,7 +107,7 @@ void GazeboRosTriggeredCamera::TriggerCamera()
   std::lock_guard<std::mutex> lock(this->mutex);
   if (!this->parentSensor_)
     return;
-  this->triggered = true;
+  this->triggered++;
 }
 
 bool GazeboRosTriggeredCamera::CanTriggerCamera()
@@ -117,8 +118,10 @@ bool GazeboRosTriggeredCamera::CanTriggerCamera()
 void GazeboRosTriggeredCamera::PreRender()
 {
   std::lock_guard<std::mutex> lock(this->mutex);
-  if (this->triggered)
+  if (this->triggered > 0)
+  {
     this->SetCameraEnabled(true);
+  }
 }
 
 void GazeboRosTriggeredCamera::SetCameraEnabled(const bool _enabled)
diff --git a/gazebo_plugins/test/camera/triggered_camera.cpp b/gazebo_plugins/test/camera/triggered_camera.cpp
index 4ce6f628a..1c4323858 100644
--- a/gazebo_plugins/test/camera/triggered_camera.cpp
+++ b/gazebo_plugins/test/camera/triggered_camera.cpp
@@ -28,7 +28,7 @@ class CameraTest : public testing::Test
 TEST_F(CameraTest, cameraSubscribeTest)
 {
   image_transport::ImageTransport it(nh_);
-  cam_sub_ = it.subscribe("camera1/image_raw", 1,
+  cam_sub_ = it.subscribe("camera1/image_raw", 5,
                           &CameraTest::imageCallback,
                           dynamic_cast<CameraTest*>(this));
 
@@ -80,19 +80,20 @@ TEST_F(CameraTest, cameraSubscribeTest)
   }
   EXPECT_EQ(images_received_, 1);
 
-  // then send two trigger messages very close together, but only expect one image
+  // then send two trigger messages very close together, and expect two more
+  // images
   trigger_pub.publish(msg);
   ros::spinOnce();
   ros::Duration(0.01).sleep();
   trigger_pub.publish(msg);
   ros::spinOnce();
   ros::Duration(0.01).sleep();
-  for (unsigned int i = 0; i < 10 && 1 == images_received_; ++i)
+  for (unsigned int i = 0; i < 10 && images_received_ < 2; ++i)
   {
     ros::spinOnce();
     ros::Duration(0.1).sleep();
   }
-  EXPECT_EQ(images_received_, 2);
+  EXPECT_EQ(images_received_, 3);
 
   // then wait for 3 seconds to confirm that we don't receive any more images
   for (unsigned int i = 0; i < 30; ++i)
@@ -100,7 +101,7 @@ TEST_F(CameraTest, cameraSubscribeTest)
     ros::spinOnce();
     ros::Duration(0.1).sleep();
   }
-  EXPECT_EQ(images_received_, 2);
+  EXPECT_EQ(images_received_, 3);
 
   cam_sub_.shutdown();
 }