Added reflectance output image for DepthCamera

gazebo/rendering/DepthCamera.cc

@@ -659,6 +659,18 @@ const float* DepthCamera::DepthData() const
   return this->dataPtr->depthBuffer;
 }
 
+//////////////////////////////////////////////////
+bool DepthCamera::GetOutputReflectance() const
+{
+  return this->dataPtr->outputReflectance;
+}
+
+//////////////////////////////////////////////////
+const float* DepthCamera::ReflectanceData() const
+{
+  return this->dataPtr->reflectanceBuffer;
+}
+
 //////////////////////////////////////////////////
 void DepthCamera::SetDepthTarget(Ogre::RenderTarget *_target)
 {

gazebo/rendering/DepthCamera.hh

@@ -92,6 +92,14 @@ namespace gazebo
       /// \return The z-buffer as a float array
       public: virtual const float *DepthData() const;
 
+      /// \brief Get whether reflectance has been generated.
+      /// \return True if reflectance is set to generate.
+      public: bool GetOutputReflectance() const;
+
+      /// \brief All things needed to get reflectance data
+      /// \return The reflectance buffer as a float array
+      public: virtual const float *ReflectanceData() const;
+
       /// \brief Set the render target, which renders the depth data
       /// \param[in] _target Pointer to the render target
       public: virtual void SetDepthTarget(Ogre::RenderTarget *_target);

gazebo/sensors/DepthCameraSensor.cc

@@ -124,6 +124,20 @@ void DepthCameraSensor::Init()
     gzerr << "No world name" << std::endl;
   }
 
+  if (this->dataPtr->depthCamera->GetOutputReflectance())
+  {
+    ignition::transport::AdvertiseMessageOptions opts;
+    opts.SetMsgsPerSec(50);
+
+    this->dataPtr->imageReflectancePub =
+        this->node->Advertise<msgs::ImageStamped>(
+            this->Topic() + "_reflectance", 50);
+
+    this->dataPtr->imageReflectancePubIgn =
+        this->nodeIgn.Advertise<ignition::msgs::Image>(
+            this->TopicIgn() + "_reflectance", opts);
+  }
+
   // Disable clouds and moon on server side until fixed and also to improve
   // performance
   this->scene->SetSkyXMode(rendering::Scene::GZ_SKYX_ALL &
@@ -188,6 +202,40 @@ bool DepthCameraSensor::UpdateImpl(const bool /*_force*/)
     this->imagePub->Publish(msg);
   }
 
+  if (this->dataPtr->imageReflectancePub &&
+      this->dataPtr->imageReflectancePub->HasConnections() &&
+      // check if reflectance data is available.
+      this->dataPtr->depthCamera->ReflectanceData())
+  {
+    msgs::ImageStamped msg;
+    msgs::Set(msg.mutable_time(), this->scene->SimTime());
+    msg.mutable_image()->set_width(this->camera->ImageWidth());
+    msg.mutable_image()->set_height(this->camera->ImageHeight());
+    msg.mutable_image()->set_pixel_format(common::Image::R_FLOAT32);
+
+
+    msg.mutable_image()->set_step(this->camera->ImageWidth() *
+        this->camera->ImageDepth());
+
+    unsigned int reflectanceSamples =
+        msg.image().width() * msg.image().height();
+
+    float f;
+    // cppchecker recommends using sizeof(varname)
+    unsigned int reflectanceBufferSize = reflectanceSamples * sizeof(f);
+
+    if (!this->dataPtr->reflectanceBuffer)
+      this->dataPtr->reflectanceBuffer = new float[reflectanceSamples];
+
+    memcpy(this->dataPtr->reflectanceBuffer,
+        this->dataPtr->depthCamera->ReflectanceData(), reflectanceBufferSize);
+
+    msg.mutable_image()->set_data(this->dataPtr->reflectanceBuffer,
+        reflectanceBufferSize);
+
+    this->dataPtr->imageReflectancePub->Publish(msg);
+  }
+
   this->SetRendered(false);
   IGN_PROFILE_END();
   return true;

gazebo/sensors/DepthCameraSensorPrivate.hh

@@ -30,8 +30,17 @@ namespace gazebo
       /// \brief Depth data buffer.
       public: float *depthBuffer = nullptr;
 
+      /// \brief Reflectance data buffer.
+      public: float *reflectanceBuffer = nullptr;
+
       /// \brief Local pointer to the depthCamera.
       public: rendering::DepthCameraPtr depthCamera;
+
+      /// \brief Publisher of reflectance image messages.
+      public: transport::PublisherPtr imageReflectancePub;
+
+      /// \brief Publisher of reflectance image messages.
+      public: ignition::transport::Node::Publisher imageReflectancePubIgn;
     };
   }
 }