static bool check_success(FlyCapture2::Error error) {
if(error != FlyCapture2::PGRERROR_OK) {
ROS_ERROR ("%s", error.GetDescription());
return false;
}
return true;
}
bool Camera::setup()
{
///////////////////////////////////////////////////////
// Get a camera:
FlyCapture2::Error error;
FlyCapture2::BusManager busMgr;
unsigned int N;
if ((error = busMgr.GetNumOfCameras(&N)) != PGRERROR_OK)
ROS_ERROR (error.GetDescription ());
if (camSerNo == 0)
{
if ((error = busMgr.GetCameraFromIndex(0, &guid_)) != PGRERROR_OK)
PRINT_ERROR_AND_RETURN_FALSE;
ROS_INFO ("did busMgr.GetCameraFromIndex(0, &guid)");
}
else
{
if ((error = busMgr.GetCameraFromSerialNumber(camSerNo, &guid_)) != PGRERROR_OK)
PRINT_ERROR_AND_RETURN_FALSE;
}
ROS_INFO ("Setup: Camera GUID = %u %u %u %u", guid_.value[0], guid_.value[1], guid_.value[2], guid_.value[3]);
ROS_INFO ("Setup successful");
return true;
}
void Camera::SetExposure(bool _auto, bool onoff, unsigned int value)
{
FlyCapture2::Property prop;
FlyCapture2::Error error;
prop.type = FlyCapture2::AUTO_EXPOSURE;
prop.autoManualMode = _auto;
prop.onOff = onoff;
prop.valueA = value;
if ((error = camPGR_.SetProperty(&prop)) != PGRERROR_OK)
{
ROS_ERROR (error.GetDescription ());
}
}
void PointGreyCamera::handleError(const std::string &prefix, const FlyCapture2::Error &error)
{
if(error == PGRERROR_TIMEOUT)
{
throw CameraTimeoutException("PointGreyCamera: Failed to retrieve buffer within timeout.");
}
else if(error != PGRERROR_OK) // If there is actually an error (PGRERROR_OK means the function worked as intended...)
{
std::string start(" | FlyCapture2::ErrorType ");
std::stringstream out;
out << error.GetType();
std::string desc(error.GetDescription());
throw std::runtime_error(prefix + start + out.str() + " " + desc);
}
}
void init()
{
FlyCapture2::Error error;
FlyCapture2::BusManager busMgr;
for (int tries = 0; tries < 5; ++tries)
{
if ((error = busMgr.GetNumOfCameras(&cameraNum)) != PGRERROR_OK)
ROS_ERROR (error.GetDescription ());
if (cameraNum)
{
ROS_INFO ("Found %d cameras", cameraNum);
unsigned int serNo;
for (unsigned int i = 0; i < cameraNum; i++)
{
if ((error = busMgr.GetCameraSerialNumberFromIndex(i, &serNo)) != PGRERROR_OK)
ROS_ERROR (error.GetDescription ());
else
ROS_INFO ("Camera %u: S/N %u", i, serNo);
}
}
return;
usleep(200000);
}
}
void Camera::SetShutter (bool _auto, float value)
{
FlyCapture2::Property prop;
FlyCapture2::Error error;
prop.type = FlyCapture2::AUTO_EXPOSURE;
prop.autoManualMode = _auto;
prop.onOff = true;
prop.absValue = value;
if ((error = camPGR_.SetProperty(&prop)) != PGRERROR_OK)
{
ROS_ERROR (error.GetDescription ());
}
return;
}
void Camera::start()
{
FlyCapture2::Error error;
if (camPGR_.IsConnected())
{
ROS_INFO ("IsConnected returned true");
}
else
ROS_INFO ("IsConnected returned false");
if ((error = camPGR_.StartCapture(frameDone, (void *) this)) != PGRERROR_OK)
{ //frameDone, (void*) this)) != PGRERROR_OK) {
ROS_ERROR (error.GetDescription ());
}
else
{
ROS_INFO ("StartCapture succeeded.");
}
}
static void throw_if_error(FlyCapture2::Error error) {
if(error != FlyCapture2::PGRERROR_OK) {
throw runtime_error(error.GetDescription());
}
}
bool Camera::initCam()
{
// Start capturing images:
FlyCapture2::Error error;
FlyCapture2::BusManager busMgr;
/*if (guid_ == 0)
if ((error = busMgr.GetCameraFromIndex(camIndex, &guid_)) != PGRERROR_OK)
PRINT_ERROR;
*/
if ((error = camPGR_.Connect(&guid_)) != PGRERROR_OK)
PRINT_ERROR;
ROS_INFO ("Camera GUID = %u %u %u %u", guid_.value[0], guid_.value[1], guid_.value[2], guid_.value[3]);
// Set to software triggering:
FlyCapture2::TriggerMode triggerMode;
if ((error = camPGR_.GetTriggerMode(&triggerMode)) != PGRERROR_OK)
PRINT_ERROR;
// Set camera to trigger mode 0
triggerMode.onOff = false;
if ((error = camPGR_.SetTriggerMode(&triggerMode)) != PGRERROR_OK)
PRINT_ERROR;
// Set other camera configuration stuff:
FlyCapture2::FC2Config fc2Config;
if ((error = camPGR_.GetConfiguration(&fc2Config)) != PGRERROR_OK)
PRINT_ERROR;
fc2Config.grabMode = FlyCapture2::DROP_FRAMES; // supposedly the default, but just in case..
if ((error = camPGR_.SetConfiguration(&fc2Config)) != PGRERROR_OK)
PRINT_ERROR;
ROS_INFO ("Setting video mode to VIDEOMODE_640x480Y8, framerate to FRAMERATE_30...");
if ((error = camPGR_.SetVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_640x480Y8, FlyCapture2::FRAMERATE_30))
!= PGRERROR_OK)
ROS_ERROR (error.GetDescription ());
else
ROS_INFO ("...success");
FlyCapture2::EmbeddedImageInfo embedInfo;
embedInfo.frameCounter.onOff = true;
if ((error = camPGR_.SetEmbeddedImageInfo(&embedInfo)) != PGRERROR_OK)
PRINT_ERROR;
FlyCapture2::CameraInfo camInfo;
if ((error = camPGR_.GetCameraInfo(&camInfo)) != PGRERROR_OK)
PRINT_ERROR;
ROS_INFO ("camInfo.driverName = %s", camInfo.driverName);
ROS_INFO ("camInfo.firmwareVersion = %s", camInfo.firmwareVersion);
ROS_INFO ("camInfo.isColorCamera = %d", camInfo.isColorCamera);
// FIXME: breaks dynamic Resizing of image
// Query for available Format 7 modes
FlyCapture2::Format7Info fmt7Info;
bool supported;
// TODO: Setup modes here (working as Raw for now...either Mono or Color...whatever the camera's raw is)
// if(camInfo.isColorCamera)
// {
// k_fmt7Mode_ = FlyCapture2::MODE_0; // TODO: make dynamic
// k_fmt7PixFmt_ = FlyCapture2::PIXEL_FORMAT_BGR; // TODO: make dynamic
// }
// else // Mono
// {
// k_fmt7Mode_ = FlyCapture2::MODE_1; // TODO: make dynamic
// k_fmt7PixFmt_ = FlyCapture2::PIXEL_FORMAT_MONO8; // TODO: make dynamic
k_fmt7Mode_ = FlyCapture2::MODE_0; // TODO: make dynamic
k_fmt7PixFmt_ = FlyCapture2::PIXEL_FORMAT_RAW8; // TODO: make dynamic
// }
fmt7Info.mode = k_fmt7Mode_;
error = camPGR_.GetFormat7Info( &fmt7Info, &supported );
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
PrintFormat7Capabilities( fmt7Info );
FlyCapture2::Format7ImageSettings fmt7ImageSettings;
fmt7ImageSettings.mode = k_fmt7Mode_;
fmt7ImageSettings.offsetX = 0;
fmt7ImageSettings.offsetY = 0;
fmt7ImageSettings.width = fmt7Info.maxWidth;
fmt7ImageSettings.height = fmt7Info.maxHeight;
fmt7ImageSettings.pixelFormat = k_fmt7PixFmt_;
ROS_INFO("Pixel Format: 0x%08x", k_fmt7PixFmt_);
bool valid;
FlyCapture2::Format7PacketInfo fmt7PacketInfo;
// Validate the settings to make sure that they are valid
error = camPGR_.ValidateFormat7Settings(
&fmt7ImageSettings,
&valid,
&fmt7PacketInfo );
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
if ( !valid )
{
// Settings are not valid
printf("Format7 settings are not valid\n");
return false;
}
// Set the settings to the camera
error = camPGR_.SetFormat7Configuration(
&fmt7ImageSettings,
fmt7PacketInfo.recommendedBytesPerPacket );
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
return true;
}
