diff --git a/gazebo/gui/viewers/ImageFrame.cc b/gazebo/gui/viewers/ImageFrame.cc index e7db704b05..ed37e644ee 100644 --- a/gazebo/gui/viewers/ImageFrame.cc +++ b/gazebo/gui/viewers/ImageFrame.cc @@ -72,6 +72,7 @@ void ImageFrame::OnImage(const msgs::Image &_msg) std::lock_guard lock(this->dataPtr->mutex); QImage::Format qFormat; bool isDepthImage = false; + bool isReflectanceImage = false; switch (_msg.pixel_format()) { case common::Image::PixelFormat::L_INT8: @@ -86,6 +87,12 @@ void ImageFrame::OnImage(const msgs::Image &_msg) isDepthImage = true; break; } + case common::Image::PixelFormat::UNKNOWN_PIXEL_FORMAT: + { + qFormat = QImage::Format_RGB888; + isReflectanceImage = true; + break; + } default: { qFormat = QImage::Format_RGB888; @@ -108,7 +115,7 @@ void ImageFrame::OnImage(const msgs::Image &_msg) } // Convert the image data to RGB - if (isDepthImage) + if (isDepthImage || isReflectanceImage) { unsigned int depthSamples = _msg.width() * _msg.height(); float f; @@ -118,7 +125,8 @@ void ImageFrame::OnImage(const msgs::Image &_msg) if (!this->dataPtr->depthBuffer) this->dataPtr->depthBuffer = new float[depthSamples]; - if (_msg.pixel_format() == common::Image::PixelFormat::R_FLOAT32) + if (_msg.pixel_format() == common::Image::PixelFormat::R_FLOAT32 || + _msg.pixel_format() == common::Image::PixelFormat::UNKNOWN_PIXEL_FORMAT) { memcpy(this->dataPtr->depthBuffer, _msg.data().c_str(), depthBufferSize); } @@ -159,7 +167,12 @@ void ImageFrame::OnImage(const msgs::Image &_msg) for (unsigned int i = 0; i < _msg.width(); ++i) { float d = this->dataPtr->depthBuffer[idx++]; - d = 255 - (d * factor); + if (isDepthImage) { + d = 255 - (d * factor); + } + else if (isReflectanceImage) { + d *= factor; + } QRgb value = qRgb(d, d, d); this->dataPtr->image.setPixel(i, j, value); } diff --git a/gazebo/rendering/DepthCamera.cc b/gazebo/rendering/DepthCamera.cc index ab82e14532..f17abe2228 100644 --- a/gazebo/rendering/DepthCamera.cc +++ b/gazebo/rendering/DepthCamera.cc @@ -654,6 +654,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) { diff --git a/gazebo/rendering/DepthCamera.hh b/gazebo/rendering/DepthCamera.hh index 013a920dfd..dbaf9ac440 100644 --- a/gazebo/rendering/DepthCamera.hh +++ b/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: 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); diff --git a/gazebo/sensors/DepthCameraSensor.cc b/gazebo/sensors/DepthCameraSensor.cc index a2552fb7a4..ce3d1aeb3d 100644 --- a/gazebo/sensors/DepthCameraSensor.cc +++ b/gazebo/sensors/DepthCameraSensor.cc @@ -124,6 +124,13 @@ void DepthCameraSensor::Init() gzerr << "No world name" << std::endl; } + if (this->dataPtr->depthCamera->GetOutputReflectance()) + { + this->dataPtr->imageReflectancePub = + this->node->Advertise( + this->Topic() + "_reflectance", 50); + } + // Disable clouds and moon on server side until fixed and also to improve // performance this->scene->SetSkyXMode(rendering::Scene::GZ_SKYX_ALL & @@ -188,6 +195,39 @@ 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::UNKNOWN_PIXEL_FORMAT); + + 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; diff --git a/gazebo/sensors/DepthCameraSensorPrivate.hh b/gazebo/sensors/DepthCameraSensorPrivate.hh index e5824d6990..517100d7b7 100644 --- a/gazebo/sensors/DepthCameraSensorPrivate.hh +++ b/gazebo/sensors/DepthCameraSensorPrivate.hh @@ -30,8 +30,14 @@ 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; }; } } diff --git a/gazebo/sensors/DepthCameraSensor_TEST.cc b/gazebo/sensors/DepthCameraSensor_TEST.cc index c7a8a13499..4281c3a45a 100644 --- a/gazebo/sensors/DepthCameraSensor_TEST.cc +++ b/gazebo/sensors/DepthCameraSensor_TEST.cc @@ -25,30 +25,64 @@ using namespace gazebo; class DepthCameraSensor_TEST : public ServerFixture { + public: DepthCameraSensor_TEST(); + public: void TestCreateDepthCameraSensor(); + public: void TestDepthCameraReflectance(); + public: void TestDepthCameraNormals(); + public: void TestReflectanceTopic(); + private: void OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + private: void OnNewReflectanceFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + private: void OnNewNormalsFrame(const float *_normals, + unsigned int _width, + unsigned int _height, + unsigned int _depth, + const std::string &_format); + private: void TxMsg(const ConstPropagationGridPtr &_msg); + + private: std::mutex depthMutex; + private: unsigned int depthCounter; + private: float *depthBuffer; + private: std::mutex reflectanceMutex; + private: unsigned int reflectanceCounter; + private: float *reflectanceBuffer; + private: unsigned int normalsCounter; + private: std::mutex reflectanceTopicMutex; + private: bool reflectanceTopicReceivedMsg; + private: boost::shared_ptr + reflectanceTopicGridMsg; }; -std::mutex g_depthMutex; -unsigned int g_depthCounter = 0; -float *g_depthBuffer = nullptr; +DepthCameraSensor_TEST::DepthCameraSensor_TEST() +{ + this->depthCounter = 0; + this->depthBuffer = nullptr; + this->reflectanceCounter = 0; + this->reflectanceBuffer = nullptr; + this->normalsCounter = 0; + this->reflectanceTopicReceivedMsg = false; +} ///////////////////////////////////////////////// -void OnNewDepthFrame(const float * _image, +void DepthCameraSensor_TEST::OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string & _format) + unsigned int _depth, const std::string &_format) { EXPECT_EQ(_depth, 1u); EXPECT_EQ(_format, std::string("FLOAT32")); ASSERT_NE(nullptr, _image); - std::lock_guard lock(g_depthMutex); - if (!g_depthBuffer) - g_depthBuffer = new float[_width * _height]; - memcpy(g_depthBuffer, _image, _width * _height * sizeof(_image[0])); - g_depthCounter++; + std::lock_guard lock(this->depthMutex); + if (!this->depthBuffer) + this->depthBuffer = new float[_width * _height]; + memcpy(this->depthBuffer, _image, _width * _height * sizeof(_image[0])); + this->depthCounter++; } ///////////////////////////////////////////////// -/// \brief Test Creation of a Depth Camera sensor -TEST_F(DepthCameraSensor_TEST, CreateDepthCamera) +void DepthCameraSensor_TEST::TestCreateDepthCameraSensor() { Load("worlds/depth_camera.world"); sensors::SensorManager *mgr = sensors::SensorManager::Instance(); @@ -72,30 +106,30 @@ TEST_F(DepthCameraSensor_TEST, CreateDepthCamera) ASSERT_NE(nullptr, depthCamera); event::ConnectionPtr c = depthCamera->ConnectNewDepthFrame( - std::bind(&::OnNewDepthFrame, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, - std::placeholders::_5)); + std::bind(&DepthCameraSensor_TEST::OnNewDepthFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); // wait for a few depth camera frames unsigned int framesToWait = 10; int i = 0; - while (i < 300 && g_depthCounter < framesToWait) + while (i < 300 && this->depthCounter < framesToWait) { common::Time::MSleep(20); i++; } - EXPECT_GE(g_depthCounter, framesToWait); + EXPECT_GE(this->depthCounter, framesToWait); unsigned int imageSize = sensor->ImageWidth() * sensor->ImageHeight(); - std::lock_guard lock(g_depthMutex); + std::lock_guard lock(this->depthMutex); // Check that the depth values are valid for (unsigned int i = 0; i < imageSize; ++i) { - EXPECT_TRUE(g_depthBuffer[i] <= depthCamera->FarClip()); - EXPECT_TRUE(g_depthBuffer[i] >= depthCamera->NearClip()); - EXPECT_TRUE(!ignition::math::equal(g_depthBuffer[i], 0.0f)); + EXPECT_TRUE(this->depthBuffer[i] <= depthCamera->FarClip()); + EXPECT_TRUE(this->depthBuffer[i] >= depthCamera->NearClip()); + EXPECT_TRUE(!ignition::math::equal(this->depthBuffer[i], 0.0f)); } // sphere with radius 1m is at 2m in front of depth camera @@ -105,40 +139,33 @@ TEST_F(DepthCameraSensor_TEST, CreateDepthCamera) for (unsigned int i = sensor->ImageWidth()*halfHeight; i < sensor->ImageWidth()*(halfHeight+1); ++i) { - EXPECT_TRUE(g_depthBuffer[i] < 2.0f); - EXPECT_TRUE(g_depthBuffer[i] >= 1.0f); + EXPECT_TRUE(this->depthBuffer[i] < 2.0f); + EXPECT_TRUE(this->depthBuffer[i] >= 1.0f); } - if (g_depthBuffer) - delete [] g_depthBuffer; + if (this->depthBuffer) + delete [] this->depthBuffer; } -class DepthCameraReflectanceSensor_TEST : public ServerFixture -{ -}; - -std::mutex g_reflectanceMutex; -unsigned int g_reflectanceCounter = 0; -float *g_reflectanceBuffer = nullptr; - ///////////////////////////////////////////////// -void OnNewReflectanceFrame(const float * _image, +void DepthCameraSensor_TEST::OnNewReflectanceFrame(const float *_image, unsigned int _width, unsigned int _height, - unsigned int _depth, const std::string & _format) + unsigned int _depth, const std::string &_format) { EXPECT_EQ(_depth, 1u); EXPECT_EQ(_format, std::string("REFLECTANCE")); ASSERT_NE(nullptr, _image); - std::lock_guard lock(g_reflectanceMutex); - if (!g_reflectanceBuffer) - g_reflectanceBuffer = new float[_width * _height]; - memcpy(g_reflectanceBuffer, _image, _width * _height * sizeof(_image[0])); - g_reflectanceCounter++; + std::lock_guard lock(this->reflectanceMutex); + if (!this->reflectanceBuffer) + this->reflectanceBuffer = new float[_width * _height]; + memcpy(this->reflectanceBuffer, + _image, _width * _height * sizeof(_image[0])); + this->reflectanceCounter++; } ///////////////////////////////////////////////// /// \brief Test Creation of a Depth Camera sensor -TEST_F(DepthCameraReflectanceSensor_TEST, CreateDepthCamera) +void DepthCameraSensor_TEST::TestDepthCameraReflectance() { Load("worlds/reflectance.world"); sensors::SensorManager *mgr = sensors::SensorManager::Instance(); @@ -162,21 +189,21 @@ TEST_F(DepthCameraReflectanceSensor_TEST, CreateDepthCamera) ASSERT_NE(nullptr, depthCamera); event::ConnectionPtr c = depthCamera->ConnectNewReflectanceFrame( - std::bind(&::OnNewReflectanceFrame, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, - std::placeholders::_5)); + std::bind(&DepthCameraSensor_TEST::OnNewReflectanceFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); // wait for a few depth camera frames unsigned int framesToWait = 10; int i = 0; - while (i < 300 && g_reflectanceCounter < framesToWait) + while (i < 300 && this->reflectanceCounter < framesToWait) { common::Time::MSleep(20); i++; } - EXPECT_GE(g_reflectanceCounter, framesToWait); + EXPECT_GE(this->reflectanceCounter, framesToWait); - std::lock_guard lock(g_reflectanceMutex); + std::lock_guard lock(this->reflectanceMutex); // Check the reflectance of the box unsigned int index = ((sensor->ImageHeight() * 0.45) * sensor->ImageWidth()) @@ -184,28 +211,21 @@ TEST_F(DepthCameraReflectanceSensor_TEST, CreateDepthCamera) for (unsigned int i = index - 25; i < index + 25 ; ++i) { - EXPECT_GT(g_reflectanceBuffer[i], 0); + EXPECT_GT(this->reflectanceBuffer[i], 0); } depthCamera.reset(); - if (g_reflectanceBuffer) - delete [] g_reflectanceBuffer; + if (this->reflectanceBuffer) + delete [] this->reflectanceBuffer; } -using namespace gazebo; -class DepthCameraSensor_normals_TEST : public ServerFixture -{ -}; - -unsigned int g_normalsCounter = 0; - ///////////////////////////////////////////////// -void OnNewNormalsFrame(const float * _normals, +void DepthCameraSensor_TEST::OnNewNormalsFrame(const float *_normals, unsigned int _width, unsigned int _height, unsigned int _depth, - const std::string & _format) + const std::string &_format) { EXPECT_EQ(_depth, 1u); EXPECT_EQ(_format, std::string("NORMALS")); @@ -227,12 +247,12 @@ void OnNewNormalsFrame(const float * _normals, EXPECT_NEAR(z, 0.0, 0.01); } } - g_normalsCounter++; + this->normalsCounter++; } ///////////////////////////////////////////////// /// \brief Test Creation of a Depth Camera sensor -TEST_F(DepthCameraSensor_normals_TEST, CreateDepthCamera) +void DepthCameraSensor_TEST::TestDepthCameraNormals() { Load("worlds/depth_camera2.world"); sensors::SensorManager *mgr = sensors::SensorManager::Instance(); @@ -256,23 +276,109 @@ TEST_F(DepthCameraSensor_normals_TEST, CreateDepthCamera) ASSERT_NE(nullptr, depthCamera); event::ConnectionPtr c2 = depthCamera->ConnectNewNormalsPointCloud( - std::bind(&::OnNewNormalsFrame, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, - std::placeholders::_5)); + std::bind(&DepthCameraSensor_TEST::OnNewNormalsFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); unsigned int framesToWait = 10; // wait for a few normals callbacks int i = 0; - while (i < 300 && g_normalsCounter < framesToWait) + while (i < 300 && this->normalsCounter < framesToWait) { common::Time::MSleep(20); i++; } - EXPECT_GE(g_normalsCounter, framesToWait); + EXPECT_GE(this->normalsCounter, framesToWait); depthCamera.reset(); } +///////////////////////////////////////////////// +/// \brief Callback executed for every propagation grid message received +void DepthCameraSensor_TEST::TxMsg(const ConstPropagationGridPtr &_msg) +{ + std::lock_guard lock(this->reflectanceTopicMutex); + // Just copy the message + this->reflectanceTopicGridMsg = _msg; + this->reflectanceTopicReceivedMsg = true; +} + +///////////////////////////////////////////////// +/// \brief Test the publication of the reflectance image used for visualization +void DepthCameraSensor_TEST::TestReflectanceTopic() +{ + { + Load("worlds/reflectance.world"); + + // Initialize gazebo transport layer + transport::NodePtr node(new transport::Node()); + node->Init("default"); + + std::string txTopic = + "/gazebo/default/camera_model/my_link/camera/image_reflectance"; + transport::SubscriberPtr sub = node->Subscribe(txTopic, + &DepthCameraSensor_TEST::TxMsg, this); + + // Make sure that the sensor is updated and some messages are published + for (int i = 0; i < 10; ++i) + { + common::Time::MSleep(100); + } + + std::lock_guard lock(this->reflectanceTopicMutex); + EXPECT_TRUE(this->reflectanceTopicReceivedMsg); + } + + this->reflectanceTopicReceivedMsg = false; + Unload(); + + { + Load("worlds/depth_camera.world"); + + // Initialize gazebo transport layer + transport::NodePtr node(new transport::Node()); + node->Init("default"); + + std::string txTopic = + "/gazebo/default/camera_model/my_link/camera/image_reflectance"; + transport::SubscriberPtr sub = node->Subscribe(txTopic, + &DepthCameraSensor_TEST::TxMsg, this); + + // Make sure that the sensor is updated and some messages are published + for (int i = 0; i < 10; ++i) + { + common::Time::MSleep(100); + } + + std::lock_guard lock(this->reflectanceTopicMutex); + EXPECT_FALSE(this->reflectanceTopicReceivedMsg); + } +} + +///////////////////////////////////////////////// +TEST_F(DepthCameraSensor_TEST, TestCreateDepthCameraSensor) +{ + TestCreateDepthCameraSensor(); +} + +///////////////////////////////////////////////// +TEST_F(DepthCameraSensor_TEST, TestDepthCameraReflectance) +{ + TestDepthCameraReflectance(); +} + +///////////////////////////////////////////////// +TEST_F(DepthCameraSensor_TEST, TestDepthCameraNormals) +{ + TestDepthCameraNormals(); +} + +///////////////////////////////////////////////// +TEST_F(DepthCameraSensor_TEST, TestReflectanceTopic) +{ + TestReflectanceTopic(); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) {