Go to the documentation of this file.00001
00022 #include <iostream>
00023 #include <cstdlib>
00024 #include <cstring>
00025 #include <boost/thread.hpp>
00026
00027 #include <vrmusbcam2.h>
00028 #include "../VrMagicHandler_camhost/VrMagicHandler_camhost.h"
00029
00030 #define PORT 1234
00031 #define MOD_VAL 2
00032 #define EXPOSURE_TIME 5.f
00033
00034
00035 int main(int argc, char *argv[])
00036 {
00037
00038
00039 atexit(VRmUsbCamCleanup);
00040
00041
00042
00043
00044
00045 VRmUsbCamUpdateDeviceKeyList();
00046 VRmDWORD size = 0;
00047 VRmUsbCamGetDeviceKeyListSize(&size);
00048
00049 std::cout << "Found "<< size << " devices" << std::endl;
00050
00051 if(size != 1)
00052 {
00053 std::cout << "found more or none devie..." << std::endl;
00054 std::cout << "this programm ist just for hanlde with one device please change code..." << std::endl;
00055 std::cout << "will exit now...." << std::endl;
00056 exit(EXIT_FAILURE);
00057 }
00058
00059 VRmUsbCamDevice device=0;
00060 VRmDeviceKey* p_device_key=0;
00061
00062 VRmUsbCamGetDeviceKeyListEntry(0, &p_device_key);
00063 if(!p_device_key->m_busy)
00064 {
00065 VRmUsbCamOpenDevice(p_device_key, &device);
00066 }
00067
00068
00069 if(!device)
00070 {
00071 std::cerr << "No suitable VRmagic device found!" << std::endl;
00072 exit(EXIT_FAILURE);
00073 }
00074
00075 std::cout << "Open deivce succesful" << std::endl;
00076
00077
00078
00079
00080
00081 VRmImageFormat target_format;
00082 VRmDWORD port = 0;
00083
00084 VRmBOOL supported;
00085
00086 VRmDWORD num_sensorports=0;
00087 VRmUsbCamGetSensorPortListSize(device, &num_sensorports);
00088 std::cout << "Found " << num_sensorports << " sensors" << std::endl;
00089
00090 VRmDWORD ports[num_sensorports];
00091
00092 for(VRmDWORD ii=0; ii<num_sensorports;ii++)
00093 {
00094 VRmUsbCamGetSensorPortListEntry(device, ii, &port);
00095 std::cout << "found PORT: " << port << std::endl;
00096
00097
00098 VRmPropId sensor_enable = (VRmPropId)(VRM_PROPID_GRAB_SENSOR_ENABLE_1_B-1+port);
00099 VRmUsbCamGetPropertySupported(device, sensor_enable, &supported);
00100 if(supported)
00101 {
00102 VRmBOOL enable = 1;
00103 VRmUsbCamSetPropertyValueB(device, sensor_enable, &enable);
00104
00105 VRmUsbCamGetSensorPortListEntry(device, ii, &ports[ii]);
00106 std::cout << "PORT: " << ports[ii] << " is used" << std::endl;
00107 }
00108 }
00109
00110
00111 VRmUsbCamGetPropertySupported(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &supported);
00112 float value=0.f;
00113 VRmUsbCamGetPropertyValueF(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &value);
00114 std::cout << "ExposureTime: " << value << "ms, changeable: "<< (supported ? "true" : "false") << std::endl;
00115 if(supported)
00116 {
00117 value=EXPOSURE_TIME;
00118 VRmUsbCamSetPropertyValueF(device, VRM_PROPID_CAM_EXPOSURE_TIME_F, &value);
00119 std::cout << "ExposureTime changed to: " << value << "ms" << std::endl;
00120 }
00121
00122
00123
00124
00125 VRmDWORD number_of_target_formats, i;
00126 VRmUsbCamGetTargetFormatListSizeEx2( device, ports[0], &number_of_target_formats );
00127 for ( i = 0; i < number_of_target_formats; ++i )
00128 {
00129 VRmUsbCamGetTargetFormatListEntryEx2(device, ports[0], i, &target_format);
00130 if(target_format.m_color_format == VRM_BGR_3X8)
00131 {
00132 std::cout << "BGR8 found !" << std::endl;
00133 break;
00134 }
00135 }
00136
00137
00138 VRmUsbCamResetFrameCounter(device);
00139 VRmUsbCamStart(device);
00140
00141 VRmImage* p_target_img[4] = {0};
00142 for(unsigned int i = 0; i < num_sensorports; i++)
00143 {
00144 VRmUsbCamNewImage(&p_target_img[i],target_format);
00145 }
00146
00147 std::cout << "target img data: "
00148 << "color_format " << p_target_img[0]->m_image_format.m_color_format
00149 << " width " << p_target_img[0]->m_image_format.m_width
00150 << " heigt " << p_target_img[0]->m_image_format.m_height
00151 << " modifier " << p_target_img[0]->m_image_format.m_image_modifier << std::endl;
00152
00153
00154 std::cout << "target img pitch: " << p_target_img[0]->m_pitch << std::endl;
00155
00156
00157 ohm::VrMagicHandler_camhost _rosBrige(PORT);
00158 ohm::ImageType _rosImage;
00159
00160 std::cout << "------------------------------------------------------------" << std::endl;
00161 std::cout << "--- Waiting for ROS-HOST... --------------------------------" << std::endl;
00162 std::cout << "------------------------------------------------------------" << std::endl;
00163 _rosBrige.connect();
00164 std::cout << "Connected to ROS-HOST" << std::endl;
00165
00166
00167
00168 _rosImage.id = port;
00169 _rosImage.dataSize = p_target_img[0]->m_image_format.m_width * 3 * p_target_img[0]->m_image_format.m_height;
00170 _rosImage.dataType = ohm::IMAGE;
00171 _rosImage.compressionType = ohm::NONE;
00172 _rosImage.width = p_target_img[0]->m_image_format.m_width;
00173 _rosImage.height = p_target_img[0]->m_image_format.m_height;
00174 _rosImage.channels = 3;
00175 _rosImage.bytePerPixel = 3;
00176 _rosImage.data = NULL;
00177
00178 OHM_DATA_TYPE* _rosImgBuffer;
00179 _rosImgBuffer = new OHM_DATA_TYPE[_rosImage.dataSize];
00180
00181
00182 bool err_loop = false;
00183 unsigned int cnt = 0;
00184
00185 VRmImage* p_source_img[4] = {0};
00186
00187
00188 while(!err_loop)
00189 {
00190 VRmDWORD frames_dropped;
00191 for(unsigned int i = 0; i < num_sensorports; i++)
00192 {
00193 if(!VRmUsbCamLockNextImageEx(device,ports[i],&p_source_img[i],&frames_dropped))
00194 {
00195 std::cerr << "Error at locking next image" << std::endl;
00196 err_loop = true;
00197 break;
00198 }
00199 }
00200
00201
00202
00203
00204
00205 for(unsigned int i = 0; i < num_sensorports; i++)
00206 {
00207 if(!VRmUsbCamConvertImage(p_source_img[i],p_target_img[i]))
00208 {
00209 std::cerr << "Error at converting image: " << VRmUsbCamGetLastError() << std::endl;
00210 err_loop = true;
00211 break;
00212 }
00213
00214
00215
00216
00217
00218
00219
00220
00221 for(unsigned int y = 0; y < _rosImage.height; y++)
00222 {
00223 for(unsigned int x = 0; x < _rosImage.width * 3; x++)
00224 {
00225 _rosImgBuffer[y*_rosImage.width*3 + x] = p_target_img[i]->mp_buffer[y*p_target_img[i]->m_pitch + x];
00226 }
00227 }
00228 _rosImage.data = _rosImgBuffer;
00229
00230 _rosImage.id = ports[i];
00231 _rosBrige.writeImage(_rosImage);
00232
00233
00234 }
00235 for(unsigned int i = 0; i < num_sensorports; i++)
00236 {
00237 if(!VRmUsbCamUnlockNextImage(device,&p_source_img[i]))
00238 {
00239 std::cerr << "Error at unlocking next image" << std::endl;
00240 err_loop = true;
00241 break;
00242 }
00243
00244 }
00245
00246
00247 if(frames_dropped)
00248 {
00249 std::cout << frames_dropped <<" frame(s) dropped" << std::endl;
00250 }
00251 cnt++;
00252 }
00253
00254 if(err_loop)
00255 {
00256 std::cerr << "exit with error" << std::endl;
00257 }
00258
00259 for(unsigned int i = 0; i < num_sensorports; i++)
00260 {
00261 VRmUsbCamFreeImage(&p_target_img[i]);
00262 }
00263
00264 VRmUsbCamStop(device);
00265
00266 VRmUsbCamCloseDevice(device);
00267 return 0;
00268 }