From 23d995b5d292b689f23e7096452817f395e41601 Mon Sep 17 00:00:00 2001
From: Steven Peters <scpeters@openrobotics.org>
Date: Fri, 23 Feb 2018 01:53:32 -0800
Subject: [PATCH] subscribe to trigger if CanTrigger is true

---
 .../gazebo_plugins/gazebo_ros_camera_utils.h  |  3 +++
 .../gazebo_ros_triggered_camera.h             |  2 ++
 .../src/gazebo_ros_camera_utils.cpp           | 20 +++++++++++++------
 .../src/gazebo_ros_triggered_camera.cpp       |  5 +++++
 4 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
index 388d3cfbf..2125f40b2 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
@@ -210,7 +210,10 @@ namespace gazebo
     private: event::EventT<void()> load_event_;
 
     // make a trigger function that the child classes can override
+    // and a function that returns bool to indicate whether the trigger
+    // should be used
     protected: virtual void TriggerCamera();
+    protected: virtual bool CanTriggerCamera();
     private: void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy);
     private: ros::Subscriber trigger_subscriber_;
 
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 911974ee9..af737db4e 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h
@@ -58,6 +58,8 @@ namespace gazebo
 
     protected: virtual void TriggerCamera();
 
+    protected: virtual bool CanTriggerCamera();
+
     protected: event::ConnectionPtr preRenderConnection_;
 
     public: void SetCameraEnabled(const bool _enabled);
diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp
index b594a67e4..ae744b41d 100644
--- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp
+++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp
@@ -350,12 +350,15 @@ void GazeboRosCameraUtils::LoadThread()
   this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so);
   */
 
-  ros::SubscribeOptions trigger_so =
-    ros::SubscribeOptions::create<std_msgs::Empty>(
-        this->trigger_topic_name_, 1,
-        boost::bind(&GazeboRosCameraUtils::TriggerCameraInternal, this, _1),
-        ros::VoidPtr(), &this->camera_queue_);
-  this->trigger_subscriber_ = this->rosnode_->subscribe(trigger_so);
+  if (this->CanTriggerCamera())
+  {
+    ros::SubscribeOptions trigger_so =
+      ros::SubscribeOptions::create<std_msgs::Empty>(
+          this->trigger_topic_name_, 1,
+          boost::bind(&GazeboRosCameraUtils::TriggerCameraInternal, this, _1),
+          ros::VoidPtr(), &this->camera_queue_);
+    this->trigger_subscriber_ = this->rosnode_->subscribe(trigger_so);
+  }
 
   this->Init();
 }
@@ -364,6 +367,11 @@ void GazeboRosCameraUtils::TriggerCamera()
 {
 }
 
+bool GazeboRosCameraUtils::CanTriggerCamera()
+{
+  return false;
+}
+
 void GazeboRosCameraUtils::TriggerCameraInternal(
     const std_msgs::Empty::ConstPtr &dummy)
 {
diff --git a/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp b/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
index 9f167df48..f8030d059 100644
--- a/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
+++ b/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp
@@ -107,6 +107,11 @@ void GazeboRosTriggeredCamera::TriggerCamera()
   this->triggered = true;
 }
 
+bool GazeboRosTriggeredCamera::CanTriggerCamera()
+{
+  return true;
+}
+
 void GazeboRosTriggeredCamera::PreRender()
 {
   std::lock_guard<std::mutex> lock(this->mutex);