00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "vrmstnode.h"
00035
00036 #include "sourceformatlist.h"
00037 #include "formatindicator.h"
00038
00039 #include <iostream>
00040 #include <sstream>
00041
00042 #include <signal.h>
00043
00044 VRMagicStereoNode *vrsnode = NULL;
00045
00046 class VRGrabException : public std::string
00047 {
00048 public:
00049 VRGrabException(const char *err) : std::string(err) {}
00050 };
00051
00052 class VRControlException : public std::string
00053 {
00054 public:
00055 VRControlException(const char *err) : std::string(err) {}
00056 };
00057
00058 class PropertyCache
00059 {
00060 public:
00061 float exposureTime;
00062 int gainLeft, gainRight;
00063 float fps;
00064 VRmBOOL useLEDs;
00065 };
00066
00067 void VRMagicStereoNode::propertyUpdate(vrmagic_multi_driver::CamParamsConfig &config, uint32_t level)
00068 {
00069 if(props->exposureTime != config.exposureTime)
00070 {
00071 boost::lock_guard<boost::mutex> lock(camAccess);
00072
00073 if(!VRmUsbCamStop(device))
00074 throw VRControlException("VRmUsbCamStop failed.");
00075
00076 float newVal = config.exposureTime;
00077 if(!VRmUsbCamSetPropertyValueF(device, VRM_PROPID_MULTICAM_MASTER_EXPOSURE_TIME_F, &newVal))
00078 std::cerr << "VRmUsbCamSetPropertyValueF(MULTICAM_MASTER_EXPOSURE_TIME_F) failed." << std::endl;
00079 else
00080 props->exposureTime = newVal;
00081
00082 if(!VRmUsbCamStart(device))
00083 throw VRControlException("VRmUsbCamStart failed.");
00084
00085 }
00086
00087 if(props->gainLeft != config.gainLeft)
00088 {
00089 boost::lock_guard<boost::mutex> lock(camAccess);
00090
00091 VRmPropId sensor = VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_1;
00092 VRmUsbCamSetPropertyValueE(device, VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_E, &sensor);
00093
00094 int newVal = config.gainLeft;
00095 if(!VRmUsbCamSetPropertyValueI(device, VRM_PROPID_CAM_GAIN_MONOCHROME_I, &newVal))
00096 std::cerr << "VRmUsbCamSetPropertyValueI(VRM_PROPID_CAM_GAIN_MONOCHROME_I) failed." << std::endl;
00097 else
00098 props->gainLeft = newVal;
00099 }
00100
00101 if(props->gainRight != config.gainRight)
00102 {
00103 boost::lock_guard<boost::mutex> lock(camAccess);
00104
00105 VRmPropId sensor = VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_3;
00106 VRmUsbCamSetPropertyValueE(device, VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_E, &sensor);
00107
00108 int newVal = config.gainRight;
00109 if(!VRmUsbCamSetPropertyValueI(device, VRM_PROPID_CAM_GAIN_MONOCHROME_I, &newVal))
00110 std::cerr << "VRmUsbCamSetPropertyValueI(VRM_PROPID_CAM_GAIN_MONOCHROME_I) failed." << std::endl;
00111 else
00112 props->gainRight = newVal;
00113 }
00114
00115 if(props->useLEDs != config.useLEDs)
00116 {
00117 boost::lock_guard<boost::mutex> lock(camAccess);
00118 VRmBOOL newValue = config.useLEDs;
00119 if(!VRmUsbCamSetPropertyValueB(device, VRM_PROPID_DEVICE_STATUS_LED_B, &newValue))
00120 std::cerr << "VRmUsbCamSetPropertyValueB(DEVICE_STATUS_LED_B) failed." << std::endl;
00121 else
00122 props->useLEDs = newValue;
00123 }
00124
00125 if(props->fps != config.fps)
00126 {
00127 boost::lock_guard<boost::mutex> lock(timerAccess);
00128 fpsLimit = ros::Rate(config.fps);
00129 props->fps = config.fps;
00130 }
00131
00132 }
00133
00134 bool VRMagicStereoNode::runUpdateLeft(sensor_msgs::SetCameraInfo::Request &req,
00135 sensor_msgs::SetCameraInfo::Response &res)
00136 {
00137 boost::lock_guard<boost::mutex> lock(calibAccess);
00138 leftCalib = req.camera_info;
00139 storeCalibration();
00140 res.success = true;
00141 res.status_message = "Calibration updated";
00142 return true;
00143 }
00144
00145 bool VRMagicStereoNode::runUpdateRight(sensor_msgs::SetCameraInfo::Request &req,
00146 sensor_msgs::SetCameraInfo::Response &res)
00147 {
00148 boost::lock_guard<boost::mutex> lock(calibAccess);
00149 rightCalib = req.camera_info;
00150 storeCalibration();
00151 res.success = true;
00152 res.status_message = "Calibration updated";
00153 return true;
00154 }
00155
00156 void VRMagicStereoNode::loadCalibration()
00157 {
00158 VRmUserData *uData;
00159 if(!VRmUsbCamLoadUserData(device, &uData))
00160 {
00161 std::cerr << "VRmUsbCamLoadUserData failed." << std::endl;
00162 return;
00163 }
00164
00165 if(uData->m_length == 0)
00166 {
00167 std::cerr << "there is no calibration stored on the camera." << std::endl;
00168 return;
00169 }
00170
00171 ros::serialization::IStream stream(uData->mp_data, uData->m_length);
00172 ros::serialization::deserialize(stream, leftCalib);
00173 ros::serialization::deserialize(stream, rightCalib);
00174
00175 VRmUsbCamFreeUserData(&uData);
00176
00177 calibrated = true;
00178 }
00179
00180 void VRMagicStereoNode::storeCalibration()
00181 {
00182 uint32_t leftLen = ros::serialization::serializationLength(leftCalib);
00183 uint32_t rightLen = ros::serialization::serializationLength(rightCalib);
00184
00185 VRmUserData *uData;
00186 if(!VRmUsbCamNewUserData(&uData, leftLen + rightLen))
00187 {
00188 std::cerr << "VRmUsbCamNewUserData failed." << std::endl;
00189 return;
00190 }
00191 ros::serialization::OStream stream(uData->mp_data, leftLen + rightLen);
00192 ros::serialization::serialize(stream, leftCalib);
00193 ros::serialization::serialize(stream, rightCalib);
00194
00195 camAccess.lock();
00196 if(!VRmUsbCamSaveUserData(device, uData))
00197 {
00198 std::cerr << "VRmUsbCamSaveUserData failed." << std::endl;
00199 }
00200 camAccess.unlock();
00201
00202 std::cout << "calibration written to camera." << std::endl;
00203
00204 VRmUsbCamFreeUserData(&uData);
00205 }
00206
00207
00208 void VRMagicStereoNode::AnnounceTopics()
00209 {
00210 camPubLeft = image_transport::ImageTransport(leftNs).advertiseCamera("image_raw", 2);
00211 camPubRight = image_transport::ImageTransport(rightNs).advertiseCamera("image_raw", 2);
00212 leftCalibUpdate = leftNs.advertiseService("set_camera_info", &VRMagicStereoNode::runUpdateLeft, this);
00213 rightCalibUpdate = rightNs.advertiseService("set_camera_info", &VRMagicStereoNode::runUpdateRight, this);
00214 }
00215
00216 void VRMagicStereoNode::AbandonTopics()
00217 {
00218 camPubLeft.shutdown();
00219 camPubRight.shutdown();
00220 leftCalibUpdate.shutdown();
00221 rightCalibUpdate.shutdown();
00222 }
00223
00224 void VRMagicStereoNode::broadcastFrame()
00225 {
00226 camAccess.lock();
00227 VRmRetVal success = VRmUsbCamSoftTrigger(device);
00228 camAccess.unlock();
00229 if(!success)
00230 throw VRGrabException("VRmUsbCamSoftTrigger failed.");
00231
00232 ros::Time triggerTime = ros::Time::now();
00233
00234 grabFrame(1, imgLeft, triggerTime);
00235 grabFrame(3, imgRight, triggerTime);
00236 leftCalib.header.stamp = triggerTime;
00237 leftCalib.header.frame_id = frame_id;
00238 rightCalib.header.stamp = triggerTime;
00239 rightCalib.header.frame_id = frame_id;
00240
00241 try
00242 {
00243 boost::lock_guard<boost::mutex> lock(calibAccess);
00244 camPubLeft.publish(imgLeft, leftCalib);
00245 }
00246 catch(ros::serialization::StreamOverrunException &crap)
00247 {
00248 std::cerr << "stream overrun in left channel" << std::endl;
00249 }
00250
00251 try
00252 {
00253 boost::lock_guard<boost::mutex> lock(calibAccess);
00254 camPubRight.publish(imgRight, rightCalib);
00255 }
00256 catch(ros::serialization::StreamOverrunException &crap)
00257 {
00258 std::cerr << "stream overrun in right channel" << std::endl;
00259 }
00260 }
00261
00262 void VRMagicStereoNode::grabFrame(VRmDWORD port, sensor_msgs::Image &img, const ros::Time &triggerTime)
00263 {
00264 img.width = width;
00265 img.height = height;
00266 img.step = width * 2;
00267 img.encoding = sensor_msgs::image_encodings::MONO16;
00268 img.data.resize(height * img.step);
00269 img.header.stamp = triggerTime;
00270 img.header.frame_id = frame_id;
00271
00272 VRmImage *VRimg = NULL;
00273 camAccess.lock();
00274 VRmRetVal success = VRmUsbCamLockNextImageEx(device, port, &VRimg, NULL);
00275 camAccess.unlock();
00276 if(!success)
00277 {
00278 std::stringstream err;
00279 err << "VRmUsbCamLockNextImageEx failed for port" << port << ".";
00280 throw VRGrabException(err.str().c_str());
00281 }
00282
00283 for(unsigned int i = 0; i < height * width; i++)
00284 {
00285 img.data[i * 2 + 1] = VRimg->mp_buffer[i * 2] >> 6;
00286 img.data[i * 2] = (VRimg->mp_buffer[i * 2] << 2)
00287 | (VRimg->mp_buffer[i * 2 + 1] & 0x3);
00288 }
00289
00290 if(!VRmUsbCamUnlockNextImage(device, &VRimg))
00291 throw VRGrabException("VRmUsbCamUnlockNextImage failed.");
00292
00293 if(!VRmUsbCamFreeImage(&VRimg))
00294 throw VRGrabException("VRmUsbCamFreeImage failed.");
00295 }
00296
00297 void VRMagicStereoNode::initProperties()
00298 {
00299 props = new PropertyCache();
00300
00301 if(!VRmUsbCamGetPropertyValueF(device, VRM_PROPID_MULTICAM_MASTER_EXPOSURE_TIME_F, &props->exposureTime))
00302 std::cerr << "VRmUsbCamGetPropertyValueF(MULTICAM_MASTER_EXPOSURE_TIME_F) failed." << std::endl;
00303
00304 if(!VRmUsbCamGetPropertyValueB(device, VRM_PROPID_DEVICE_STATUS_LED_B, &props->useLEDs))
00305 std::cerr << "VRmUsbCamGetPropertyValueB(DEVICE_STATUS_LED_B) failed." << std::endl;
00306
00307 VRmPropId sensor = VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_1;
00308 VRmUsbCamSetPropertyValueE(device, VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_E, &sensor);
00309 if(!VRmUsbCamGetPropertyValueI(device, VRM_PROPID_CAM_GAIN_MONOCHROME_I, &props->gainLeft))
00310 std::cerr << "VRmUsbCamGetPropertyValueI(VRM_PROPID_CAM_GAIN_MONOCHROME_I) failed." << std::endl;
00311
00312 sensor = VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_3;
00313 VRmUsbCamSetPropertyValueE(device, VRM_PROPID_GRAB_SENSOR_PROPS_SELECT_E, &sensor);
00314 if(!VRmUsbCamGetPropertyValueI(device, VRM_PROPID_CAM_GAIN_MONOCHROME_I, &props->gainRight))
00315 std::cerr << "VRmUsbCamGetPropertyValueI(VRM_PROPID_CAM_GAIN_MONOCHROME_I) failed." << std::endl;
00316
00317 props->fps = 0.5;
00318 }
00319
00320 void VRMagicStereoNode::initCam(VRmDWORD camDesired)
00321 {
00322 VRmDWORD libversion;
00323 if(!VRmUsbCamGetVersion(&libversion))
00324 throw VRControlException("VRmUsbCamGetVersion failed.");
00325 else
00326 std::cout << "VR Magic lib has version " << libversion << std::endl;
00327
00328 VRmDWORD size;
00329 if(!VRmUsbCamGetDeviceKeyListSize(&size))
00330 throw VRControlException("VRmUsbCamGetDeviceKeyListSize failed.");
00331
00332 if(camDesired >= size)
00333 throw VRControlException("Invalid device index.");
00334
00335 VRmDeviceKey* devKey;
00336 if(!VRmUsbCamGetDeviceKeyListEntry(camDesired, &devKey))
00337 throw VRControlException("VRmUsbCamGetDeviceKeyListEntry failed.");
00338
00339 if(devKey->m_busy)
00340 throw VRControlException("device busy");
00341
00342 if(!VRmUsbCamOpenDevice(devKey, &device))
00343 throw VRControlException("VRmUsbCamOpenDevice failed.");
00344
00345 std::cout << "device " << devKey->mp_product_str << " [" << devKey->mp_manufacturer_str
00346 << "] opened" << std::endl;
00347
00348 if(!VRmUsbCamFreeDeviceKey(&devKey))
00349 std::cerr << "VRmUsbCamFreeDeviceKey failed." << std::endl;
00350
00351 SourceFormatList sFmtList(device);
00352 SourceFormatList::iterator fmt = find_if(sFmtList.begin(), sFmtList.end(), FormatIndicator());
00353 if(fmt == sFmtList.end())
00354 throw VRControlException("no acceptable format found.");
00355
00356 leftCalib.width = rightCalib.width = width = fmt->m_width;
00357 leftCalib.height = rightCalib.height = height = fmt->m_height;
00358
00359 if(!VRmUsbCamSetSourceFormatIndex(device, fmt - sFmtList.begin()))
00360 throw VRControlException("failed to select desired format.");
00361
00362 VRmPropId mode = VRM_PROPID_GRAB_MODE_TRIGGERED_SOFT;
00363 if (!VRmUsbCamSetPropertyValueE(device, VRM_PROPID_GRAB_MODE_E, &mode))
00364 throw VRControlException("failed to set software trigger (VRM_PROPID_GRAB_MODE_TRIGGERED_SOFT).");
00365
00366 if(!VRmUsbCamStart(device))
00367 throw VRControlException("VRmUsbCamStart failed.");
00368 }
00369
00370 void VRMagicStereoNode::retireCam()
00371 {
00372 if(!VRmUsbCamStop(device))
00373 std::cerr << "VRmUsbCamStop failed." << std::endl;
00374
00375 if(!VRmUsbCamCloseDevice(device))
00376 std::cerr << "VRmUsbCamCloseDevice failed." << std::endl;
00377
00378 VRmUsbCamCleanup();
00379 }
00380
00381 VRMagicStereoNode::VRMagicStereoNode(VRmDWORD camDesired) : calibrated(false), framesDelivered(0),
00382 leftNs("left"), rightNs("right"), fpsLimit(0.5), frame_id("camer_optical_frame")
00383 {
00384 leftCalib.K[0] = rightCalib.K[0] = 0.0;
00385 initCam(camDesired);
00386 loadCalibration();
00387 initProperties();
00388 dConfServer.setCallback(boost::bind(&VRMagicStereoNode::propertyUpdate, this, _1, _2));
00389 AnnounceTopics();
00390 }
00391
00392 VRMagicStereoNode::~VRMagicStereoNode()
00393 {
00394 retireCam();
00395 AbandonTopics();
00396 delete props;
00397 }
00398
00399 void VRMagicStereoNode::spin()
00400 {
00401 while(ros::ok())
00402 {
00403 try
00404 {
00405 broadcastFrame();
00406 }
00407 catch(VRGrabException &ex)
00408 {
00409 std::cerr << ex << std::endl;
00410 }
00411 ros::spinOnce();
00412 framesDelivered++;
00413
00414 boost::lock_guard<boost::mutex> lock(timerAccess);
00415 fpsLimit.sleep();
00416 }
00417 }
00418
00419 void forceShutdown(int sig)
00420 {
00421 signal(SIGSEGV, SIG_DFL);
00422 std::cerr << "Segfault, shutting down camera !" << std::endl;
00423
00424 if (vrsnode)
00425 vrsnode->retireCam();
00426 }
00427
00428 int main(int argc, char **argv)
00429 {
00430 ros::init(argc, argv, "vrmagic_stereo_node", ros::init_options::AnonymousName);
00431
00432 signal(SIGSEGV, forceShutdown);
00433
00434 try
00435 {
00436 vrsnode = new VRMagicStereoNode(0);
00437 vrsnode->spin();
00438 }
00439 catch(VRControlException &cex)
00440 {
00441 std::cerr << cex << std::endl;
00442 VRmUsbCamCleanup();
00443 }
00444
00445 delete vrsnode;
00446
00447 return 0;
00448 }
