Hello,
I started some weeks ago with ROS, now I want to write a node (or nodelet or package) for
my stereo-camera-system (2 Point Grey Flea3 GigE-Cameras).
The pointgrey-camera-driver for ROS doesnt let me handle things like the polarity of the trigger
or setting a fixed IP.
Basically my program already exists as a Visual Studio 2010 (yes, PointGrey doesnot support VS2012)
project. The program checks the system for exisiting cameras and then sets them up with fixed IP's, enables a trigger,
selects a ROI and so on. In the end the settings are stored to the cameras' memory channels where they persist.
Basically I have the problem that I dont know with which ROS tutorial to start and whether I want to create an own
ROS package or if it makes sense to define my program as a node/nodelet of the pointgrey-camera package.
I think my node should not subscribe to any topics, it really is more of a c++ program working on its own.
I have the compiled library files (eqivalent to windows dlls in Linux I guess) for the FlyCap SDK.
So my idea is to put that node (if succesfully) into my launch file before starting the camera,
ideally the node should return a boolean true/false on sucess/failure to the system. How can that be done?
My supervisor told me for Linux there exists a good IDE called QtCreator, so far I have used VS and eclipse in Windows.
Do you have any ideas/suggestions for me?
----UPDATE---
This is the code of my node "stereo_setup" in stereo_setup.cpp.
What happens to lines like std::cout << "xyz" or printf(...) in ROS?
I am still uncertain how to set the info that the camera_setup was successfull.
#include
#include
#include "FlyCapture2_Axel.h"
using namespace std;
using namespace FlyCapture2;
const char * const mac_address_char_12062824 = "00:B0:9D:B8:10:68";
const char * const mac_address_char_12062828 = "00:B0:9D:B8:10:6C";
const char * const ip_address_char_12062824 = "192.168.1.2";
const char * const ip_address_char_12062828 = "192.168.1.3";
const char * const ip_address_char_submask = "255.255.0.0";
const char * const ip_address_char_gateway= "0.0.0.0";
const unsigned int SN_12062824 = 12062824;
const unsigned int SN_12062828 = 12062828;
const Mode k_fmt7Mode = MODE_0;
const unsigned int image_width = 1344;
const unsigned int image_height = 1032;
const unsigned int MAX_image_width = 1384;
const unsigned int MAX_image_height = 1032;
const PixelFormat pixel_format = PIXEL_FORMAT_RGB;
//turn shutter to manual mode
const bool SHUTTER_MANUAL_MODE = false;
//shutter duration in milliseconds
const unsigned int SHUTTER_DURATION = 8;
//turn on filewriting
//const bool WRITE_TO_FILE = true;
// File Format
// File Format
//const char * file_format = "bmp";
bool initialize_cameras();
bool connect_cameras(Camera * cam_12062824, Camera * cam_12062828, PGRGuid * pgr_guid_12062824, PGRGuid * pgr_guid_12062828);
bool setup_cameras(Camera * cam_12062824, Camera * cam_12062828);
void PrintBuildInfo();
void PrintError( Error error );
void PrintCameraInfo( CameraInfo* pCamInfo );
void PrintFormat7Capabilities( Format7Info fmt7Info);
int main(int argc, char** argv )
{
ros::init(argc, argv, "stereo_setup");
PrintBuildInfo();
Error error;
//BusManager busmanager;
Camera cam_12062824, cam_12062828;
PGRGuid pgr_guid_12062824, pgr_guid_12062828;
bool ini = initialize_cameras();
if(ini == false)
{
std::cout << "Could not successfully initialize the two cameras!" << std::endl;
return -1;
}
else
{
bool connect = connect_cameras(&cam_12062824, &cam_12062828, &pgr_guid_12062824, &pgr_guid_12062828);
if(connect == false)
{
std::cout << "Could not successfully connect the two cameras!" << std::endl;
return -1;
}
}
bool setup = setup_cameras(&cam_12062824, &cam_12062828);
if (setup == false)
{
std::cout << "Could not successfully setup the two cameras!" << std::endl;
return -1;
}
cam_12062824.Disconnect();
cam_12062828.Disconnect();
return 0;
}
bool initialize_cameras()
{
Error error;
BusManager busmanager;
MACAddress mac_address_12062824 = MACAddress(mac_address_char_12062824);
IPAddress ip_address_12062824 = IPAddress(ip_address_char_12062824);
MACAddress mac_address_12062828 = MACAddress(mac_address_char_12062828);
IPAddress ip_address_12062828 = IPAddress(ip_address_char_12062828);
IPAddress ip_address_gateway = IPAddress(ip_address_char_gateway);
IPAddress ip_address_submask = IPAddress(ip_address_char_submask);
unsigned int GigECamera_arraysize = 2;
CameraInfo camerainfo [2];
error = busmanager.DiscoverGigECameras(camerainfo , &GigECamera_arraysize);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
if((camerainfo[0].serialNumber == SN_12062824 || camerainfo[0].serialNumber == SN_12062828) && (camerainfo[1].serialNumber == SN_12062824 || camerainfo[1].serialNumber == SN_12062828))
{
//switch cameras if swapped
if(camerainfo[0].serialNumber == SN_12062828)
{
CameraInfo temp = camerainfo[0];
camerainfo [0] = camerainfo[1];
camerainfo [1] = temp;
}
if(camerainfo[0].ipAddress != ip_address_12062824)
{
error = busmanager.ForceIPAddressToCamera(mac_address_12062824, ip_address_12062824, ip_address_submask, ip_address_gateway );
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
}
if(camerainfo[1].ipAddress != ip_address_12062828)
{
error = busmanager.ForceIPAddressToCamera(mac_address_12062828, ip_address_12062828, ip_address_submask, ip_address_gateway );
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
}
std::cout << "Camera Initialization successfull!!!" << std::endl;
for(int i=0; i < GigECamera_arraysize; i++)
{
printf("\n Displaying Camera Information: Camera No. %d \n", i+1);
PrintCameraInfo(camerainfo+i);
}
return true;
}
else
{
return false;
}
}
bool connect_cameras(Camera * cam_12062824, Camera * cam_12062828, PGRGuid * pgr_guid_12062824, PGRGuid * pgr_guid_12062828)
{
Error error;
BusManager busmanager;
error = busmanager.RescanBus();
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
error = busmanager.GetCameraFromSerialNumber(SN_12062824 , pgr_guid_12062824);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
error = busmanager.GetCameraFromSerialNumber(SN_12062828 , pgr_guid_12062828);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
error = cam_12062824->Connect(pgr_guid_12062824);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
error = cam_12062828->Connect(pgr_guid_12062828);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
unsigned int number_of_cameras;
error = busmanager.GetNumOfCameras(&number_of_cameras);
if (error != PGRERROR_OK)
{
PrintError(error);
return false;
}
if (number_of_cameras < 2)
return false;
else
return true;
}
bool setup_cameras(Camera * cam_12062824, Camera * cam_12062828)
{
Error error;
Format7ImageSettings frmt7_img_settings;
frmt7_img_settings.pixelFormat = pixel_format;
frmt7_img_settings.mode = k_fmt7Mode;
frmt7_img_settings.height = image_height;
frmt7_img_settings.width = image_width;
frmt7_img_settings.offsetX = (unsigned int)((MAX_image_width - image_width)/2);
frmt7_img_settings.offsetY = (unsigned int)((MAX_image_height - image_height)/2);
Property property_shutter;
if(SHUTTER_MANUAL_MODE == true)
{
property_shutter.type = SHUTTER;
property_shutter.onOff = true;
property_shutter.autoManualMode = false;
property_shutter.absControl = true;
property_shutter.absValue = SHUTTER_DURATION;
}
else
{
property_shutter.type = SHUTTER;
property_shutter.onOff = true;
property_shutter.autoManualMode = true;
property_shutter.absControl = false;
}
Camera* stereocameras [2];
stereocameras[0] = cam_12062824;
stereocameras[1] = cam_12062828;
Format7ImageSettings current_img_settings;
unsigned int packet_size;
float percentage;
Property current_property;
current_property.type = SHUTTER;
TriggerMode triggermode;
triggermode.mode = 0;
triggermode.onOff = true;
triggermode.polarity = 1;
for(int i = 0; i<2; i++)
{
error = stereocameras[i]->GetFormat7Configuration(¤t_img_settings, &packet_size, &percentage);
if(error != PGRERROR_OK)
{
PrintError(error);
return false;
}
if(frmt7_img_settings != current_img_settings)
{
bool valid;
Format7PacketInfo fmt7PacketInfo;
// Validate the settings to make sure that they are valid
error = stereocameras[i]->ValidateFormat7Settings(&frmt7_img_settings,&valid,&fmt7PacketInfo );
if (error != PGRERROR_OK)
{
PrintError( error );
return -1;
}
if (!valid )
{
// Settings are not valid
printf("Format7 settings are not valid\n");
return false;
}
// Set the settings to the camera
error = stereocameras[i]->SetFormat7Configuration(&frmt7_img_settings,
fmt7PacketInfo.recommendedBytesPerPacket );
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
error = stereocameras[i]->SaveToMemoryChannel(1);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
}
error = stereocameras[i]->GetProperty(¤t_property);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
if(current_property != property_shutter)
{
error = stereocameras[i]->SetProperty(&property_shutter);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
error = stereocameras[i]->SaveToMemoryChannel(1);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
}
/*TriggerMode current_triggermode;
error = stereocameras[i]->GetTriggerMode(¤t_triggermode);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
if(current_triggermode != triggermode)
{
*/
error = stereocameras[i]->SetTriggerMode(&triggermode);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
error = stereocameras[i]->SaveToMemoryChannel(1);
if (error != PGRERROR_OK)
{
PrintError( error );
return false;
}
//}
}
return true;
}
void PrintBuildInfo()
{
FC2Version fc2Version;
Utilities::GetLibraryVersion( &fc2Version );
char version[128];
sprintf(
version,
"FlyCapture2 library version: %d.%d.%d.%d\n",
fc2Version.major, fc2Version.minor, fc2Version.type, fc2Version.build );
printf( "%s", version );
char timeStamp[512];
sprintf( timeStamp, "Application build date: %s %s\n\n", __DATE__, __TIME__ );
printf( "%s", timeStamp );
}
void PrintError( Error error )
{
error.PrintErrorTrace();
}
void PrintCameraInfo( CameraInfo* pCamInfo )
{
printf(
"\n*** CAMERA INFORMATION ***\n"
"Serial number - %u\n"
"Camera model - %s\n"
"Camera vendor - %s\n"
"Sensor - %s\n"
"Resolution - %s\n"
"Firmware version - %s\n"
"Firmware build time - %s\n\n",
pCamInfo->serialNumber,
pCamInfo->modelName,
pCamInfo->vendorName,
pCamInfo->sensorInfo,
pCamInfo->sensorResolution,
pCamInfo->firmwareVersion,
pCamInfo->firmwareBuildTime );
}
↧