DepthCamera and RGBDCamera - optimize RGB point cloud connection (#413)

Signed-off-by: Ian Chen <[email protected]>

src/DepthCameraSensor.cc

@@ -407,12 +407,6 @@ bool DepthCameraSensor::CreateCamera()
         std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
         std::placeholders::_4, std::placeholders::_5));
 
-  this->dataPtr->pointCloudConnection =
-      this->dataPtr->depthCamera->ConnectNewRgbPointCloud(
-      std::bind(&DepthCameraSensor::OnNewRgbPointCloud, this,
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
-        std::placeholders::_4, std::placeholders::_5));
-
   // Initialize the point message.
   // \todo(anyone) The true value in the following function call forces
   // the xyz and rgb fields to be aligned to memory boundaries. This is need
@@ -536,6 +530,19 @@ bool DepthCameraSensor::Update(
     return false;
   }
 
+  if (this->HasPointConnections() && !this->dataPtr->pointCloudConnection)
+  {
+    this->dataPtr->pointCloudConnection =
+        this->dataPtr->depthCamera->ConnectNewRgbPointCloud(
+        std::bind(&DepthCameraSensor::OnNewRgbPointCloud, this,
+          std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+          std::placeholders::_4, std::placeholders::_5));
+  }
+  else if (!this->HasPointConnections() && this->dataPtr->pointCloudConnection)
+  {
+    this->dataPtr->pointCloudConnection.reset();
+  }
+
   // generate sensor data
   this->Render();
 

src/RgbdCameraSensor.cc

@@ -375,13 +375,6 @@ bool RgbdCameraSensor::CreateCameras()
         std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
         std::placeholders::_4, std::placeholders::_5));
 
-  this->dataPtr->pointCloudConnection =
-      this->dataPtr->depthCamera->ConnectNewRgbPointCloud(
-        std::bind(&RgbdCameraSensorPrivate::OnNewRgbPointCloud,
-        this->dataPtr.get(),
-        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
-        std::placeholders::_4, std::placeholders::_5));
-
   // Set the values of the point message based on the camera information.
   this->dataPtr->pointMsg.set_width(this->ImageWidth());
   this->dataPtr->pointMsg.set_height(this->ImageHeight());
@@ -472,6 +465,22 @@ bool RgbdCameraSensor::Update(const std::chrono::steady_clock::duration &_now)
     return false;
   }
 
+  if ((this->HasPointConnections() || HasColorConnections()) &&
+      !this->dataPtr->pointCloudConnection)
+  {
+    this->dataPtr->pointCloudConnection =
+        this->dataPtr->depthCamera->ConnectNewRgbPointCloud(
+        std::bind(&RgbdCameraSensorPrivate::OnNewRgbPointCloud,
+        this->dataPtr.get(),
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+        std::placeholders::_4, std::placeholders::_5));
+  }
+  else if (!this->HasPointConnections() && !this->HasColorConnections() &&
+      this->dataPtr->pointCloudConnection)
+  {
+    this->dataPtr->pointCloudConnection.reset();
+  }
+
   unsigned int width = this->dataPtr->depthCamera->ImageWidth();
   unsigned int height = this->dataPtr->depthCamera->ImageHeight();
   unsigned int depthSamples = height * width;