void CameraHandle::setTargetFormat() {
VRmDWORD numberOfTargetFormats, i;
VRM_CHECK(VRmUsbCamGetTargetFormatListSizeEx2(device, conf.portLeft, &numberOfTargetFormats));
for (i = 0; i < numberOfTargetFormats; ++i) {
VRM_CHECK(VRmUsbCamGetTargetFormatListEntryEx2(device, conf.portLeft, i, &targetFormat));
if (targetFormat.m_color_format == TARGET_COLOR_FORMAT) break;
}
// Check for right target format
if (targetFormat.m_color_format != TARGET_COLOR_FORMAT) {
const char* screen_color_format_str;
VRM_CHECK(VRmUsbCamGetStringFromColorFormat(TARGET_COLOR_FORMAT, &screen_color_format_str));
ROS_FATAL("%s not found in target format list.", screen_color_format_str);
exit(-1);
}
const char* targetColorFormatStr;
VRM_CHECK(VRmUsbCamGetStringFromColorFormat(targetFormat.m_color_format, &targetColorFormatStr));
ROS_INFO("Selected target format: %d x %d (%s)",
targetFormat.m_width,
targetFormat.m_height,
targetColorFormatStr);
}
bool VRCam::init()
{
if (systemInited == true)
return false;
atexit(VRmUsbCamCleanup);
//更新设备列表
if (!VRmUsbCamUpdateDeviceKeyList()){
sendMsg("update Device failed");
return false;
}
//检查连接的设备
VRmDWORD size = 0;
if (!(VRmUsbCamGetDeviceKeyListSize(&size))){
sendMsg("Get Device Size failed");
return false;
}
if (size == 0){
sendMsg("0 Cam Connect");
return false;
}
sendMsg(tr("%1 Cam Find").arg(size));
//打开第一个可用的设备
VRmDeviceKey* p_device_key = 0;
for (VRmDWORD i = 0; i < size; ++i)
{
VRMEXECANDCHECK(VRmUsbCamGetDeviceKeyListEntry(i, &p_device_key));
if (!p_device_key->m_busy)
{
if (!(VRmUsbCamOpenDevice(p_device_key, &device)))
{
sendMsg("Open Cam Failed");
}
}
else
{
sendMsg("Device is busy now");
}
VRMEXECANDCHECK(VRmUsbCamFreeDeviceKey(&p_device_key));
}
if (!device)
{
return false;
}
//now get the first sensor port
VRMEXECANDCHECK(VRmUsbCamGetSensorPortListEntry(device, 0, &port));
// get the current source format
VRmImageFormat source_format;
VRMEXECANDCHECK(VRmUsbCamGetSourceFormatEx(device, port, &source_format));
const char *source_color_format_str;
VRMEXECANDCHECK(VRmUsbCamGetStringFromColorFormat(source_format.m_color_format, &source_color_format_str));
VRmDWORD number_of_target_formats, i;
VRMEXECANDCHECK(VRmUsbCamGetTargetFormatListSizeEx2(device, port, &number_of_target_formats));
for (i = 0; i < number_of_target_formats; ++i)
{
VRMEXECANDCHECK(VRmUsbCamGetTargetFormatListEntryEx2(device, port, i, &target_format));
if (target_format.m_color_format == VRM_ARGB_4X8)
break;
}
systemInited = true;
return true;
}
int main(int argc, char *argv[])
{
// at first, be sure to call VRmUsbCamCleanup() at exit, even in case
// of an error
atexit(VRmUsbCamCleanup);
//-- scan for devices --
//update
VRmUsbCamUpdateDeviceKeyList();
VRmDWORD size = 0;
VRmUsbCamGetDeviceKeyListSize(&size);
std::cout << "Found "<< size << " devices" << std::endl;
if(size != 1)
{
std::cout << "found more or none devie..." << std::endl;
std::cout << "this programm ist just for hanlde with one device please change code..." << std::endl;
std::cout << "will exit now...." << std::endl;
exit(EXIT_FAILURE);
}
VRmUsbCamDevice device=0;
VRmDeviceKey* p_device_key=0;
//open device
VRmUsbCamGetDeviceKeyListEntry(0, &p_device_key);
if(!p_device_key->m_busy)
{
VRmUsbCamOpenDevice(p_device_key, &device);
}
// display error when no camera has been found
if(!device)
{
std::cerr << "No suitable VRmagic device found!" << std::endl;
exit(EXIT_FAILURE);
}
std::cout << "Open deivce succesful" << std::endl;
//init camera
VRmImageFormat target_format;
VRmDWORD port = 0; //id of Sensor //for multisensorunits
VRmBOOL supported;
// check number of connected sensors
VRmDWORD num_sensorports=0;
VRmUsbCamGetSensorPortListSize(device, &num_sensorports);
// for this demo we switch off all connected sensor but the first one in the port list
for(VRmDWORD ii=0; ii<num_sensorports;ii++)
{
VRmUsbCamGetSensorPortListEntry(device, ii, &port);
std::cout << "found PORT: " << port << std::endl;
// on single sensor devices this property does not exist
VRmPropId sensor_enable = (VRmPropId)(VRM_PROPID_GRAB_SENSOR_ENABLE_1_B-1+port);
VRmUsbCamGetPropertySupported(device, sensor_enable, &supported);
if(supported)
{
//enable first sensor in port list
VRmBOOL enable = 1;
if(ii)
enable = 0;
VRmUsbCamSetPropertyValueB(device, sensor_enable, &enable);
}
}
//now get the first sensor port
VRmUsbCamGetSensorPortListEntry(device, 0, &port);
std::cout << "PORT: " << port << " is used" << std::endl;
//check if exposure time can be set
VRmUsbCamGetPropertySupported(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &supported);
float value=0.f;
VRmUsbCamGetPropertyValueF(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &value);
std::cout << "ExposureTime: " << value << "ms, changeable: "<< (supported ? "true" : "false") << std::endl;
if(supported)
{
value=EXPOSURE_TIME;
// uncomment the following lines to change exposure time to 25ms
// when camera supports this feature
VRmUsbCamSetPropertyValueF(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &value);
std::cout << "ExposureTime changed to : " << value << "ms" << std::endl;
}
//prepare imageformat
VRmDWORD number_of_target_formats, i;
VRmUsbCamGetTargetFormatListSizeEx2( device, port, &number_of_target_formats );
for ( i = 0; i < number_of_target_formats; ++i )
{
VRmUsbCamGetTargetFormatListEntryEx2(device, port, i, &target_format);
if(target_format.m_color_format == VRM_BGR_3X8)
{
std::cout << "BGR8 found !" << std::endl;
break;
}
}
//start grab
VRmUsbCamResetFrameCounter(device);
VRmUsbCamStart(device);
VRmImage* p_target_img = 0;
VRmUsbCamNewImage(&p_target_img,target_format);
std::cout << "target img data: "
<< "color_format " << p_target_img->m_image_format.m_color_format
<< " width " << p_target_img->m_image_format.m_width
<< " heigt " << p_target_img->m_image_format.m_height
<< " modifier " << p_target_img->m_image_format.m_image_modifier << std::endl;
//p_target_img->m_pitch = 3 * p_target_img->m_image_format.m_width;
std::cout << "target img pitch: " << p_target_img->m_pitch << std::endl;
//===================================================================================================================
ohm::VrMagicHandler_camhost _rosBrige(PORT);
ohm::ImageType _rosImage;
std::cout << "------------------------------------------------------------" << std::endl;
std::cout << "--- Waiting for ROS-HOST... --------------------------------" << std::endl;
std::cout << "------------------------------------------------------------" << std::endl;
_rosBrige.connect();
std::cout << "Connected to ROS-HOST" << std::endl;
//set rosimage
_rosImage.id = port;
_rosImage.dataSize = p_target_img->m_image_format.m_width * 3 * p_target_img->m_image_format.m_height;
_rosImage.dataType = ohm::IMAGE;
_rosImage.compressionType = ohm::NONE;
_rosImage.width = p_target_img->m_image_format.m_width;
_rosImage.height = p_target_img->m_image_format.m_height;
_rosImage.channels = 3;
_rosImage.bytePerPixel = 3;
_rosImage.data = NULL;
OHM_DATA_TYPE* _rosImgBuffer;
_rosImgBuffer = new OHM_DATA_TYPE[_rosImage.dataSize];
//===================================================================================================================
bool err_loop = false;
unsigned int cnt = 0;
//source img
VRmImage* p_source_img = 0;
//enter main loop
while(!err_loop)
{
VRmDWORD frames_dropped;
if(!VRmUsbCamLockNextImageEx(device,port,&p_source_img,&frames_dropped))
{
std::cerr << "Error at locking next image" << std::endl;
err_loop = true;
break;
}
if(cnt % MOD_VAL == 0)
{//transmitt every MOD_VAL'th image to ensure that the camera grabbuffer is emty
if(!VRmUsbCamConvertImage(p_source_img,p_target_img))
{
std::cerr << "Error at converting image: " << VRmUsbCamGetLastError() << std::endl;
err_loop = true;
break;
}
//-- work on image --
//-- end work on image --
//-- transmitt to ros --
//===================================================================================================================
//copy data to _rosImgBuffer:
for(unsigned int y = 0; y < _rosImage.height; y++)
{
for(unsigned int x = 0; x < _rosImage.width * 3; x++)
{
_rosImgBuffer[y*_rosImage.width*3 + x] = p_target_img->mp_buffer[y*p_target_img->m_pitch + x];
}
}
_rosImage.data = _rosImgBuffer;
_rosBrige.writeImage(_rosImage);
//===================================================================================================================
// -- end trasmitt to ros --
}
if(!VRmUsbCamUnlockNextImage(device,&p_source_img))
{
std::cerr << "Error at unlocking next image" << std::endl;
err_loop = true;
break;
}
//droped images:
if(frames_dropped)
{
std::cout << frames_dropped <<" frame(s) dropped" << std::endl;
}
cnt++;
}
if(err_loop)
{
std::cerr << "exit with error" << std::endl;
}
//free target image
VRmUsbCamFreeImage(&p_target_img);
//stop grab
VRmUsbCamStop(device);
//close device
VRmUsbCamCloseDevice(device);
return 0;
}
