Skip to content

Commit

Permalink
Merge branch 'gz-sensors7' into ahcorde/7/projection_matrix
Browse files Browse the repository at this point in the history
  • Loading branch information
@iche033
iche033 authored Feb 8, 2023
2 parents 918cc7f + d17d535 commit 45c1761
Show file tree
Hide file tree
Showing 10 changed files with 212 additions and 94 deletions.
10 changes: 10 additions & 0 deletions include/gz/sensors/CameraSensor.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,16 @@ namespace gz
// Documentation inherited.
public: virtual bool HasConnections() const override;

/// \brief Check if there are any image subscribers
/// \return True if there are image subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasImageConnections() const;

/// \brief Check if there are any info subscribers
/// \return True if there are info subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasInfoConnections() const;

/// \brief Advertise camera info topic.
/// \return True if successful.
protected: bool AdvertiseInfo();
Expand Down
10 changes: 10 additions & 0 deletions include/gz/sensors/DepthCameraSensor.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,16 @@ namespace gz
// Documentation inherited.
public: virtual bool HasConnections() const override;

/// \brief Check if there are any depth subscribers
/// \return True if there are subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasDepthConnections() const;

/// \brief Check if there are any point subscribers
/// \return True if there are subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasPointConnections() const;

/// \brief Create a camera in a scene
/// \return True on success.
private: bool CreateCamera();
Expand Down
15 changes: 15 additions & 0 deletions include/gz/sensors/RgbdCameraSensor.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,21 @@ namespace gz
// Documentation inherited.
public: virtual bool HasConnections() const override;

/// \brief Check if there are color subscribers
/// \return True if there are subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasColorConnections() const;

/// \brief Check if there are depth subscribers
/// \return True if there are subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasDepthConnections() const;

/// \brief Check if there are point cloud subscribers
/// \return True if there are subscribers, false otherwise
/// \todo(iche033) Make this function virtual on Harmonic
public: bool HasPointConnections() const;

/// \brief Create an RGB camera and a depth camera.
/// \return True on success.
private: bool CreateCameras();
Expand Down
10 changes: 8 additions & 2 deletions src/BoundingBoxCameraSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -384,6 +384,12 @@ bool BoundingBoxCameraSensor::Update(
return false;
}

if (this->HasInfoConnections())
{
// publish the camera info message
this->PublishInfo(_now);
}

// don't render if there are no subscribers nor saving
if (!this->dataPtr->imagePublisher.HasConnections() &&
!this->dataPtr->boxesPublisher.HasConnections() &&
Expand Down Expand Up @@ -503,7 +509,6 @@ bool BoundingBoxCameraSensor::Update(
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);

// Publish
this->PublishInfo(_now);
if (this->dataPtr->type == rendering::BoundingBoxType::BBT_BOX3D)
{
this->AddSequence(boxes3DMsg.mutable_header(), "boundingboxes");
Expand Down Expand Up @@ -673,5 +678,6 @@ bool BoundingBoxCameraSensor::HasConnections() const
return (this->dataPtr->imagePublisher &&
this->dataPtr->imagePublisher.HasConnections()) ||
(this->dataPtr->boxesPublisher &&
this->dataPtr->boxesPublisher.HasConnections());
this->dataPtr->boxesPublisher.HasConnections()) ||
this->HasInfoConnections();
}
156 changes: 87 additions & 69 deletions src/CameraSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -513,6 +513,12 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
// move the camera to the current pose
this->dataPtr->camera->SetLocalPose(this->Pose());

if (this->HasInfoConnections())
{
// publish the camera info message
this->PublishInfo(_now);
}

// render only if necessary
if (this->dataPtr->isTriggeredCamera &&
!this->dataPtr->isTriggered)
Expand Down Expand Up @@ -543,85 +549,85 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now)
}
}

// generate sensor data
this->Render();
if (this->HasImageConnections() || this->dataPtr->saveImage)
{
GZ_PROFILE("CameraSensor::Update Copy image");
this->dataPtr->camera->Copy(this->dataPtr->image);
}
// generate sensor data
this->Render();
{
GZ_PROFILE("CameraSensor::Update Copy image");
this->dataPtr->camera->Copy(this->dataPtr->image);
}

unsigned int width = this->dataPtr->camera->ImageWidth();
unsigned int height = this->dataPtr->camera->ImageHeight();
unsigned char *data = this->dataPtr->image.Data<unsigned char>();
unsigned int width = this->dataPtr->camera->ImageWidth();
unsigned int height = this->dataPtr->camera->ImageHeight();
unsigned char *data = this->dataPtr->image.Data<unsigned char>();

gz::common::Image::PixelFormatType
format{common::Image::UNKNOWN_PIXEL_FORMAT};
msgs::PixelFormatType msgsPixelFormat =
msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT;
gz::common::Image::PixelFormatType
format{common::Image::UNKNOWN_PIXEL_FORMAT};
msgs::PixelFormatType msgsPixelFormat =
msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT;

switch (this->dataPtr->camera->ImageFormat())
{
case rendering::PF_R8G8B8:
format = common::Image::RGB_INT8;
msgsPixelFormat = msgs::PixelFormatType::RGB_INT8;
break;
case rendering::PF_L8:
format = common::Image::L_INT8;
msgsPixelFormat = msgs::PixelFormatType::L_INT8;
break;
case rendering::PF_L16:
format = common::Image::L_INT16;
msgsPixelFormat = msgs::PixelFormatType::L_INT16;
break;
default:
gzerr << "Unsupported pixel format ["
<< this->dataPtr->camera->ImageFormat() << "]\n";
break;
}

// create message
msgs::Image msg;
{
GZ_PROFILE("CameraSensor::Update Message");
msg.set_width(width);
msg.set_height(height);
msg.set_step(width * rendering::PixelUtil::BytesPerPixel(
this->dataPtr->camera->ImageFormat()));
msg.set_pixel_format_type(msgsPixelFormat);
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
auto frame = msg.mutable_header()->add_data();
frame->set_key("frame_id");
frame->add_value(this->dataPtr->opticalFrameId);
msg.set_data(data, this->dataPtr->camera->ImageMemorySize());
}

// publish the image message
{
this->AddSequence(msg.mutable_header());
GZ_PROFILE("CameraSensor::Update Publish");
this->dataPtr->pub.Publish(msg);
switch (this->dataPtr->camera->ImageFormat())
{
case rendering::PF_R8G8B8:
format = common::Image::RGB_INT8;
msgsPixelFormat = msgs::PixelFormatType::RGB_INT8;
break;
case rendering::PF_L8:
format = common::Image::L_INT8;
msgsPixelFormat = msgs::PixelFormatType::L_INT8;
break;
case rendering::PF_L16:
format = common::Image::L_INT16;
msgsPixelFormat = msgs::PixelFormatType::L_INT16;
break;
default:
gzerr << "Unsupported pixel format ["
<< this->dataPtr->camera->ImageFormat() << "]\n";
break;
}

// publish the camera info message
this->PublishInfo(_now);
}
// create message
msgs::Image msg;
{
GZ_PROFILE("CameraSensor::Update Message");
msg.set_width(width);
msg.set_height(height);
msg.set_step(width * rendering::PixelUtil::BytesPerPixel(
this->dataPtr->camera->ImageFormat()));
msg.set_pixel_format_type(msgsPixelFormat);
*msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
auto frame = msg.mutable_header()->add_data();
frame->set_key("frame_id");
frame->add_value(this->dataPtr->opticalFrameId);
msg.set_data(data, this->dataPtr->camera->ImageMemorySize());
}

// Trigger callbacks.
if (this->dataPtr->imageEvent.ConnectionCount() > 0)
{
try
// publish the image message
{
this->dataPtr->imageEvent(msg);
this->AddSequence(msg.mutable_header());
GZ_PROFILE("CameraSensor::Update Publish");
this->dataPtr->pub.Publish(msg);
}
catch(...)

// Trigger callbacks.
if (this->dataPtr->imageEvent.ConnectionCount() > 0)
{
gzerr << "Exception thrown in an image callback.\n";
try
{
this->dataPtr->imageEvent(msg);
}
catch(...)
{
ignerr << "Exception thrown in an image callback.\n";
}
}
}

// Save image
if (this->dataPtr->saveImage)
{
this->dataPtr->SaveImage(data, width, height, format);
// Save image
if (this->dataPtr->saveImage)
{
this->dataPtr->SaveImage(data, width, height, format);
}
}

if (this->dataPtr->isTriggeredCamera)
Expand Down Expand Up @@ -845,9 +851,21 @@ double CameraSensor::Baseline() const

//////////////////////////////////////////////////
bool CameraSensor::HasConnections() const
{
return this->HasImageConnections() || this->HasInfoConnections();
}

//////////////////////////////////////////////////
bool CameraSensor::HasImageConnections() const
{
return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) ||
this->dataPtr->imageEvent.ConnectionCount() > 0u;
this->dataPtr->imageEvent.ConnectionCount() > 0u;
}

//////////////////////////////////////////////////
bool CameraSensor::HasInfoConnections() const
{
return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections();
}

//////////////////////////////////////////////////
Expand Down
33 changes: 27 additions & 6 deletions src/DepthCameraSensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -522,6 +522,17 @@ bool DepthCameraSensor::Update(
return false;
}

if (this->HasInfoConnections())
{
// publish the camera info message
this->PublishInfo(_now);
}

if (!this->HasDepthConnections() && !this->HasPointConnections())
{
return false;
}

// generate sensor data
this->Render();

Expand Down Expand Up @@ -550,8 +561,6 @@ bool DepthCameraSensor::Update(
this->AddSequence(msg.mutable_header(), "default");
this->dataPtr->pub.Publish(msg);

// publish the camera info message
this->PublishInfo(_now);

if (this->dataPtr->imageEvent.ConnectionCount() > 0u)
{
Expand All @@ -566,7 +575,7 @@ bool DepthCameraSensor::Update(
}
}

if (this->dataPtr->pointPub.HasConnections() &&
if (this->HasPointConnections() &&
this->dataPtr->pointCloudBuffer)
{
// Set the time stamp
Expand Down Expand Up @@ -632,7 +641,19 @@ double DepthCameraSensor::NearClip() const
//////////////////////////////////////////////////
bool DepthCameraSensor::HasConnections() const
{
return (this->dataPtr->pub && this->dataPtr->pub.HasConnections()) ||
(this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections()) ||
this->dataPtr->imageEvent.ConnectionCount() > 0u;
return this->HasDepthConnections() || this->HasPointConnections() ||
this->HasInfoConnections();
}

//////////////////////////////////////////////////
bool DepthCameraSensor::HasDepthConnections() const
{
return (this->dataPtr->pub && this->dataPtr->pub.HasConnections())
|| this->dataPtr->imageEvent.ConnectionCount() > 0u;
}

//////////////////////////////////////////////////
bool DepthCameraSensor::HasPointConnections() const
{
return this->dataPtr->pointPub && this->dataPtr->pointPub.HasConnections();
}
Loading

0 comments on commit 45c1761

Please sign in to comment.