reconfigure.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/reconfigure.h>
00035 
00036 using namespace crl::multisense;
00037 
00038 namespace multisense_ros {
00039 
00040 Reconfigure::Reconfigure(Channel* driver,
00041                          boost::function<void ()> resolutionChangeCallback,
00042                          boost::function<void (int, int)> borderClipChangeCallback) :
00043     driver_(driver),
00044     resolution_change_callback_(resolutionChangeCallback),
00045     device_nh_(""),
00046     device_modes_(),
00047     imu_samples_per_message_(0),
00048     imu_configs_(),
00049     server_sl_bm_cmv2000_(),
00050     server_sl_bm_cmv2000_imu_(),
00051     server_sl_bm_cmv4000_(),
00052     server_sl_bm_cmv4000_imu_(),
00053     server_sl_sgm_cmv2000_imu_(),
00054     server_sl_sgm_cmv4000_imu_(),
00055     server_bcam_imx104_(),
00056     server_st21_vga_(),
00057     lighting_supported_(false),
00058     motor_supported_(false),
00059     border_clip_type_(RECTANGULAR),
00060     border_clip_value_(0.0),
00061     border_clip_change_callback_(borderClipChangeCallback),
00062     crop_mode_changed_(false)
00063 {
00064     system::DeviceInfo  deviceInfo;
00065     system::VersionInfo versionInfo;
00066 
00067     //
00068     // Query device and version information from sensor
00069 
00070     Status status = driver_->getVersionInfo(versionInfo);
00071     if (Status_Ok != status) {
00072         ROS_ERROR("Reconfigure: failed to query version info: %s",
00073                   Channel::statusString(status));
00074         return;
00075     }
00076     status = driver_->getDeviceInfo(deviceInfo);
00077     if (Status_Ok != status) {
00078         ROS_ERROR("Reconfigure: failed to query device info: %s",
00079                   Channel::statusString(status));
00080         return;
00081     }
00082 
00083     if (deviceInfo.lightingType != 0)
00084     {
00085         lighting_supported_ = true;
00086     }
00087     if (deviceInfo.motorType != 0)
00088     {
00089         motor_supported_ = true;
00090     }
00091 
00092     //
00093     // Launch the correct reconfigure server for this device configuration.
00094 
00095     if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
00096 
00097         server_bcam_imx104_ =
00098             boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::bcam_imx104Config> > (
00099                 new dynamic_reconfigure::Server<multisense_ros::bcam_imx104Config>(device_nh_));
00100         server_bcam_imx104_->setCallback(boost::bind(&Reconfigure::callback_bcam_imx104, this, _1, _2));
00101 
00102     } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == deviceInfo.hardwareRevision) {
00103 
00104         server_st21_vga_ =
00105             boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::st21_sgm_vga_imuConfig> > (
00106                 new dynamic_reconfigure::Server<multisense_ros::st21_sgm_vga_imuConfig>(device_nh_));
00107         server_st21_vga_->setCallback(boost::bind(&Reconfigure::callback_st21_vga, this, _1, _2));
00108 
00109     } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == deviceInfo.hardwareRevision) {
00110 
00111         switch(deviceInfo.imagerType) {
00112         case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
00113         case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
00114 
00115             server_mono_cmv2000_ =
00116                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::mono_cmv2000Config> > (
00117                     new dynamic_reconfigure::Server<multisense_ros::mono_cmv2000Config>(device_nh_));
00118             server_mono_cmv2000_->setCallback(boost::bind(&Reconfigure::callback_mono_cmv2000, this, _1, _2));
00119 
00120             break;
00121         case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
00122         case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
00123 
00124             server_mono_cmv4000_ =
00125                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::mono_cmv4000Config> > (
00126                     new dynamic_reconfigure::Server<multisense_ros::mono_cmv4000Config>(device_nh_));
00127             server_mono_cmv4000_->setCallback(boost::bind(&Reconfigure::callback_mono_cmv4000, this, _1, _2));
00128 
00129             break;
00130         }
00131 
00132     } else if (versionInfo.sensorFirmwareVersion <= 0x0202) {
00133 
00134         switch(deviceInfo.imagerType) {
00135         case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
00136         case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
00137 
00138             server_sl_bm_cmv2000_ =
00139                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000Config> > (
00140                     new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000Config>(device_nh_));
00141             server_sl_bm_cmv2000_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv2000, this, _1, _2));
00142 
00143             break;
00144         case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
00145         case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
00146 
00147             server_sl_bm_cmv4000_ =
00148                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000Config> > (
00149                     new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000Config>(device_nh_));
00150             server_sl_bm_cmv4000_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv4000, this, _1, _2));
00151 
00152             break;
00153         default:
00154 
00155             ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
00156             return;
00157         }
00158 
00159     } else if (versionInfo.sensorFirmwareVersion < 0x0300) {
00160 
00161         switch(deviceInfo.imagerType) {
00162         case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
00163         case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
00164 
00165             server_sl_bm_cmv2000_imu_ =
00166                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000_imuConfig> > (
00167                     new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000_imuConfig>(device_nh_));
00168             server_sl_bm_cmv2000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv2000_imu, this, _1, _2));
00169 
00170             break;
00171         case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
00172         case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
00173 
00174             server_sl_bm_cmv4000_imu_ =
00175                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000_imuConfig> > (
00176                     new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000_imuConfig>(device_nh_));
00177             server_sl_bm_cmv4000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv4000_imu, this, _1, _2));
00178 
00179             break;
00180         default:
00181 
00182             ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
00183             return;
00184         }
00185 
00186     } else {
00187 
00188         switch(deviceInfo.imagerType) {
00189         case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
00190         case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
00191 
00192             server_sl_sgm_cmv2000_imu_ =
00193                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv2000_imuConfig> > (
00194                     new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv2000_imuConfig>(device_nh_));
00195             server_sl_sgm_cmv2000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_sgm_cmv2000_imu, this, _1, _2));
00196 
00197             break;
00198         case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
00199         case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
00200 
00201             server_sl_sgm_cmv4000_imu_ =
00202                 boost::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv4000_imuConfig> > (
00203                     new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv4000_imuConfig>(device_nh_));
00204             server_sl_sgm_cmv4000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_sgm_cmv4000_imu, this, _1, _2));
00205             break;
00206         default:
00207 
00208             ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
00209             return;
00210         }
00211     }
00212 }
00213 
00214 Reconfigure::~Reconfigure()
00215 {
00216 }
00217 
00218 //
00219 // Helper to change resolution. Will check against supported device modes
00220 
00221 bool Reconfigure::changeResolution(image::Config& cfg,
00222                                    int32_t        width,
00223                                    int32_t        height,
00224                                    int32_t        disparities)
00225 {
00226     //
00227     // See if we need to change resolutions
00228 
00229     if (width       == static_cast<int32_t>(cfg.width())   &&
00230         height      == static_cast<int32_t>(cfg.height())  &&
00231         disparities == static_cast<int32_t>(cfg.disparities()))
00232         return false;
00233 
00234     //
00235     // Query all supported resolutions from the sensor, if we haven't already
00236 
00237     if (device_modes_.empty()) {
00238 
00239         Status status = driver_->getDeviceModes(device_modes_);
00240         if (Status_Ok != status) {
00241             ROS_ERROR("Reconfigure: failed to query sensor modes: %s",
00242                       Channel::statusString(status));
00243             return false;
00244         }
00245     }
00246 
00247     //
00248     // Verify that this resolution is supported
00249 
00250     bool supported = false;
00251     std::vector<system::DeviceMode>::const_iterator it = device_modes_.begin();
00252     for(; it != device_modes_.end(); ++it) {
00253 
00254         const system::DeviceMode& m = *it;
00255 
00256         if (width       == static_cast<int32_t>(m.width)  &&
00257             height      == static_cast<int32_t>(m.height) &&
00258             disparities == static_cast<int32_t>(m.disparities)) {
00259 
00260             supported = true;
00261             break;
00262         }
00263     }
00264 
00265     if (false == supported) {
00266         ROS_ERROR("Reconfigure: sensor does not support a resolution of: %dx%d (%d disparities)",
00267                   width, height, disparities);
00268         return false;
00269     }
00270 
00271     ROS_WARN("Reconfigure: changing sensor resolution to %dx%d (%d disparities), from %dx%d "
00272          "(%d disparities): reconfiguration may take up to 30 seconds",
00273              width, height, disparities,
00274              cfg.width(), cfg.height(), cfg.disparities());
00275 
00276     cfg.setResolution(width, height);
00277     cfg.setDisparities(disparities);
00278 
00279     return true;
00280 }
00281 
00282 template<class T> void Reconfigure::configureCropMode(crl::multisense::image::Config& cfg, const T& dyn)
00283 {
00284     cfg.setCamMode(dyn.crop_mode == 1 ? 2000 : 4000);
00285     cfg.setOffset(dyn.crop_offset);
00286     ROS_WARN("Reconfigure: changing cam mode to %s with offset %d: reconfiguration may take up to 30 seconds",
00287              dyn.crop_mode == 1 ? "ON" : "OFF" , cfg.offset());
00288     crop_mode_changed_ = true;
00289 }
00290 
00291 template<class T> void Reconfigure::configureSgm(image::Config& cfg, const T& dyn)
00292 {
00293     cfg.setStereoPostFilterStrength(dyn.stereo_post_filtering);
00294 }
00295 
00296 template<class T> void Reconfigure::configureCamera(image::Config& cfg, const T& dyn)
00297 {
00298     DataSource    streamsEnabled = 0;
00299     int32_t       width, height, disparities;
00300     bool          resolutionChange=false;
00301     Status        status=Status_Ok;
00302 
00303     //
00304     // Decode the resolution string
00305 
00306     if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
00307         ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
00308         return;
00309     }
00310 
00311     //
00312     // If a resolution change is desired
00313 
00314     if ((resolutionChange = changeResolution(cfg, width, height, disparities) || crop_mode_changed_)) {
00315         crop_mode_changed_ = false;
00316         //
00317         // Halt streams during the resolution change
00318         status = driver_->getEnabledStreams(streamsEnabled);
00319         if (Status_Ok != status) {
00320             ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
00321                       Channel::statusString(status));
00322             return;
00323         }
00324         status = driver_->stopStreams(streamsEnabled);
00325         if (Status_Ok != status) {
00326             ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
00327                       Channel::statusString(status));
00328             return;
00329         }
00330     }
00331 
00332     //
00333     // Set all other image config from dynamic reconfigure
00334 
00335     cfg.setFps(dyn.fps);
00336     cfg.setGain(dyn.gain);
00337     cfg.setExposure(dyn.exposure_time * 1e6);
00338     cfg.setAutoExposure(dyn.auto_exposure);
00339     cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
00340     cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
00341     cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
00342     cfg.setWhiteBalance(dyn.white_balance_red,
00343                         dyn.white_balance_blue);
00344     cfg.setAutoWhiteBalance(dyn.auto_white_balance);
00345     cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
00346     cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);
00347     cfg.setHdr(dyn.hdr_enable);
00348 
00349     //
00350     // Apply, sensor enforces limits per setting.
00351 
00352     status = driver_->setImageConfig(cfg);
00353     if (Status_Ok != status)
00354         ROS_ERROR("Reconfigure: failed to set image config: %s",
00355                   Channel::statusString(status));
00356 
00357     //
00358     // If we are changing the resolution, let others know about it
00359 
00360     if (resolutionChange) {
00361         if (false == resolution_change_callback_.empty())
00362             resolution_change_callback_();
00363 
00364         status = driver_->startStreams(streamsEnabled);
00365         if (Status_Ok != status)
00366             ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
00367                       Channel::statusString(status));
00368     }
00369 
00370     //
00371     // Send the desired motor speed
00372 
00373     if (motor_supported_) {
00374 
00375         const float radiansPerSecondToRpm = 9.54929659643;
00376 
00377         status = driver_->setMotorSpeed(radiansPerSecondToRpm * dyn.motor_speed);
00378         if (Status_Ok != status) {
00379             if (Status_Unsupported == status)
00380                 motor_supported_ = false;
00381             else
00382                 ROS_ERROR("Reconfigure: failed to set motor speed: %s",
00383                           Channel::statusString(status));
00384         }
00385     }
00386 
00387     //
00388     // Send the desired lighting configuration
00389 
00390     if (lighting_supported_) {
00391 
00392         lighting::Config leds;
00393 
00394         if (false == dyn.lighting) {
00395             leds.setFlash(false);
00396             leds.setDutyCycle(0.0);
00397         } else {
00398             leds.setFlash(dyn.flash);
00399             leds.setDutyCycle(dyn.led_duty_cycle * 100.0);
00400         }
00401 
00402         status = driver_->setLightingConfig(leds);
00403         if (Status_Ok != status) {
00404             if (Status_Unsupported == status)
00405                 lighting_supported_ = false;
00406             else
00407                 ROS_ERROR("Reconfigure: failed to set lighting config: %s",
00408                           Channel::statusString(status));
00409         }
00410     }
00411 
00412     //
00413     // Enable/disable network-based time synchronization.
00414     //
00415     // If enabled, sensor timestamps will be reported in the local
00416     // system clock's frame, using a continuously updated offset from
00417     // the sensor's internal clock.
00418     //
00419     // If disabled, sensor timestamps will be reported in the sensor
00420     // clock's frame, which is free-running from zero on power up.
00421     //
00422     // Enabled by default.
00423 
00424     driver_->networkTimeSynchronization(dyn.network_time_sync);
00425 
00426     //
00427     // Set our transmit delay
00428     image::TransmitDelay d;
00429     d.delay = dyn.desired_transmit_delay;
00430     status = driver_->setTransmitDelay(d);
00431     if (Status_Ok != status) {
00432         ROS_ERROR("Reconfigure: failed to set transmit delay: %s",
00433                   Channel::statusString(status));
00434     }
00435 }
00436 
00437 template<class T> void Reconfigure::configureImu(const T& dyn)
00438 {
00439     if (imu_configs_.empty()) {
00440         Status status = driver_->getImuConfig(imu_samples_per_message_,
00441                                               imu_configs_);
00442         if (Status_Ok != status) {
00443             ROS_ERROR("Reconfigure: failed to query IMU config: %s",
00444                       Channel::statusString(status));
00445             return;
00446         }
00447     }
00448 
00449     std::vector<imu::Config> changedConfigs;
00450     std::vector<imu::Config>::iterator it = imu_configs_.begin();
00451     for(; it!=imu_configs_.end(); ++it) {
00452 
00453         imu::Config& c = *it;
00454 
00455         if ("accelerometer" == c.name &&
00456             (c.enabled      != dyn.accelerometer_enabled ||
00457              static_cast<int>(c.rateTableIndex)  != dyn.accelerometer_rate    ||
00458              static_cast<int>(c.rangeTableIndex) != dyn.accelerometer_range)) {
00459 
00460             c.enabled         = dyn.accelerometer_enabled;
00461             c.rateTableIndex  = dyn.accelerometer_rate;
00462             c.rangeTableIndex = dyn.accelerometer_range;
00463             changedConfigs.push_back(c);
00464         }
00465 
00466         if ("gyroscope" == c.name &&
00467             (c.enabled  != dyn.gyroscope_enabled ||
00468              static_cast<int>(c.rateTableIndex)  != dyn.gyroscope_rate    ||
00469              static_cast<int>(c.rangeTableIndex) != dyn.gyroscope_range)) {
00470 
00471             c.enabled         = dyn.gyroscope_enabled;
00472             c.rateTableIndex  = dyn.gyroscope_rate;
00473             c.rangeTableIndex = dyn.gyroscope_range;
00474             changedConfigs.push_back(c);
00475         }
00476 
00477         if ("magnetometer" == c.name &&
00478             (c.enabled     != dyn.magnetometer_enabled ||
00479              static_cast<int>(c.rateTableIndex)  != dyn.magnetometer_rate    ||
00480              static_cast<int>(c.rangeTableIndex) != dyn.magnetometer_range)) {
00481 
00482             c.enabled         = dyn.magnetometer_enabled;
00483             c.rateTableIndex  = dyn.magnetometer_rate;
00484             c.rangeTableIndex = dyn.magnetometer_range;
00485             changedConfigs.push_back(c);
00486         }
00487     }
00488 
00489     if (changedConfigs.size() > 0 ||
00490         static_cast<int>(imu_samples_per_message_) != dyn.imu_samples_per_message) {
00491 
00492         ROS_WARN("Reconfigure: IMU configuration changes will take effect after all IMU "
00493          "topic subscriptions have been closed.");
00494 
00495         imu_samples_per_message_ = dyn.imu_samples_per_message;
00496 
00497         Status status = driver_->setImuConfig(false, // store in non-volatile flash
00498                                               imu_samples_per_message_,
00499                                               changedConfigs);  // can be empty
00500         if (Status_Ok != status) {
00501             ROS_ERROR("Reconfigure: failed to set IMU configuration: %s",
00502                       Channel::statusString(status));
00503             imu_configs_.clear();
00504         }
00505     }
00506 }
00507 
00508 template<class T> void Reconfigure::configureBorderClip(const T& dyn)
00509 {
00510     bool regenerate = false;
00511 
00512     if (dyn.border_clip_type != border_clip_type_)
00513     {
00514         border_clip_type_ = dyn.border_clip_type;
00515         regenerate = true;
00516     }
00517 
00518     if (dyn.border_clip_value != border_clip_value_)
00519     {
00520         border_clip_value_ = dyn.border_clip_value;
00521         regenerate = true;
00522     }
00523 
00524     if (regenerate)
00525     {
00526         if (false == border_clip_change_callback_.empty())
00527         {
00528             border_clip_change_callback_(dyn.border_clip_type, dyn.border_clip_value);
00529         }
00530     }
00531 }
00532 
00533 
00534 #define GET_CONFIG()                                                    \
00535     image::Config cfg;                                                  \
00536     Status status = driver_->getImageConfig(cfg);                       \
00537     if (Status_Ok != status) {                                          \
00538         ROS_ERROR("Reconfigure: failed to query image config: %s",      \
00539                   Channel::statusString(status));                       \
00540         return;                                                         \
00541     }                                                                   \
00542 
00543 #define SL_BM()  do {                                           \
00544         GET_CONFIG();                                           \
00545         configureCamera(cfg, dyn);                              \
00546         configureBorderClip(dyn);                               \
00547     } while(0)
00548 
00549 #define SL_BM_IMU()  do {                                       \
00550         GET_CONFIG();                                           \
00551         configureCamera(cfg, dyn);                              \
00552         configureImu(dyn);                                      \
00553         configureBorderClip(dyn);                               \
00554     } while(0)
00555 
00556 #define SL_SGM_IMU()  do {                                      \
00557         GET_CONFIG();                                           \
00558         configureSgm(cfg, dyn);                                 \
00559         configureCamera(cfg, dyn);                              \
00560         configureImu(dyn);                                      \
00561         configureBorderClip(dyn);                               \
00562     } while(0)
00563 
00564 #define SL_SGM_IMU_CMV4000()  do {                              \
00565         GET_CONFIG();                                           \
00566         configureSgm(cfg, dyn);                                 \
00567         configureCropMode(cfg, dyn);                            \
00568         configureCamera(cfg, dyn);                              \
00569         configureImu(dyn);                                      \
00570         configureBorderClip(dyn);                               \
00571     } while(0)
00572 
00573 
00574 
00575 //
00576 // The dynamic reconfigure callbacks (MultiSense S* variations)
00577 
00578 void Reconfigure::callback_sl_bm_cmv2000      (multisense_ros::sl_bm_cmv2000Config&      dyn, uint32_t level) { SL_BM();       };
00579 void Reconfigure::callback_sl_bm_cmv2000_imu  (multisense_ros::sl_bm_cmv2000_imuConfig&  dyn, uint32_t level) { SL_BM_IMU();   };
00580 void Reconfigure::callback_sl_bm_cmv4000      (multisense_ros::sl_bm_cmv4000Config&      dyn, uint32_t level) { SL_BM();       };
00581 void Reconfigure::callback_sl_bm_cmv4000_imu  (multisense_ros::sl_bm_cmv4000_imuConfig&  dyn, uint32_t level) { SL_BM_IMU();   };
00582 void Reconfigure::callback_sl_sgm_cmv2000_imu (multisense_ros::sl_sgm_cmv2000_imuConfig& dyn, uint32_t level) { SL_SGM_IMU();  };
00583 void Reconfigure::callback_sl_sgm_cmv4000_imu (multisense_ros::sl_sgm_cmv4000_imuConfig& dyn, uint32_t level) { SL_SGM_IMU_CMV4000();  };
00584 void Reconfigure::callback_mono_cmv2000       (multisense_ros::mono_cmv2000Config&       dyn, uint32_t level) { SL_BM_IMU();   };
00585 void Reconfigure::callback_mono_cmv4000       (multisense_ros::mono_cmv4000Config&       dyn, uint32_t level) { SL_BM_IMU();   };
00586 
00587 //
00588 // BCAM (Sony IMX104)
00589 
00590 void Reconfigure::callback_bcam_imx104(multisense_ros::bcam_imx104Config& dyn,
00591                                        uint32_t                           level)
00592 {
00593     GET_CONFIG();
00594     DataSource  streamsEnabled = 0;
00595     int32_t     width, height;
00596     bool        resolutionChange=false;
00597 
00598     //
00599     // Decode the resolution string
00600 
00601     if (2 != sscanf(dyn.resolution.c_str(), "%dx%dx", &width, &height)) {
00602         ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
00603         return;
00604     }
00605 
00606     //
00607     // If a resolution change is desired
00608 
00609     if ((resolutionChange = changeResolution(cfg, width, height, 0))) {
00610 
00611         //
00612         // Halt streams during the resolution change
00613 
00614         status = driver_->getEnabledStreams(streamsEnabled);
00615         if (Status_Ok != status) {
00616             ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
00617                       Channel::statusString(status));
00618             return;
00619         }
00620         status = driver_->stopStreams(streamsEnabled);
00621         if (Status_Ok != status) {
00622             ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
00623                       Channel::statusString(status));
00624             return;
00625         }
00626     }
00627 
00628     //
00629     // Set all other image config from dynamic reconfigure
00630 
00631     cfg.setFps(static_cast<float>(dyn.fps));
00632     cfg.setGain(dyn.gain);
00633     cfg.setExposure(dyn.exposure_time * 1e6);
00634     cfg.setAutoExposure(dyn.auto_exposure);
00635     cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
00636     cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
00637     cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
00638     cfg.setWhiteBalance(dyn.white_balance_red,
00639                         dyn.white_balance_blue);
00640     cfg.setAutoWhiteBalance(dyn.auto_white_balance);
00641     cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
00642     cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);
00643 
00644     //
00645     // Apply, sensor enforces limits per setting.
00646 
00647     status = driver_->setImageConfig(cfg);
00648     if (Status_Ok != status)
00649         ROS_ERROR("Reconfigure: failed to set image config: %s",
00650                   Channel::statusString(status));
00651 
00652     //
00653     // If we are changing the resolution, let others know about it
00654 
00655     if (resolutionChange) {
00656 
00657         if (false == resolution_change_callback_.empty())
00658             resolution_change_callback_();
00659 
00660         status = driver_->startStreams(streamsEnabled);
00661         if (Status_Ok != status)
00662             ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
00663                       Channel::statusString(status));
00664     }
00665 }
00666 
00667 //
00668 // ST21 FLIR Thermal Imagers
00669 // Seperate callback required due to limited subset of dyn parameters
00670 // in st21_sgm_vga_imuConfig. configureCamera results in SFINAE errors
00671 
00672 void Reconfigure::callback_st21_vga(multisense_ros::st21_sgm_vga_imuConfig& dyn,
00673                                        uint32_t                           level)
00674 {
00675 
00676     DataSource    streamsEnabled = 0;
00677     int32_t       width, height, disparities;
00678     bool          resolutionChange=false;
00679 
00680     GET_CONFIG();
00681 
00682     //
00683     // Decode the resolution string
00684 
00685     if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
00686         ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
00687         return;
00688     }
00689 
00690     //
00691     // If a resolution change is desired
00692 
00693     if ((resolutionChange = changeResolution(cfg, width, height, disparities))) {
00694 
00695         //
00696         // Halt streams during the resolution change
00697 
00698         status = driver_->getEnabledStreams(streamsEnabled);
00699         if (Status_Ok != status) {
00700             ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
00701                       Channel::statusString(status));
00702             return;
00703         }
00704         status = driver_->stopStreams(streamsEnabled);
00705         if (Status_Ok != status) {
00706             ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
00707                       Channel::statusString(status));
00708             return;
00709         }
00710     }
00711 
00712     cfg.setFps(dyn.fps);
00713 
00714     configureSgm(cfg, dyn);
00715     configureImu(dyn);
00716 
00717     //
00718     // Apply, sensor enforces limits per setting.
00719 
00720     status = driver_->setImageConfig(cfg);
00721     if (Status_Ok != status)
00722         ROS_ERROR("Reconfigure: failed to set image config: %s",
00723                   Channel::statusString(status));
00724 
00725     //
00726     // If we are changing the resolution, let others know about it
00727 
00728     if (resolutionChange) {
00729 
00730         if (false == resolution_change_callback_.empty())
00731             resolution_change_callback_();
00732 
00733         status = driver_->startStreams(streamsEnabled);
00734         if (Status_Ok != status)
00735             ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
00736                       Channel::statusString(status));
00737     }
00738 
00739     configureBorderClip(dyn);
00740 }
00741 
00742 } // namespace


multisense_ros
Author(s):
autogenerated on Fri Apr 5 2019 02:28:29