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;
}
int main(int argc, char *argv[])
{
get_param();
std::string t_stamp = get_timestamp();
sprintf (data_filename, "%s/data_%s.out", save_dir, t_stamp.c_str());
std::cout << "data_filename: " << data_filename << std::endl;
FlyCapture2::BusManager busMgr;
PGRGuid guid;
err = busMgr.GetCameraFromSerialNumber(cam_serial, &guid);
if (err != PGRERROR_OK) {
std::cout << "Index error" << std::endl;
return false;
}
// Point Grey cam init
init_cam_capture(pt_grey, guid);
if (use_calib)
load_calib_param();
imageCallback(&pt_grey);
printf("Stopping...\n");
err = pt_grey.StopCapture();
if ( err != PGRERROR_OK )
{
// This may fail when the camera was removed, so don't show
// an error message
}
pt_grey.Disconnect();
}
Camera::Camera(unsigned int serNo) : frameRate(FlyCapture2::FRAMERATE_30)
{
FlyCapture2::BusManager busMgr;
throw_if_error(busMgr.GetCameraFromSerialNumber(serNo, &guid));
}
// Fill out physical_cameras_p vector
int TrackerDataSource::init_cameras(){
// we don't yet know whether we're reading from camera or file
file_sources = BOU_UNSET;
FlyCapture2::BusManager busMgr;
FlyCapture2::Error error;
unsigned int n_cams;
ptgr_err( busMgr.GetNumOfCameras(&n_cams) );
if(n_cams < 1){
std::cerr << "No cameras found.\n";
return -1;
}
int n_software_cams = 0;
for(int g = 0; g < opt.group_size(); g++){
for(int c = 0; c < opt.group(g).cam_size(); c++){
n_software_cams++;
bool_or_unset this_cam_from_file =
opt.group(g).cam(c).has_input_file_name() ? BOU_TRUE : BOU_FALSE ;
// Complain if some cameras want input files and others don't
if( (this_cam_from_file != file_sources) && (file_sources != BOU_UNSET) ){
std::cerr << "Configuration error: group " << g << " camera " << c
<< " has input_file settings inconsistent with other cameras.";
} else {
file_sources = this_cam_from_file;
}
if(file_sources == BOU_FALSE){
// Get guid
FlyCapture2::PGRGuid guid;
ptgr_err( error =
busMgr.GetCameraFromSerialNumber(opt.group(g).cam(c).serial_number(),
&guid) );
if(error != FlyCapture2::PGRERROR_OK){
std::cerr << "Error getting guid from serial number "
<< opt.group(g).cam(c).serial_number()
<< std::endl;
list_serial_numbers();
}
// Connect
FlyCapture2::Camera *this_camera_p = new FlyCapture2::Camera ();
ptgr_err( error = this_camera_p->Connect( &guid ) );
if(error != FlyCapture2::PGRERROR_OK){
std::cerr << "Error connecting to camera with serial number "
<< opt.group(g).cam(c).serial_number() << std::endl;
}
// Find videomode
// I don't know the type of these macros, auto for now TODO: fix type
auto the_mode = FlyCapture2::VIDEOMODE_640x480Y8;
auto the_rate = FlyCapture2::FRAMERATE_30;
if( FRAME_WIDTH == 640 && FRAME_HEIGHT == 480 ){
the_mode = FlyCapture2::VIDEOMODE_640x480Y8;
} else {
std::cerr << "Unsupported image size\n";
}
if( FRAME_RATE == 30 ){
the_rate = FlyCapture2::FRAMERATE_30;
} else if (FRAME_RATE == 15) {
the_rate = FlyCapture2::FRAMERATE_15;
} else {
std::cerr << "Unsupported frame rate\n";
}
// Setup camera
ptgr_err( error = this_camera_p->
SetVideoModeAndFrameRate(the_mode, the_rate) );
if( error != FlyCapture2::PGRERROR_OK ){
std::cerr << "Error in setup for camera with serial number "
<< opt.group(g).cam(c).serial_number() << std::endl;
}
// Register camera with tracker
// Insert a pair into the camera listing map
// camera_pointer -> image_pointer
physical_cameras_p[this_camera_p] = (*frames)[g][c];
// Initialize the capture
// Build the camera list in the way that flycapture2 wants it: pointer array
std::map<FlyCapture2::Camera*, ArteFrame*>::iterator it;
int n_cams = physical_cameras_p.size();
ppCameras = new FlyCapture2::Camera*[n_cams];
it = physical_cameras_p.begin();
for(int i = 0; i < n_cams; i++){
ppCameras[i] = it->first;
it++;
}
} // endif this camera really is camera soure
if(file_sources == BOU_TRUE){
// Create the opencv file capture
CvCapture *this_file_capture =
cvCreateFileCapture( opt.group(g).cam(c).input_file_name().c_str() );
if(!this_file_capture){
std::cerr << "TrackerDataSource error opening file "
<< opt.group(g).cam(c).input_file_name().c_str() << std::endl;
}
// Register the file capture with tracker
simulated_cameras_p[this_file_capture] = (*frames)[g][c];
} // endif this camera has file source
} // done with this camera
} // done with this camera group
}
void StereoCamera::initCamera(FlyCapture2::GigECamera &cam, const int serial) {
FlyCapture2::Error error;
FlyCapture2::BusManager busMgr;
FlyCapture2::PGRGuid guid;
error = busMgr.GetCameraFromSerialNumber(serial, &guid);
if (error != FlyCapture2::PGRERROR_OK)
{
printf( "Error\n" );
//PrintError( error );
//return -1;
}
// Connect to a camera
error = cam.Connect(&guid);
if (error != FlyCapture2::PGRERROR_OK)
{
printf( "Error\n" );
//PrintError( error );
//return -1;
}
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK)
{
printf( "Error\n" );
//PrintError( error );
//return -1;
}
//PrintCameraInfo(&camInfo);
FlyCapture2::GigEImageSettingsInfo imageSettingsInfo;
error = cam.GetGigEImageSettingsInfo( &imageSettingsInfo );
if (error != FlyCapture2::PGRERROR_OK)
{
printf( "Error\n" );
// PrintError( error );
//P return -1;
}
FlyCapture2::GigEImageSettings imageSettings;
imageSettings.offsetX = 0;
imageSettings.offsetY = 0;
imageSettings.height = imageSettingsInfo.maxHeight;
imageSettings.width = imageSettingsInfo.maxWidth;
imageSettings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RAW8;
printf( "Setting GigE image settings...\n" );
error = cam.SetGigEImageSettings( &imageSettings );
if (error != FlyCapture2::PGRERROR_OK)
{
printf( "Error\n" );
// PrintError( error );
// return -1;
}
}
Camera::Camera(ros::NodeHandle _comm_nh, ros::NodeHandle _param_nh) :
node(_comm_nh), pnode(_param_nh), it(_comm_nh),
info_mgr(_comm_nh, "camera"), cam() {
FlyCapture2::Error error;
/* default config values */
width = 640;
height = 480;
fps = 10;
frame = "camera";
rotate = false;
/* set up information manager */
std::string url;
pnode.getParam("camera_info_url", url);
info_mgr.loadCameraInfo(url);
/* pull other configuration */
pnode.getParam("serial", serial);
pnode.getParam("fps", fps);
pnode.getParam("skip_frames", skip_frames);
pnode.getParam("width", width);
pnode.getParam("height", height);
pnode.getParam("frame_id", frame);
/* advertise image streams and info streams */
pub = it.advertise("image_raw", 1);
info_pub = node.advertise<CameraInfo>("camera_info", 1);
/* initialize the cameras */
FlyCapture2::BusManager busMgr;
FlyCapture2::PGRGuid guid;
error = busMgr.GetCameraFromSerialNumber(serial, &guid);
// Connect to a camera
error = cam.Connect(&guid);
if (error != FlyCapture2::PGRERROR_OK)
{
//PrintError( error );
//return -1;
}
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK)
{
//PrintError( error );
//return -1;
}
PrintCameraInfo(&camInfo);
FlyCapture2::GigEImageSettingsInfo imageSettingsInfo;
error = cam.GetGigEImageSettingsInfo( &imageSettingsInfo );
if (error != FlyCapture2::PGRERROR_OK)
{
// PrintError( error );
//P return -1;
}
FlyCapture2::GigEImageSettings imageSettings;
imageSettings.offsetX = 0;
imageSettings.offsetY = 0;
imageSettings.height = imageSettingsInfo.maxHeight;
imageSettings.width = imageSettingsInfo.maxWidth;
imageSettings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RAW8;
printf( "Setting GigE image settings...\n" );
error = cam.SetGigEImageSettings( &imageSettings );
if (error != FlyCapture2::PGRERROR_OK)
{
// PrintError( error );
// return -1;
}
/* and turn on the streamer */
error = cam.StartCapture();
if (error != FlyCapture2::PGRERROR_OK)
{
// PrintError( error );
// return -1;
}
ok = true;
image_thread = boost::thread(boost::bind(&Camera::feedImages, this));
}