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 "device_nodelet.h"
00035 #include "publishers/camera_info_publisher.h"
00036 #include "publishers/image_publisher.h"
00037 #include "publishers/disparity_publisher.h"
00038 #include "publishers/disparity_color_publisher.h"
00039 #include "publishers/depth_publisher.h"
00040 #include "publishers/confidence_publisher.h"
00041 #include "publishers/error_disparity_publisher.h"
00042 #include "publishers/error_depth_publisher.h"
00043 #include "publishers/points2_publisher.h"
00044
00045 #include <rc_genicam_api/device.h>
00046 #include <rc_genicam_api/stream.h>
00047 #include <rc_genicam_api/buffer.h>
00048 #include <rc_genicam_api/config.h>
00049
00050 #include <pluginlib/class_list_macros.h>
00051 #include <exception>
00052
00053 #include <rc_genicam_api/pixel_formats.h>
00054
00055 #include <sstream>
00056 #include <stdexcept>
00057
00058 #include <ros/ros.h>
00059 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00060 #include <tf/transform_broadcaster.h>
00061
00062
00063 namespace rc
00064 {
00065
00066 namespace rcd = dynamics;
00067
00068
00069 ThreadedStream::Ptr DeviceNodelet::CreateDynamicsStreamOfType(
00070 rcd::RemoteInterface::Ptr rcdIface,
00071 const std::string &stream, ros::NodeHandle& nh,
00072 const std::string &frame_id_prefix, bool tfEnabled)
00073 {
00074 if (stream=="pose")
00075 {
00076 return ThreadedStream::Ptr(new PoseStream(rcdIface, stream, nh, frame_id_prefix, tfEnabled));
00077 }
00078 if (stream=="pose_ins" || stream=="pose_rt" || stream=="pose_rt_ins" || stream=="imu")
00079 {
00080 return ThreadedStream::Ptr(new Protobuf2RosStream(rcdIface, stream, nh, frame_id_prefix));
00081 }
00082
00083 throw std::runtime_error(std::string("Not yet implemented! Stream type: ") + stream);
00084 }
00085
00086
00087 DeviceNodelet::DeviceNodelet()
00088 {
00089 reconfig=0;
00090 dev_supports_gain=false;
00091 dev_supports_wb=false;
00092 level=0;
00093
00094 stopImageThread = imageRequested = imageSuccess = false;
00095
00096 dynamicsStreams = ThreadedStream::Manager::create();
00097
00098 stopRecoverThread = false;
00099 recoveryRequested = true;
00100 cntConsecutiveRecoveryFails = -1;
00101 }
00102
00103 DeviceNodelet::~DeviceNodelet()
00104 {
00105 std::cout << "rc_visard_driver: Shutting down" << std::endl;
00106
00107
00108
00109 stopImageThread=true;
00110 dynamicsStreams->stop_all();
00111 stopRecoverThread=true;
00112
00113 if (imageThread.joinable()) imageThread.join();
00114 dynamicsStreams->join_all();
00115 if (recoverThread.joinable()) recoverThread.join();
00116
00117 delete reconfig;
00118
00119 rcg::System::clearSystems();
00120 }
00121
00122 void DeviceNodelet::onInit()
00123 {
00124
00125
00126 recoverThread = std::thread(&DeviceNodelet::keepAliveAndRecoverFromFails, this);
00127 }
00128
00129 void DeviceNodelet::keepAliveAndRecoverFromFails()
00130 {
00131
00132
00133 ros::NodeHandle pnh(getPrivateNodeHandle());
00134
00135 std::string device;
00136 std::string access="control";
00137
00138 tfEnabled = false;
00139 autostartDynamics = autostopDynamics = false;
00140 std::string ns = tf::strip_leading_slash(ros::this_node::getNamespace());
00141 tfPrefix = (ns != "") ? ns + "_" : "";
00142
00143 pnh.param("device", device, device);
00144 pnh.param("gev_access", access, access);
00145 pnh.param("enable_tf", tfEnabled, tfEnabled);
00146 pnh.param("autostart_dynamics", autostartDynamics, autostartDynamics);
00147 pnh.param("autostop_dynamics", autostopDynamics, autostopDynamics);
00148
00149 rcg::Device::ACCESS access_id;
00150 if (access == "exclusive")
00151 {
00152 access_id=rcg::Device::EXCLUSIVE;
00153 }
00154 else if (access == "control")
00155 {
00156 access_id=rcg::Device::CONTROL;
00157 }
00158 else if (access == "off")
00159 {
00160 access_id=rcg::Device::READONLY;
00161 }
00162 else
00163 {
00164 ROS_FATAL_STREAM("rc_visard_driver: Access must be 'control', 'exclusive' or 'off': " << access);
00165 return;
00166 }
00167
00168
00169
00170 dynamicsStartService = pnh.advertiseService("startDynamics",
00171 &DeviceNodelet::startDynamics, this);
00172 dynamicsRestartService = pnh.advertiseService("restartDynamics",
00173 &DeviceNodelet::restartDynamics, this);
00174 dynamicsStopService = pnh.advertiseService("stopDynamics",
00175 &DeviceNodelet::stopDynamics, this);
00176
00177
00178
00179 static int maxNumRecoveryTrials = 5;
00180
00181 while (!stopRecoverThread &&
00182 cntConsecutiveRecoveryFails <= maxNumRecoveryTrials)
00183 {
00184
00185
00186 recoveryRequested = recoveryRequested || dynamicsStreams->any_failed();
00187
00188 if (!recoveryRequested)
00189 {
00190 bool allSucceeded = (!imageRequested || imageSuccess);
00191 allSucceeded = allSucceeded && dynamicsStreams->all_succeeded();
00192 if ( (cntConsecutiveRecoveryFails > 0) && allSucceeded )
00193 {
00194 cntConsecutiveRecoveryFails = 0;
00195 ROS_INFO("rc_visard_driver: Device successfully recovered from previous fail(s)!");
00196 }
00197
00198 usleep(1000 * 100);
00199 continue;
00200 }
00201 cntConsecutiveRecoveryFails++;
00202
00203
00204
00205 stopImageThread = true;
00206 dynamicsStreams->stop_all();
00207
00208 if (imageThread.joinable()) imageThread.join();
00209 dynamicsStreams->join_all();
00210
00211
00212
00213 bool successfullyOpened = false;
00214 while (!stopRecoverThread && !successfullyOpened
00215 && cntConsecutiveRecoveryFails <= maxNumRecoveryTrials)
00216 {
00217
00218
00219 if (cntConsecutiveRecoveryFails>0)
00220 {
00221 ROS_ERROR_STREAM("rc_visard_driver: Failed or lost connection. Trying to recover"
00222 " rc_visard_driver from failed state ("
00223 << cntConsecutiveRecoveryFails
00224 << "/" << maxNumRecoveryTrials << ")...");
00225 usleep(1000 * 500);
00226 }
00227
00228 try
00229 {
00230 if (rcgdev)
00231 {
00232 rcgdev->close();
00233 }
00234 rcgdev=rcg::getDevice(device.c_str());
00235 if (!rcgdev)
00236 {
00237 throw std::invalid_argument("Unknown device '" + device + "'");
00238 }
00239
00240 ROS_INFO_STREAM("rc_visard_driver: Opening connection to '" << rcgdev->getID() << "'");
00241 rcgdev->open(access_id);
00242 rcgnodemap=rcgdev->getRemoteNodeMap();
00243
00244 std::string currentIPAddress = rcg::getString(rcgnodemap, "GevCurrentIPAddress", true);
00245 dynamicsInterface = rcd::RemoteInterface::create(currentIPAddress);
00246 if (autostartDynamics)
00247 {
00248 std_srvs::Trigger::Request dummyreq;
00249 std_srvs::Trigger::Response dummyresp;
00250 if (!this->startDynamics(dummyreq, dummyresp))
00251 {
00252 ROS_ERROR("rc_visard_driver: Could not auto-start dynamics module!");
00253 cntConsecutiveRecoveryFails++;
00254 continue;
00255 }
00256 }
00257
00258
00259
00260 auto availStreams = dynamicsInterface->getAvailableStreams();
00261 dynamicsStreams = ThreadedStream::Manager::create();
00262 for (const auto &streamName : availStreams)
00263 {
00264 try
00265 {
00266 if (streamName != "dynamics" && streamName != "dynamics_ins")
00267 {
00268 auto newStream = CreateDynamicsStreamOfType(dynamicsInterface, streamName,
00269 getNodeHandle(), tfPrefix, tfEnabled);
00270 dynamicsStreams->add(newStream);
00271 }
00272 else
00273 {
00274 ROS_INFO_STREAM("Unsupported dynamics stream: " << streamName);
00275 }
00276 } catch(const std::exception &e)
00277 {
00278 ROS_WARN_STREAM("Unable to create dynamics stream of type "
00279 << streamName << ": " << e.what());
00280 }
00281 }
00282
00283 successfullyOpened = true;
00284 } catch (std::exception &ex)
00285 {
00286 cntConsecutiveRecoveryFails++;
00287 ROS_ERROR_STREAM("rc_visard_driver: " << ex.what());
00288 }
00289 }
00290 if (stopRecoverThread) break;
00291
00292 if (cntConsecutiveRecoveryFails > maxNumRecoveryTrials)
00293 {
00294 ROS_FATAL_STREAM("rc_visard_driver: could not recover from failed state!");
00295 break;
00296 }
00297
00298
00299
00300 recoveryRequested = false;
00301 if (access_id != rcg::Device::READONLY)
00302 {
00303 imageThread = std::thread(&DeviceNodelet::grab, this, device, access_id);
00304 }
00305 dynamicsStreams->start_all();
00306 }
00307
00308 if (autostopDynamics)
00309 {
00310 std_srvs::Trigger::Request dummyreq;
00311 std_srvs::Trigger::Response dummyresp;
00312 if (!this->stopDynamics(dummyreq, dummyresp))
00313 {
00314 ROS_ERROR("rc_visard_driver: Could not auto-stop dynamics module!");
00315 }
00316 }
00317 std::cout << "rc_visard_driver: stopped." << std::endl;
00318 }
00319
00320 void DeviceNodelet::initConfiguration(const std::shared_ptr<GenApi::CNodeMapRef> &nodemap,
00321 rc_visard_driver::rc_visard_driverConfig &cfg, rcg::Device::ACCESS access)
00322 {
00323 ros::NodeHandle pnh(getPrivateNodeHandle());
00324
00325
00326
00327 cfg.camera_fps=rcg::getFloat(nodemap, "AcquisitionFrameRate", 0, 0, true);
00328
00329 std::string v=rcg::getEnum(nodemap, "ExposureAuto", true);
00330 cfg.camera_exp_auto=(v != "Off");
00331
00332 cfg.camera_exp_value=rcg::getFloat(nodemap, "ExposureTime", 0, 0, true)/1000000;
00333 cfg.camera_exp_max=rcg::getFloat(nodemap, "ExposureTimeAutoMax", 0, 0, true)/1000000;
00334
00335
00336
00337 v=rcg::getEnum(nodemap, "GainSelector", false);
00338 if (v.size() > 0)
00339 {
00340 dev_supports_gain=true;
00341
00342 if (v != "All")
00343 {
00344 dev_supports_gain=rcg::setEnum(nodemap, "GainSelector", "All", true);
00345 }
00346
00347 if(dev_supports_gain)
00348 {
00349 cfg.camera_gain_value=rcg::getFloat(nodemap, "Gain", 0, 0, true);
00350 }
00351 }
00352 else
00353 {
00354 ROS_WARN("rc_visard_driver: Device does not support setting gain. gain_value is without function.");
00355
00356 dev_supports_gain=false;
00357 cfg.camera_gain_value=0;
00358 }
00359
00360
00361
00362 v=rcg::getEnum(nodemap, "BalanceWhiteAuto", false);
00363 if (v.size() > 0)
00364 {
00365 dev_supports_wb=true;
00366 cfg.camera_wb_auto=(v != "Off");
00367 rcg::setEnum(nodemap, "BalanceRatioSelector", "Red", true);
00368 cfg.camera_wb_ratio_red=rcg::getFloat(nodemap, "BalanceRatio", 0, 0, true);
00369 rcg::setEnum(nodemap, "BalanceRatioSelector", "Blue", true);
00370 cfg.camera_wb_ratio_blue=rcg::getFloat(nodemap, "BalanceRatio", 0, 0, true);
00371 }
00372 else
00373 {
00374 ROS_WARN("rc_visard_driver: Not a color camera. wb_auto, wb_ratio_red and wb_ratio_blue are without function.");
00375
00376 dev_supports_wb=false;
00377 cfg.camera_wb_auto=true;
00378 cfg.camera_wb_ratio_red=1;
00379 cfg.camera_wb_ratio_blue=1;
00380 }
00381
00382
00383
00384 v=rcg::getEnum(nodemap, "DepthQuality", true);
00385 cfg.depth_quality=v.substr(0, 1);
00386
00387 cfg.depth_disprange=rcg::getInteger(nodemap, "DepthDispRange", 0, 0, true);
00388 cfg.depth_seg=rcg::getInteger(nodemap, "DepthSeg", 0, 0, true);
00389 cfg.depth_median=rcg::getInteger(nodemap, "DepthMedian", 0, 0, true);
00390 cfg.depth_fill=rcg::getInteger(nodemap, "DepthFill", 0, 0, true);
00391 cfg.depth_minconf=rcg::getFloat(nodemap, "DepthMinConf", 0, 0, true);
00392 cfg.depth_mindepth=rcg::getFloat(nodemap, "DepthMinDepth", 0, 0, true);
00393 cfg.depth_maxdepth=rcg::getFloat(nodemap, "DepthMaxDepth", 0, 0, true);
00394 cfg.depth_maxdeptherr=rcg::getFloat(nodemap, "DepthMaxDepthErr", 0, 0, true);
00395
00396
00397
00398 if (reconfig == 0)
00399 {
00400
00401
00402 pnh.setParam("camera_fps", cfg.camera_fps);
00403 pnh.setParam("camera_exp_auto", cfg.camera_exp_auto);
00404 pnh.setParam("camera_exp_value", cfg.camera_exp_value);
00405 pnh.setParam("camera_gain_value", cfg.camera_gain_value);
00406 pnh.setParam("camera_exp_max", cfg.camera_exp_max);
00407 pnh.setParam("camera_wb_auto", cfg.camera_wb_auto);
00408 pnh.setParam("camera_wb_ratio_red", cfg.camera_wb_ratio_red);
00409 pnh.setParam("camera_wb_ratio_blue", cfg.camera_wb_ratio_blue);
00410 pnh.setParam("depth_quality", cfg.depth_quality);
00411 pnh.setParam("depth_disprange", cfg.depth_disprange);
00412 pnh.setParam("depth_seg", cfg.depth_seg);
00413 pnh.setParam("depth_median", cfg.depth_median);
00414 pnh.setParam("depth_fill", cfg.depth_fill);
00415 pnh.setParam("depth_minconf", cfg.depth_minconf);
00416 pnh.setParam("depth_mindepth", cfg.depth_mindepth);
00417 pnh.setParam("depth_maxdepth", cfg.depth_maxdepth);
00418 pnh.setParam("depth_maxdeptherr", cfg.depth_maxdeptherr);
00419
00420
00421 reconfig=new dynamic_reconfigure::Server<rc_visard_driver::rc_visard_driverConfig>(pnh);
00422 dynamic_reconfigure::Server<rc_visard_driver::rc_visard_driverConfig>::CallbackType cb;
00423 cb=boost::bind(&DeviceNodelet::reconfigure, this, _1, _2);
00424 reconfig->setCallback(cb);
00425 }
00426 }
00427
00428 void DeviceNodelet::reconfigure(rc_visard_driver::rc_visard_driverConfig &c, uint32_t l)
00429 {
00430 mtx.lock();
00431
00432
00433
00434 if (!dev_supports_gain)
00435 {
00436 c.camera_gain_value=0;
00437 l&=~8192;
00438 }
00439
00440 c.camera_gain_value=round(c.camera_gain_value/6)*6;
00441
00442 if (!dev_supports_wb)
00443 {
00444 c.camera_wb_auto=true;
00445 c.camera_wb_ratio_red=1;
00446 c.camera_wb_ratio_blue=1;
00447 l&=~(16384|32768|65536);
00448 }
00449
00450 c.depth_quality=c.depth_quality.substr(0, 1);
00451
00452 if (c.depth_quality[0] != 'L' && c.depth_quality[0] != 'M' && c.depth_quality[0] != 'H' &&
00453 c.depth_quality[0] != 'S')
00454 {
00455 c.depth_quality="H";
00456 }
00457
00458
00459
00460 config=c;
00461 level|=l;
00462
00463 mtx.unlock();
00464 }
00465
00466 namespace
00467 {
00468
00469
00470
00471
00472
00473 void setConfiguration(const std::shared_ptr<GenApi::CNodeMapRef> &nodemap,
00474 const rc_visard_driver::rc_visard_driverConfig &cfg, uint32_t lvl)
00475 {
00476 uint32_t prev_lvl=0;
00477
00478 while (lvl != 0 && prev_lvl != lvl)
00479 {
00480 prev_lvl=lvl;
00481
00482 try
00483 {
00484
00485
00486
00487
00488 if (lvl&1)
00489 {
00490 lvl&=~1;
00491 rcg::setFloat(nodemap, "AcquisitionFrameRate", cfg.camera_fps, true);
00492 }
00493
00494 if (lvl&2)
00495 {
00496 lvl&=~2;
00497
00498 if (cfg.camera_exp_auto)
00499 {
00500 rcg::setEnum(nodemap, "ExposureAuto", "Continuous", true);
00501 }
00502 else
00503 {
00504 rcg::setEnum(nodemap, "ExposureAuto", "Off", true);
00505 }
00506 }
00507
00508 if (lvl&4)
00509 {
00510 lvl&=~4;
00511 rcg::setFloat(nodemap, "ExposureTime", 1000000*cfg.camera_exp_value, true);
00512 }
00513
00514 if (lvl&8192)
00515 {
00516 lvl&=~8192;
00517 rcg::setFloat(nodemap, "Gain", cfg.camera_gain_value, true);
00518 }
00519
00520 if (lvl&8)
00521 {
00522 lvl&=~8;
00523 rcg::setFloat(nodemap, "ExposureTimeAutoMax", 1000000*cfg.camera_exp_max, true);
00524 }
00525
00526 if (lvl&16384)
00527 {
00528 lvl&=~16384;
00529
00530 if (cfg.camera_wb_auto)
00531 {
00532 rcg::setEnum(nodemap, "BalanceWhiteAuto", "Continuous", true);
00533 }
00534 else
00535 {
00536 rcg::setEnum(nodemap, "BalanceWhiteAuto", "Off", true);
00537 }
00538 }
00539
00540 if (lvl&32768)
00541 {
00542 lvl&=~32768;
00543 rcg::setEnum(nodemap, "BalanceRatioSelector", "Red", true);
00544 rcg::setFloat(nodemap, "BalanceRatio", cfg.camera_wb_ratio_red, true);
00545 }
00546
00547 if (lvl&65536)
00548 {
00549 lvl&=~65536;
00550 rcg::setEnum(nodemap, "BalanceRatioSelector", "Blue", true);
00551 rcg::setFloat(nodemap, "BalanceRatio", cfg.camera_wb_ratio_blue, true);
00552 }
00553
00554 if (lvl&16)
00555 {
00556 lvl&=~16;
00557
00558 std::vector<std::string> list;
00559 rcg::getEnum(nodemap, "DepthQuality", list, true);
00560
00561 std::string val;
00562 for (size_t i=0; i<list.size(); i++)
00563 {
00564 if (list[i].compare(0, 1, cfg.depth_quality, 0, 1) == 0)
00565 {
00566 val=list[i];
00567 }
00568 }
00569
00570 if (val.size() > 0)
00571 {
00572 rcg::setEnum(nodemap, "DepthQuality", val.c_str(), true);
00573 }
00574 }
00575
00576 if (lvl&32)
00577 {
00578 lvl&=~32;
00579 rcg::setInteger(nodemap, "DepthDispRange", cfg.depth_disprange, true);
00580 }
00581
00582 if (lvl&64)
00583 {
00584 lvl&=~64;
00585 rcg::setInteger(nodemap, "DepthSeg", cfg.depth_seg, true);
00586 }
00587
00588 if (lvl&128)
00589 {
00590 lvl&=~128;
00591 rcg::setInteger(nodemap, "DepthMedian", cfg.depth_median, true);
00592 }
00593
00594 if (lvl&256)
00595 {
00596 lvl&=~256;
00597 rcg::setInteger(nodemap, "DepthFill", cfg.depth_fill, true);
00598 }
00599
00600 if (lvl&512)
00601 {
00602 lvl&=~512;
00603 rcg::setFloat(nodemap, "DepthMinConf", cfg.depth_minconf, true);
00604 }
00605
00606 if (lvl&1024)
00607 {
00608 lvl&=~1024;
00609 rcg::setFloat(nodemap, "DepthMinDepth", cfg.depth_mindepth, true);
00610 }
00611
00612 if (lvl&2048)
00613 {
00614 lvl&=~2048;
00615 rcg::setFloat(nodemap, "DepthMaxDepth", cfg.depth_maxdepth, true);
00616 }
00617
00618 if (lvl&4096)
00619 {
00620 lvl&=~4096;
00621 rcg::setFloat(nodemap, "DepthMaxDepthErr", cfg.depth_maxdeptherr, true);
00622 }
00623 }
00624 catch (const std::exception &ex)
00625 {
00626 ROS_ERROR_STREAM("rc_visard_driver: " << ex.what());
00627 }
00628 }
00629 }
00630
00631
00632
00633
00634
00635
00636
00637 void disableAll(const std::shared_ptr<GenApi::CNodeMapRef> &nodemap)
00638 {
00639 std::vector<std::string> component;
00640
00641 rcg::getEnum(nodemap, "ComponentSelector", component, true);
00642
00643 for (size_t i=0; i<component.size(); i++)
00644 {
00645 rcg::setEnum(nodemap, "ComponentSelector", component[i].c_str(), true);
00646 rcg::setBoolean(nodemap, "ComponentEnable", false, true);
00647 }
00648
00649 rcg::setEnum(nodemap, "PixelFormat", "Mono8");
00650 }
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661 int enable(const std::shared_ptr<GenApi::CNodeMapRef> &nodemap, const char *component,
00662 bool &en_curr, bool en_new)
00663 {
00664 if (en_new != en_curr)
00665 {
00666 if (en_new)
00667 ROS_INFO_STREAM("rc_visard_driver: Enabled image stream: " << component);
00668 else
00669 ROS_INFO_STREAM("rc_visard_driver: Disabled image stream: " << component);
00670
00671 rcg::setEnum(nodemap, "ComponentSelector", component, true);
00672 rcg::setBoolean(nodemap, "ComponentEnable", en_new);
00673 en_curr=en_new;
00674
00675 return 1;
00676 }
00677
00678 return 0;
00679 }
00680
00681 }
00682
00683 void DeviceNodelet::grab(std::string device, rcg::Device::ACCESS access)
00684 {
00685 unsigned int maxNumConsecutiveFails = 5;
00686
00687 stopImageThread=false;
00688
00689 imageRequested = true;
00690 imageSuccess = false;
00691
00692
00693
00694 unsigned int cntConsecutiveFails = 0;
00695 while (!stopImageThread && cntConsecutiveFails < maxNumConsecutiveFails)
00696 {
00697
00698 imageSuccess = false;
00699 try
00700 {
00701
00702
00703 disableAll(rcgnodemap);
00704
00705 bool ccolor=false;
00706 bool cintensity=false;
00707 bool cintensitycombined=false;
00708 bool cdisparity=false;
00709 bool cconfidence=false;
00710 bool cerror=false;
00711
00712 bool firstTime = true;
00713
00714
00715
00716 double f=rcg::getFloat(rcgnodemap, "FocalLengthFactor", 0, 0, false);
00717 if (!f)
00718 {
00719 f=rcg::getFloat(rcgnodemap, "FocalLength", 0, 0, false);
00720 if (!f)
00721 {
00722 throw std::runtime_error("Focal length not found: Neither 'FocalLength' nor 'FocalLengthFactor'!");
00723 }
00724 }
00725
00726
00727
00728 double t=rcg::getFloat(rcgnodemap, "Baseline", 0, 0, true);
00729 GenApi::INode *node=rcgnodemap->_GetNode("Baseline");
00730 if (!node || !GenApi::IsReadable(node))
00731 {
00732 throw std::invalid_argument(
00733 "Feature not found or not readable: Baseline");
00734 }
00735 GenApi::IFloat *val=dynamic_cast<GenApi::IFloat *>(node);
00736 if (!val)
00737 {
00738 throw std::invalid_argument("Feature not float: Baseline");
00739 }
00740 if (val->GetUnit()=="mm")
00741 {
00742 t /= 1000;
00743 }
00744
00745
00746
00747 double scale=rcg::getFloat(rcgnodemap, "Scan3dCoordinateScale", 0, 0, true);
00748
00749
00750
00751 rcg::checkFeature(rcgnodemap, "Scan3dOutputMode", "DisparityC");
00752 rcg::checkFeature(rcgnodemap, "Scan3dCoordinateOffset", "0");
00753 rcg::checkFeature(rcgnodemap, "Scan3dInvalidDataFlag", "1");
00754 rcg::checkFeature(rcgnodemap, "Scan3dInvalidDataValue", "0");
00755
00756
00757
00758 initConfiguration(rcgnodemap, config, access);
00759
00760 int disprange=config.depth_disprange;
00761
00762
00763
00764 ros::NodeHandle nh(getNodeHandle(), "stereo");
00765 image_transport::ImageTransport it(nh);
00766
00767 CameraInfoPublisher lcaminfo(nh, tfPrefix, f, t, true);
00768 CameraInfoPublisher rcaminfo(nh, tfPrefix, f, t, false);
00769
00770 ImagePublisher limage(it, tfPrefix, true, false);
00771 ImagePublisher rimage(it, tfPrefix, false, false);
00772
00773 DisparityPublisher disp(nh, tfPrefix, f, t, scale);
00774 DisparityColorPublisher cdisp(it, tfPrefix, scale);
00775 DepthPublisher depth(nh, tfPrefix, f, t, scale);
00776
00777 ConfidencePublisher confidence(nh, tfPrefix);
00778 ErrorDisparityPublisher error_disp(nh, tfPrefix, scale);
00779 ErrorDepthPublisher error_depth(nh, tfPrefix, f, t, scale);
00780
00781 Points2Publisher points2(nh, tfPrefix, f, t, scale);
00782
00783
00784
00785 std::shared_ptr<GenICam2RosPublisher> limage_color;
00786 std::shared_ptr<GenICam2RosPublisher> rimage_color;
00787
00788 {
00789 std::vector<std::string> format;
00790 rcg::getEnum(rcgnodemap, "PixelFormat", format, true);
00791
00792 for (size_t i=0; i<format.size(); i++)
00793 {
00794 if (format[i] == "YCbCr411_8")
00795 {
00796 limage_color=std::make_shared<ImagePublisher>(it, tfPrefix, true, true);
00797 rimage_color=std::make_shared<ImagePublisher>(it, tfPrefix, false, true);
00798 break;
00799 }
00800 }
00801 }
00802
00803
00804
00805 std::vector<std::shared_ptr<rcg::Stream> > stream=rcgdev->getStreams();
00806
00807 if (stream.size() > 0)
00808 {
00809 stream[0]->open();
00810 stream[0]->startStreaming();
00811
00812 ROS_INFO_STREAM("rc_visard_driver: Image streams ready (Package size " <<
00813 rcg::getString(rcgnodemap, "GevSCPSPacketSize") << ")");
00814
00815
00816
00817 int missing=0;
00818 ros::Time tlastimage=ros::Time::now();
00819
00820 while (!stopImageThread)
00821 {
00822 const rcg::Buffer *buffer=stream[0]->grab(500);
00823
00824 if (buffer != 0 && !buffer->getIsIncomplete() && buffer->getImagePresent())
00825 {
00826
00827
00828 missing=0;
00829 tlastimage=ros::Time::now();
00830 cntConsecutiveFails=0;
00831 imageSuccess = true;
00832
00833
00834
00835 disp.setDisprange(disprange);
00836 cdisp.setDisprange(disprange);
00837
00838 uint64_t pixelformat=buffer->getPixelFormat();
00839
00840 lcaminfo.publish(buffer, pixelformat);
00841 rcaminfo.publish(buffer, pixelformat);
00842
00843 limage.publish(buffer, pixelformat);
00844 rimage.publish(buffer, pixelformat);
00845
00846 if (limage_color && rimage_color)
00847 {
00848 limage_color->publish(buffer, pixelformat);
00849 rimage_color->publish(buffer, pixelformat);
00850 }
00851
00852 disp.publish(buffer, pixelformat);
00853 cdisp.publish(buffer, pixelformat);
00854 depth.publish(buffer, pixelformat);
00855
00856 confidence.publish(buffer, pixelformat);
00857 error_disp.publish(buffer, pixelformat);
00858 error_depth.publish(buffer, pixelformat);
00859
00860 points2.publish(buffer, pixelformat);
00861 }
00862 else if (buffer != 0 && buffer->getIsIncomplete())
00863 {
00864 missing=0;
00865 ROS_WARN("rc_visard_driver: Received incomplete image buffer");
00866 }
00867 else if (buffer == 0)
00868 {
00869
00870
00871
00872 if (cintensity || cintensitycombined || cdisparity || cconfidence || cerror)
00873 {
00874 missing++;
00875 if (missing >= 6)
00876 {
00877 std::ostringstream out;
00878
00879 out << "No images received for ";
00880 out << (ros::Time::now()-tlastimage).toSec();
00881 out << " seconds!";
00882
00883 throw std::underflow_error(out.str());
00884 }
00885 }
00886 }
00887
00888
00889
00890
00891
00892
00893 if (limage_color && rimage_color && (limage_color->used() || rimage_color->used() ||
00894 points2.used()))
00895 {
00896 if (!ccolor)
00897 {
00898 rcg::setEnum(rcgnodemap, "PixelFormat", "YCbCr411_8", true);
00899 ccolor=true;
00900 }
00901 }
00902 else
00903 {
00904 if (ccolor)
00905 {
00906 rcg::setEnum(rcgnodemap, "PixelFormat", "Mono8", true);
00907 ccolor=false;
00908 }
00909 }
00910
00911
00912
00913 int changed=enable(rcgnodemap, "IntensityCombined", cintensitycombined,
00914 rimage.used() || (rimage_color && rimage_color->used()));
00915
00916 changed+=enable(rcgnodemap, "Intensity", cintensity, !cintensitycombined &&
00917 (lcaminfo.used() || rcaminfo.used() || limage.used() ||
00918 (limage_color && limage_color->used()) || points2.used()));
00919
00920 changed+=enable(rcgnodemap, "Disparity", cdisparity, disp.used() || cdisp.used() ||
00921 depth.used() || error_depth.used() || points2.used());
00922
00923 changed+=enable(rcgnodemap, "Confidence", cconfidence, confidence.used());
00924
00925 changed+=enable(rcgnodemap, "Error", cerror, error_disp.used() || error_depth.used());
00926
00927
00928
00929
00930 if (changed > 0 || firstTime)
00931 {
00932 if (cintensity || cintensitycombined || cdisparity || cconfidence || cerror)
00933 imageRequested = true;
00934 else
00935 imageRequested = false;
00936 firstTime = false;
00937 }
00938
00939
00940
00941 if (level != 0)
00942 {
00943
00944
00945 mtx.lock();
00946 rc_visard_driver::rc_visard_driverConfig cfg=config;
00947 uint32_t lvl=level;
00948 level=0;
00949 mtx.unlock();
00950
00951 setConfiguration(rcgnodemap, cfg, lvl);
00952
00953 disprange=cfg.depth_disprange;
00954 }
00955 }
00956
00957 stream[0]->stopStreaming();
00958 stream[0]->close();
00959 }
00960 else
00961 {
00962 ROS_ERROR("rc_visard_driver: No stream");
00963 cntConsecutiveFails++;
00964 }
00965 }
00966 catch (const std::exception &ex)
00967 {
00968 ROS_ERROR_STREAM("rc_visard_driver: " << ex.what());
00969 cntConsecutiveFails++;
00970 }
00971 }
00972
00973
00974
00975 if (cntConsecutiveFails > 0)
00976 {
00977 ROS_ERROR_STREAM("rc_visard_driver: Number of consecutive failures: " << cntConsecutiveFails);
00978 }
00979 if (cntConsecutiveFails >= maxNumConsecutiveFails)
00980 {
00981 ROS_ERROR_STREAM("rc_visard_driver: Image grabbing failed.");
00982 recoveryRequested = true;
00983 }
00984
00985 }
00986
00987 void handleDynamicsStateChangeRequest(
00988 rcd::RemoteInterface::Ptr dynIF,
00989 int state, std_srvs::Trigger::Response &resp)
00990 {
00991 resp.success = true;
00992 resp.message = "";
00993
00994 if (dynIF)
00995 {
00996 try
00997 {
00998 switch (state) {
00999 case 0:
01000 dynIF->stop();
01001 break;
01002 case 1:
01003 dynIF->start(false);
01004 break;
01005 case 2:
01006 dynIF->start(true);
01007 break;
01008 default:
01009 throw std::runtime_error("handleDynamicsStateChangeRequest: unrecognized state change request");
01010 }
01011 }
01012 catch (std::exception &e)
01013 {
01014 resp.success = false;
01015 resp.message = std::string("Failed to change state of rcdynamics module: ") + e.what();
01016 }
01017 }
01018 else
01019 {
01020 resp.success = false;
01021 resp.message = "rcdynamics remote interface not yet initialized!";
01022 }
01023
01024 if (!resp.success) ROS_ERROR_STREAM(resp.message);
01025 }
01026
01027 bool DeviceNodelet::startDynamics(std_srvs::Trigger::Request &req,
01028 std_srvs::Trigger::Response &resp){
01029 handleDynamicsStateChangeRequest(dynamicsInterface, 1, resp);
01030 return true;
01031 }
01032
01033 bool DeviceNodelet::restartDynamics(std_srvs::Trigger::Request &req,
01034 std_srvs::Trigger::Response &resp){
01035 handleDynamicsStateChangeRequest(dynamicsInterface, 2, resp);
01036 return true;
01037 }
01038
01039 bool DeviceNodelet::stopDynamics(std_srvs::Trigger::Request &req,
01040 std_srvs::Trigger::Response &resp){
01041 handleDynamicsStateChangeRequest(dynamicsInterface, 0, resp);
01042 return true;
01043 }
01044
01045 }
01046
01047 PLUGINLIB_EXPORT_CLASS(rc::DeviceNodelet, nodelet::Nodelet)