fix race condition

ros-simulation · Apr 19, 2018 · 9cd1ae5 · 9cd1ae5
1 parent e9d72ba
commit 9cd1ae5
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 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
@@ -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
@@ -97,16 +97,17 @@ 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()
 {
   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
@@ -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,27 +80,28 @@ 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)
   {
     ros::spinOnce();
     ros::Duration(0.1).sleep();
   }
-  EXPECT_EQ(images_received_, 2);
+  EXPECT_EQ(images_received_, 3);
 
   cam_sub_.shutdown();
 }