Skip to content

Commit da37069

Browse files
luis-cameroluis-camero
authored
Branch to Support GigE Cameras (#79)
* Changes required to use GigE Blackfly S version * Update SpinnakerCamera.cpp
1 parent faf7cd6 commit da37069

File tree

3 files changed

+92
-83
lines changed

3 files changed

+92
-83
lines changed

‎spinnaker_camera_driver/launch/camera.launch‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
2929
<arg name="camera_name" default="camera" />
3030
<arg name="camera_serial" default="0" />
3131
<arg name="calibrated" default="0" />
32+
<arg name="device_type" default="GigE" /> <!-- USB3 or GigE -->
3233

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

4748
<param name="frame_id" value="camera" />
4849
<param name="serial" value="$(arg camera_serial)" />
50+
<param name="device_type" value="$(arg device_type)" />
4951

5052
<!-- Frame rate -->
5153
<param name="acquisition_frame_rate_enable" value="$(arg control_frame_rate)" />

‎spinnaker_camera_driver/src/SpinnakerCamera.cpp‎

Lines changed: 84 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -334,109 +334,111 @@ void SpinnakerCamera::grabImage(sensor_msgs::Image* image, const std::string& fr
334334
// std::string format(image_ptr->GetPixelFormatName());
335335
// std::printf("\033[100m format: %s \n", format.c_str());
336336

337-
if (image_ptr->IsIncomplete())
337+
//throw std::runtime_error("[SpinnakerCamera::grabImage] Image received from camera " + std::to_string(serial_) +
338+
// " is incomplete.");
339+
while (image_ptr->IsIncomplete())
338340
{
339-
throw std::runtime_error("[SpinnakerCamera::grabImage] Image received from camera " + std::to_string(serial_) +
340-
" is incomplete.");
341+
ROS_WARN_STREAM_ONCE("[SpinnakerCamera::grabImage] Image received from camera " << std::to_string(serial_) << " is incomplete. Trying again.");
342+
image_ptr = pCam_->GetNextImage(timeout_);
341343
}
342-
else
344+
345+
346+
// Set Image Time Stamp
347+
image->header.stamp.sec = image_ptr->GetTimeStamp() * 1e-9;
348+
image->header.stamp.nsec = image_ptr->GetTimeStamp();
349+
350+
// Check the bits per pixel.
351+
size_t bitsPerPixel = image_ptr->GetBitsPerPixel();
352+
353+
// --------------------------------------------------
354+
// Set the image encoding
355+
std::string imageEncoding = sensor_msgs::image_encodings::MONO8;
356+
357+
Spinnaker::GenApi::CEnumerationPtr color_filter_ptr =
358+
static_cast<Spinnaker::GenApi::CEnumerationPtr>(node_map_->GetNode("PixelColorFilter"));
359+
360+
Spinnaker::GenICam::gcstring color_filter_str = color_filter_ptr->ToString();
361+
Spinnaker::GenICam::gcstring bayer_rg_str = "BayerRG";
362+
Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR";
363+
Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB";
364+
Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG";
365+
366+
// if(isColor_ && bayer_format != NONE)
367+
if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None"))
343368
{
344-
// Set Image Time Stamp
345-
image->header.stamp.sec = image_ptr->GetTimeStamp() * 1e-9;
346-
image->header.stamp.nsec = image_ptr->GetTimeStamp();
347-
348-
// Check the bits per pixel.
349-
size_t bitsPerPixel = image_ptr->GetBitsPerPixel();
350-
351-
// --------------------------------------------------
352-
// Set the image encoding
353-
std::string imageEncoding = sensor_msgs::image_encodings::MONO8;
354-
355-
Spinnaker::GenApi::CEnumerationPtr color_filter_ptr =
356-
static_cast<Spinnaker::GenApi::CEnumerationPtr>(node_map_->GetNode("PixelColorFilter"));
357-
358-
Spinnaker::GenICam::gcstring color_filter_str = color_filter_ptr->ToString();
359-
Spinnaker::GenICam::gcstring bayer_rg_str = "BayerRG";
360-
Spinnaker::GenICam::gcstring bayer_gr_str = "BayerGR";
361-
Spinnaker::GenICam::gcstring bayer_gb_str = "BayerGB";
362-
Spinnaker::GenICam::gcstring bayer_bg_str = "BayerBG";
363-
364-
// if(isColor_ && bayer_format != NONE)
365-
if (color_filter_ptr->GetCurrentEntry() != color_filter_ptr->GetEntryByName("None"))
369+
if (bitsPerPixel == 16)
366370
{
367-
if (bitsPerPixel == 16)
371+
// 16 Bits per Pixel
372+
if (color_filter_str.compare(bayer_rg_str) == 0)
373+
{
374+
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16;
375+
}
376+
else if (color_filter_str.compare(bayer_gr_str) == 0)
377+
{
378+
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16;
379+
}
380+
else if (color_filter_str.compare(bayer_gb_str) == 0)
368381
{
369-
// 16 Bits per Pixel
370-
if (color_filter_str.compare(bayer_rg_str) == 0)
371-
{
372-
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB16;
373-
}
374-
else if (color_filter_str.compare(bayer_gr_str) == 0)
375-
{
376-
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG16;
377-
}
378-
else if (color_filter_str.compare(bayer_gb_str) == 0)
379-
{
380-
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16;
381-
}
382-
else if (color_filter_str.compare(bayer_bg_str) == 0)
383-
{
384-
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16;
385-
}
386-
else
387-
{
388-
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 16-bit format.");
389-
}
382+
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG16;
383+
}
384+
else if (color_filter_str.compare(bayer_bg_str) == 0)
385+
{
386+
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR16;
390387
}
391388
else
392389
{
393-
// 8 Bits per Pixel
394-
if (color_filter_str.compare(bayer_rg_str) == 0)
395-
{
396-
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
397-
}
398-
else if (color_filter_str.compare(bayer_gr_str) == 0)
399-
{
400-
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
401-
}
402-
else if (color_filter_str.compare(bayer_gb_str) == 0)
403-
{
404-
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
405-
}
406-
else if (color_filter_str.compare(bayer_bg_str) == 0)
407-
{
408-
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
409-
}
410-
else
411-
{
412-
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 8-bit format.");
413-
}
390+
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 16-bit format.");
414391
}
415392
}
416-
else // Mono camera or in pixel binned mode.
393+
else
417394
{
418-
if (bitsPerPixel == 16)
395+
// 8 Bits per Pixel
396+
if (color_filter_str.compare(bayer_rg_str) == 0)
419397
{
420-
imageEncoding = sensor_msgs::image_encodings::MONO16;
398+
imageEncoding = sensor_msgs::image_encodings::BAYER_RGGB8;
421399
}
422-
else if (bitsPerPixel == 24)
400+
else if (color_filter_str.compare(bayer_gr_str) == 0)
423401
{
424-
imageEncoding = sensor_msgs::image_encodings::RGB8;
402+
imageEncoding = sensor_msgs::image_encodings::BAYER_GRBG8;
403+
}
404+
else if (color_filter_str.compare(bayer_gb_str) == 0)
405+
{
406+
imageEncoding = sensor_msgs::image_encodings::BAYER_GBRG8;
407+
}
408+
else if (color_filter_str.compare(bayer_bg_str) == 0)
409+
{
410+
imageEncoding = sensor_msgs::image_encodings::BAYER_BGGR8;
425411
}
426412
else
427413
{
428-
imageEncoding = sensor_msgs::image_encodings::MONO8;
414+
throw std::runtime_error("[SpinnakerCamera::grabImage] Bayer format not recognized for 8-bit format.");
429415
}
430416
}
417+
}
418+
else // Mono camera or in pixel binned mode.
419+
{
420+
if (bitsPerPixel == 16)
421+
{
422+
imageEncoding = sensor_msgs::image_encodings::MONO16;
423+
}
424+
else if (bitsPerPixel == 24)
425+
{
426+
imageEncoding = sensor_msgs::image_encodings::RGB8;
427+
}
428+
else
429+
{
430+
imageEncoding = sensor_msgs::image_encodings::MONO8;
431+
}
432+
}
431433

432-
int width = image_ptr->GetWidth();
433-
int height = image_ptr->GetHeight();
434-
int stride = image_ptr->GetStride();
434+
int width = image_ptr->GetWidth();
435+
int height = image_ptr->GetHeight();
436+
int stride = image_ptr->GetStride();
435437

436-
// ROS_INFO_ONCE("\033[93m wxh: (%d, %d), stride: %d \n", width, height, stride);
437-
fillImage(*image, imageEncoding, height, width, stride, image_ptr->GetData());
438-
image->header.frame_id = frame_id;
439-
} // end else
438+
// ROS_INFO_ONCE("\033[93m wxh: (%d, %d), stride: %d \n", width, height, stride);
439+
fillImage(*image, imageEncoding, height, width, stride, image_ptr->GetData());
440+
image->header.frame_id = frame_id;
441+
440442
}
441443
catch (const Spinnaker::Exception& e)
442444
{

‎spinnaker_camera_driver/src/nodelet.cpp‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -338,6 +338,8 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet
338338

339339
// Set up a diagnosed publisher
340340
double desired_freq;
341+
std::string device_type;
342+
pnh.param<std::string>("device_type", device_type, "USB3");
341343
pnh.param<double>("desired_freq", desired_freq, 30.0);
342344
pnh.param<double>("min_freq", min_freq_, desired_freq);
343345
pnh.param<double>("max_freq", max_freq_, desired_freq);
@@ -374,7 +376,10 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet
374376
diag_man->addDiagnostic("PowerSupplyVoltage", true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f);
375377
diag_man->addDiagnostic("PowerSupplyCurrent", true, std::make_pair(0.4f, 0.6f), 0.3f, 1.0f);
376378
diag_man->addDiagnostic<int>("DeviceUptime");
377-
diag_man->addDiagnostic<int>("U3VMessageChannelID");
379+
if( device_type.compare("USB3") == 0 )
380+
{
381+
diag_man->addDiagnostic<int>("U3VMessageChannelID");
382+
}
378383
}
379384

380385
/**

0 commit comments

Comments
 (0)