Skip to content

Commit

Permalink
Branch to Support GigE Cameras (#79)
Browse files Browse the repository at this point in the history 
* Changes required to use GigE Blackfly S version

* Update SpinnakerCamera.cpp
  • Loading branch information
@luis-camero
luis-camero authored Oct 29, 2021
1 parent faf7cd6 commit da37069
Show file tree
Hide file tree
Showing 3 changed files with 92 additions and 83 deletions.
2 changes: 2 additions & 0 deletions spinnaker_camera_driver/launch/camera.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<arg name="camera_name" default="camera" />
<arg name="camera_serial" default="0" />
<arg name="calibrated" default="0" />
<arg name="device_type" default="GigE" /> <!-- USB3 or GigE -->

<!-- When unspecified, the driver will use the default framerate as given by the
camera itself. Use the parameter 'control_frame_rate' to enable manual frame
Expand All @@ -46,6 +47,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI

<param name="frame_id" value="camera" />
<param name="serial" value="$(arg camera_serial)" />
<param name="device_type" value="$(arg device_type)" />

<!-- Frame rate -->
<param name="acquisition_frame_rate_enable" value="$(arg control_frame_rate)" />
Expand Down
166 changes: 84 additions & 82 deletions spinnaker_camera_driver/src/SpinnakerCamera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,109 +334,111 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr
// std::string format(image_ptr->GetPixelFormatName());
// std::printf("\033[100m format: %s \n", format.c_str());

if (image_ptr->IsIncomplete())
//throw std::runtime_error("[SpinnakerCamera::grabImage] Image received from camera " + std::to_string(serial_) +
// " is incomplete.");
while (image_ptr->IsIncomplete())
{
throw std::runtime_error("[SpinnakerCamera::grabImage] Image received from camera " + std::to_string(serial_) +
" is incomplete.");
ROS_WARN_STREAM_ONCE("[SpinnakerCamera::grabImage] Image received from camera " << std::to_string(serial_) << " is incomplete. Trying again.");
image_ptr = pCam_->GetNextImage(timeout_);
}
else


// Set Image Time Stamp
image->header.stamp.sec = image_ptr->GetTimeStamp() * 1e-9;
image->header.stamp.nsec = image_ptr->GetTimeStamp();

// Check the bits per pixel.
size_t bitsPerPixel = image_ptr->GetBitsPerPixel();

// --------------------------------------------------
// Set the image encoding
std::string imageEncoding = sensor_msgs::image_encodings::MONO8;

Spinnaker::GenApi::CEnumerationPtr color_filter_ptr =
static_cast<Spinnaker::GenApi::CEnumerationPtr>(node_map_->GetNode("PixelColorFilter"));

Spinnaker::GenICam::gcstring color_filter_str = color_filter_ptr->ToString();
Spinnaker::GenICam::gcstring bayer_rg_str = "BayerRG";
Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR";
Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB";
Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG";

// if(isColor_ && bayer_format != NONE)
if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None"))
{
// Set Image Time Stamp
image->header.stamp.sec = image_ptr->GetTimeStamp() * 1e-9;
image->header.stamp.nsec = image_ptr->GetTimeStamp();

// Check the bits per pixel.
size_t bitsPerPixel = image_ptr->GetBitsPerPixel();

// --------------------------------------------------
// Set the image encoding
std::string imageEncoding = sensor_msgs::image_encodings::MONO8;

Spinnaker::GenApi::CEnumerationPtr color_filter_ptr =
static_cast<Spinnaker::GenApi::CEnumerationPtr>(node_map_->GetNode("PixelColorFilter"));

Spinnaker::GenICam::gcstring color_filter_str = color_filter_ptr->ToString();
Spinnaker::GenICam::gcstring bayer_rg_str = "BayerRG";
Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR";
Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB";
Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG";

// if(isColor_ && bayer_format != NONE)
if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None"))
if (bitsPerPixel == 16)
{
if (bitsPerPixel == 16)
// 16 Bits per Pixel
if (color_filter_str.compare(bayer_rg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16;
}
else if (color_filter_str.compare(bayer_gr_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16;
}
else if (color_filter_str.compare(bayer_gb_str) == 0)
{
// 16 Bits per Pixel
if (color_filter_str.compare(bayer_rg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16;
}
else if (color_filter_str.compare(bayer_gr_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16;
}
else if (color_filter_str.compare(bayer_gb_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16;
}
else if (color_filter_str.compare(bayer_bg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16;
}
else
{
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 16-bit format.");
}
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16;
}
else if (color_filter_str.compare(bayer_bg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16;
}
else
{
// 8 Bits per Pixel
if (color_filter_str.compare(bayer_rg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
}
else if (color_filter_str.compare(bayer_gr_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
}
else if (color_filter_str.compare(bayer_gb_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
}
else if (color_filter_str.compare(bayer_bg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
}
else
{
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 8-bit format.");
}
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 16-bit format.");
}
}
else // Mono camera or in pixel binned mode.
else
{
if (bitsPerPixel == 16)
// 8 Bits per Pixel
if (color_filter_str.compare(bayer_rg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::MONO16;
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
}
else if (bitsPerPixel == 24)
else if (color_filter_str.compare(bayer_gr_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::RGB8;
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
}
else if (color_filter_str.compare(bayer_gb_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
}
else if (color_filter_str.compare(bayer_bg_str) == 0)
{
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
}
else
{
imageEncoding = sensor_msgs::image_encodings::MONO8;
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 8-bit format.");
}
}
}
else // Mono camera or in pixel binned mode.
{
if (bitsPerPixel == 16)
{
imageEncoding = sensor_msgs::image_encodings::MONO16;
}
else if (bitsPerPixel == 24)
{
imageEncoding = sensor_msgs::image_encodings::RGB8;
}
else
{
imageEncoding = sensor_msgs::image_encodings::MONO8;
}
}

int width = image_ptr->GetWidth();
int height = image_ptr->GetHeight();
int stride = image_ptr->GetStride();
int width = image_ptr->GetWidth();
int height = image_ptr->GetHeight();
int stride = image_ptr->GetStride();

// ROS_INFO_ONCE("\033[93m wxh: (%d, %d), stride: %d \n", width, height, stride);
fillImage(*image, imageEncoding, height, width, stride, image_ptr->GetData());
image->header.frame_id = frame_id;
} // end else
// ROS_INFO_ONCE("\033[93m wxh: (%d, %d), stride: %d \n", width, height, stride);
fillImage(*image, imageEncoding, height, width, stride, image_ptr->GetData());
image->header.frame_id = frame_id;

}
catch (const Spinnaker::Exception& e)
{
Expand Down
7 changes: 6 additions & 1 deletion spinnaker_camera_driver/src/nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,8 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet

// Set up a diagnosed publisher
double desired_freq;
std::string device_type;
pnh.param<std::string>("device_type", device_type, "USB3");
pnh.param<double>("desired_freq", desired_freq, 30.0);
pnh.param<double>("min_freq", min_freq_, desired_freq);
pnh.param<double>("max_freq", max_freq_, desired_freq);
Expand Down Expand Up @@ -374,7 +376,10 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet
diag_man->addDiagnostic("PowerSupplyVoltage", true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f);
diag_man->addDiagnostic("PowerSupplyCurrent", true, std::make_pair(0.4f, 0.6f), 0.3f, 1.0f);
diag_man->addDiagnostic<int>("DeviceUptime");
diag_man->addDiagnostic<int>("U3VMessageChannelID");
if( device_type.compare("USB3") == 0 )
{
diag_man->addDiagnostic<int>("U3VMessageChannelID");
}
}

/**
Expand Down

0 comments on commit da37069

Please sign in to comment.