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 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 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 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(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(); }