vrmstnode.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, TU Darmstadt.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of TU Darmstadt nor the names of the
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


vrmagic_multi_driver
Author(s): Philipp Schneider, Stefan Kohlbrecher
autogenerated on Mon Jul 15 2013 16:57:01