reconfigure.cpp
Go to the documentation of this file.
1 
35 
36 using namespace crl::multisense;
37 
38 namespace multisense_ros {
39 
40 Reconfigure::Reconfigure(Channel* driver,
41  boost::function<void ()> resolutionChangeCallback,
42  boost::function<void (int, int)> borderClipChangeCallback) :
43  driver_(driver),
44  resolution_change_callback_(resolutionChangeCallback),
45  device_nh_(""),
46  device_modes_(),
47  imu_samples_per_message_(0),
48  imu_configs_(),
49  server_sl_bm_cmv2000_(),
50  server_sl_bm_cmv2000_imu_(),
51  server_sl_bm_cmv4000_(),
52  server_sl_bm_cmv4000_imu_(),
53  server_sl_sgm_cmv2000_imu_(),
54  server_sl_sgm_cmv4000_imu_(),
55  server_bcam_imx104_(),
56  server_st21_vga_(),
57  lighting_supported_(false),
58  motor_supported_(false),
59  border_clip_type_(RECTANGULAR),
60  border_clip_value_(0.0),
61  border_clip_change_callback_(borderClipChangeCallback),
62  crop_mode_changed_(false)
63 {
64  system::DeviceInfo deviceInfo;
65  system::VersionInfo versionInfo;
66 
67  //
68  // Query device and version information from sensor
69 
70  Status status = driver_->getVersionInfo(versionInfo);
71  if (Status_Ok != status) {
72  ROS_ERROR("Reconfigure: failed to query version info: %s",
73  Channel::statusString(status));
74  return;
75  }
76  status = driver_->getDeviceInfo(deviceInfo);
77  if (Status_Ok != status) {
78  ROS_ERROR("Reconfigure: failed to query device info: %s",
79  Channel::statusString(status));
80  return;
81  }
82 
83  if (deviceInfo.lightingType != 0)
84  {
85  lighting_supported_ = true;
86  }
87  if (deviceInfo.motorType != 0)
88  {
89  motor_supported_ = true;
90  }
91 
92  //
93  // Launch the correct reconfigure server for this device configuration.
94 
95  if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {
96 
99  new dynamic_reconfigure::Server<multisense_ros::bcam_imx104Config>(device_nh_));
100  server_bcam_imx104_->setCallback(boost::bind(&Reconfigure::callback_bcam_imx104, this, _1, _2));
101 
102  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == deviceInfo.hardwareRevision) {
103 
106  new dynamic_reconfigure::Server<multisense_ros::st21_sgm_vga_imuConfig>(device_nh_));
107  server_st21_vga_->setCallback(boost::bind(&Reconfigure::callback_st21_vga, this, _1, _2));
108 
109  } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == deviceInfo.hardwareRevision) {
110 
111  switch(deviceInfo.imagerType) {
112  case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
113  case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
114 
117  new dynamic_reconfigure::Server<multisense_ros::mono_cmv2000Config>(device_nh_));
118  server_mono_cmv2000_->setCallback(boost::bind(&Reconfigure::callback_mono_cmv2000, this, _1, _2));
119 
120  break;
121  case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
122  case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
123 
126  new dynamic_reconfigure::Server<multisense_ros::mono_cmv4000Config>(device_nh_));
127  server_mono_cmv4000_->setCallback(boost::bind(&Reconfigure::callback_mono_cmv4000, this, _1, _2));
128 
129  break;
130  }
131 
132  } else if (versionInfo.sensorFirmwareVersion <= 0x0202) {
133 
134  switch(deviceInfo.imagerType) {
135  case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
136  case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
137 
140  new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000Config>(device_nh_));
141  server_sl_bm_cmv2000_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv2000, this, _1, _2));
142 
143  break;
144  case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
145  case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
146 
149  new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000Config>(device_nh_));
150  server_sl_bm_cmv4000_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv4000, this, _1, _2));
151 
152  break;
153  default:
154 
155  ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
156  return;
157  }
158 
159  } else if (versionInfo.sensorFirmwareVersion < 0x0300) {
160 
161  switch(deviceInfo.imagerType) {
162  case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
163  case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
164 
167  new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000_imuConfig>(device_nh_));
168  server_sl_bm_cmv2000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv2000_imu, this, _1, _2));
169 
170  break;
171  case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
172  case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
173 
176  new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000_imuConfig>(device_nh_));
177  server_sl_bm_cmv4000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_bm_cmv4000_imu, this, _1, _2));
178 
179  break;
180  default:
181 
182  ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
183  return;
184  }
185 
186  } else {
187 
188  switch(deviceInfo.imagerType) {
189  case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
190  case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:
191 
194  new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv2000_imuConfig>(device_nh_));
195  server_sl_sgm_cmv2000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_sgm_cmv2000_imu, this, _1, _2));
196 
197  break;
198  case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
199  case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:
200 
203  new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv4000_imuConfig>(device_nh_));
204  server_sl_sgm_cmv4000_imu_->setCallback(boost::bind(&Reconfigure::callback_sl_sgm_cmv4000_imu, this, _1, _2));
205  break;
206  default:
207 
208  ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
209  return;
210  }
211  }
212 }
213 
215 {
216 }
217 
218 //
219 // Helper to change resolution. Will check against supported device modes
220 
222  int32_t width,
223  int32_t height,
224  int32_t disparities)
225 {
226  //
227  // See if we need to change resolutions
228 
229  if (width == static_cast<int32_t>(cfg.width()) &&
230  height == static_cast<int32_t>(cfg.height()) &&
231  disparities == static_cast<int32_t>(cfg.disparities()))
232  return false;
233 
234  //
235  // Query all supported resolutions from the sensor, if we haven't already
236 
237  if (device_modes_.empty()) {
238 
240  if (Status_Ok != status) {
241  ROS_ERROR("Reconfigure: failed to query sensor modes: %s",
242  Channel::statusString(status));
243  return false;
244  }
245  }
246 
247  //
248  // Verify that this resolution is supported
249 
250  bool supported = false;
251  std::vector<system::DeviceMode>::const_iterator it = device_modes_.begin();
252  for(; it != device_modes_.end(); ++it) {
253 
254  const system::DeviceMode& m = *it;
255 
256  if (width == static_cast<int32_t>(m.width) &&
257  height == static_cast<int32_t>(m.height) &&
258  disparities == static_cast<int32_t>(m.disparities)) {
259 
260  supported = true;
261  break;
262  }
263  }
264 
265  if (false == supported) {
266  ROS_ERROR("Reconfigure: sensor does not support a resolution of: %dx%d (%d disparities)",
267  width, height, disparities);
268  return false;
269  }
270 
271  ROS_WARN("Reconfigure: changing sensor resolution to %dx%d (%d disparities), from %dx%d "
272  "(%d disparities): reconfiguration may take up to 30 seconds",
273  width, height, disparities,
274  cfg.width(), cfg.height(), cfg.disparities());
275 
276  cfg.setResolution(width, height);
277  cfg.setDisparities(disparities);
278 
279  return true;
280 }
281 
282 template<class T> void Reconfigure::configureCropMode(crl::multisense::image::Config& cfg, const T& dyn)
283 {
284  cfg.setCamMode(dyn.crop_mode == 1 ? 2000 : 4000);
285  cfg.setOffset(dyn.crop_offset);
286  ROS_WARN("Reconfigure: changing cam mode to %s with offset %d: reconfiguration may take up to 30 seconds",
287  dyn.crop_mode == 1 ? "ON" : "OFF" , cfg.offset());
288  crop_mode_changed_ = true;
289 }
290 
291 template<class T> void Reconfigure::configureSgm(image::Config& cfg, const T& dyn)
292 {
293  cfg.setStereoPostFilterStrength(dyn.stereo_post_filtering);
294 }
295 
296 template<class T> void Reconfigure::configureCamera(image::Config& cfg, const T& dyn)
297 {
298  DataSource streamsEnabled = 0;
299  int32_t width, height, disparities;
300  bool resolutionChange=false;
301  Status status=Status_Ok;
302 
303  //
304  // Decode the resolution string
305 
306  if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
307  ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
308  return;
309  }
310 
311  //
312  // If a resolution change is desired
313 
314  if ((resolutionChange = changeResolution(cfg, width, height, disparities) || crop_mode_changed_)) {
315  crop_mode_changed_ = false;
316  //
317  // Halt streams during the resolution change
318  status = driver_->getEnabledStreams(streamsEnabled);
319  if (Status_Ok != status) {
320  ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
321  Channel::statusString(status));
322  return;
323  }
324  status = driver_->stopStreams(streamsEnabled);
325  if (Status_Ok != status) {
326  ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
327  Channel::statusString(status));
328  return;
329  }
330  }
331 
332  //
333  // Set all other image config from dynamic reconfigure
334 
335  cfg.setFps(dyn.fps);
336  cfg.setGain(dyn.gain);
337  cfg.setExposure(dyn.exposure_time * 1e6);
338  cfg.setAutoExposure(dyn.auto_exposure);
339  cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
340  cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
341  cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
342  cfg.setWhiteBalance(dyn.white_balance_red,
343  dyn.white_balance_blue);
344  cfg.setAutoWhiteBalance(dyn.auto_white_balance);
345  cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
346  cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);
347  cfg.setHdr(dyn.hdr_enable);
348 
349  //
350  // Apply, sensor enforces limits per setting.
351 
352  status = driver_->setImageConfig(cfg);
353  if (Status_Ok != status)
354  ROS_ERROR("Reconfigure: failed to set image config: %s",
355  Channel::statusString(status));
356 
357  //
358  // If we are changing the resolution, let others know about it
359 
360  if (resolutionChange) {
361  if (false == resolution_change_callback_.empty())
363 
364  status = driver_->startStreams(streamsEnabled);
365  if (Status_Ok != status)
366  ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
367  Channel::statusString(status));
368  }
369 
370  //
371  // Send the desired motor speed
372 
373  if (motor_supported_) {
374 
375  const float radiansPerSecondToRpm = 9.54929659643;
376 
377  status = driver_->setMotorSpeed(radiansPerSecondToRpm * dyn.motor_speed);
378  if (Status_Ok != status) {
379  if (Status_Unsupported == status)
380  motor_supported_ = false;
381  else
382  ROS_ERROR("Reconfigure: failed to set motor speed: %s",
383  Channel::statusString(status));
384  }
385  }
386 
387  //
388  // Send the desired lighting configuration
389 
390  if (lighting_supported_) {
391 
392  lighting::Config leds;
393 
394  if (false == dyn.lighting) {
395  leds.setFlash(false);
396  leds.setDutyCycle(0.0);
397  } else {
398  leds.setFlash(dyn.flash);
399  leds.setDutyCycle(dyn.led_duty_cycle * 100.0);
400  }
401 
402  status = driver_->setLightingConfig(leds);
403  if (Status_Ok != status) {
404  if (Status_Unsupported == status)
405  lighting_supported_ = false;
406  else
407  ROS_ERROR("Reconfigure: failed to set lighting config: %s",
408  Channel::statusString(status));
409  }
410  }
411 
412  //
413  // Enable/disable network-based time synchronization.
414  //
415  // If enabled, sensor timestamps will be reported in the local
416  // system clock's frame, using a continuously updated offset from
417  // the sensor's internal clock.
418  //
419  // If disabled, sensor timestamps will be reported in the sensor
420  // clock's frame, which is free-running from zero on power up.
421  //
422  // Enabled by default.
423 
424  driver_->networkTimeSynchronization(dyn.network_time_sync);
425 
426  //
427  // Set our transmit delay
429  d.delay = dyn.desired_transmit_delay;
430  status = driver_->setTransmitDelay(d);
431  if (Status_Ok != status) {
432  ROS_ERROR("Reconfigure: failed to set transmit delay: %s",
433  Channel::statusString(status));
434  }
435 }
436 
437 template<class T> void Reconfigure::configureImu(const T& dyn)
438 {
439  if (imu_configs_.empty()) {
441  imu_configs_);
442  if (Status_Ok != status) {
443  ROS_ERROR("Reconfigure: failed to query IMU config: %s",
444  Channel::statusString(status));
445  return;
446  }
447  }
448 
449  std::vector<imu::Config> changedConfigs;
450  std::vector<imu::Config>::iterator it = imu_configs_.begin();
451  for(; it!=imu_configs_.end(); ++it) {
452 
453  imu::Config& c = *it;
454 
455  if ("accelerometer" == c.name &&
456  (c.enabled != dyn.accelerometer_enabled ||
457  static_cast<int>(c.rateTableIndex) != dyn.accelerometer_rate ||
458  static_cast<int>(c.rangeTableIndex) != dyn.accelerometer_range)) {
459 
460  c.enabled = dyn.accelerometer_enabled;
461  c.rateTableIndex = dyn.accelerometer_rate;
462  c.rangeTableIndex = dyn.accelerometer_range;
463  changedConfigs.push_back(c);
464  }
465 
466  if ("gyroscope" == c.name &&
467  (c.enabled != dyn.gyroscope_enabled ||
468  static_cast<int>(c.rateTableIndex) != dyn.gyroscope_rate ||
469  static_cast<int>(c.rangeTableIndex) != dyn.gyroscope_range)) {
470 
471  c.enabled = dyn.gyroscope_enabled;
472  c.rateTableIndex = dyn.gyroscope_rate;
473  c.rangeTableIndex = dyn.gyroscope_range;
474  changedConfigs.push_back(c);
475  }
476 
477  if ("magnetometer" == c.name &&
478  (c.enabled != dyn.magnetometer_enabled ||
479  static_cast<int>(c.rateTableIndex) != dyn.magnetometer_rate ||
480  static_cast<int>(c.rangeTableIndex) != dyn.magnetometer_range)) {
481 
482  c.enabled = dyn.magnetometer_enabled;
483  c.rateTableIndex = dyn.magnetometer_rate;
484  c.rangeTableIndex = dyn.magnetometer_range;
485  changedConfigs.push_back(c);
486  }
487  }
488 
489  if (changedConfigs.size() > 0 ||
490  static_cast<int>(imu_samples_per_message_) != dyn.imu_samples_per_message) {
491 
492  ROS_WARN("Reconfigure: IMU configuration changes will take effect after all IMU "
493  "topic subscriptions have been closed.");
494 
495  imu_samples_per_message_ = dyn.imu_samples_per_message;
496 
497  Status status = driver_->setImuConfig(false, // store in non-volatile flash
499  changedConfigs); // can be empty
500  if (Status_Ok != status) {
501  ROS_ERROR("Reconfigure: failed to set IMU configuration: %s",
502  Channel::statusString(status));
503  imu_configs_.clear();
504  }
505  }
506 }
507 
508 template<class T> void Reconfigure::configureBorderClip(const T& dyn)
509 {
510  bool regenerate = false;
511 
512  if (dyn.border_clip_type != border_clip_type_)
513  {
514  border_clip_type_ = dyn.border_clip_type;
515  regenerate = true;
516  }
517 
518  if (dyn.border_clip_value != border_clip_value_)
519  {
520  border_clip_value_ = dyn.border_clip_value;
521  regenerate = true;
522  }
523 
524  if (regenerate)
525  {
526  if (false == border_clip_change_callback_.empty())
527  {
528  border_clip_change_callback_(dyn.border_clip_type, dyn.border_clip_value);
529  }
530  }
531 }
532 
533 
534 #define GET_CONFIG() \
535  image::Config cfg; \
536  Status status = driver_->getImageConfig(cfg); \
537  if (Status_Ok != status) { \
538  ROS_ERROR("Reconfigure: failed to query image config: %s", \
539  Channel::statusString(status)); \
540  return; \
541  } \
542 
543 #define SL_BM() do { \
544  GET_CONFIG(); \
545  configureCamera(cfg, dyn); \
546  configureBorderClip(dyn); \
547  } while(0)
548 
549 #define SL_BM_IMU() do { \
550  GET_CONFIG(); \
551  configureCamera(cfg, dyn); \
552  configureImu(dyn); \
553  configureBorderClip(dyn); \
554  } while(0)
555 
556 #define SL_SGM_IMU() do { \
557  GET_CONFIG(); \
558  configureSgm(cfg, dyn); \
559  configureCamera(cfg, dyn); \
560  configureImu(dyn); \
561  configureBorderClip(dyn); \
562  } while(0)
563 
564 #define SL_SGM_IMU_CMV4000() do { \
565  GET_CONFIG(); \
566  configureSgm(cfg, dyn); \
567  configureCropMode(cfg, dyn); \
568  configureCamera(cfg, dyn); \
569  configureImu(dyn); \
570  configureBorderClip(dyn); \
571  } while(0)
572 
573 
574 
575 //
576 // The dynamic reconfigure callbacks (MultiSense S* variations)
577 
578 void Reconfigure::callback_sl_bm_cmv2000 (multisense_ros::sl_bm_cmv2000Config& dyn, uint32_t level) { SL_BM(); };
579 void Reconfigure::callback_sl_bm_cmv2000_imu (multisense_ros::sl_bm_cmv2000_imuConfig& dyn, uint32_t level) { SL_BM_IMU(); };
580 void Reconfigure::callback_sl_bm_cmv4000 (multisense_ros::sl_bm_cmv4000Config& dyn, uint32_t level) { SL_BM(); };
581 void Reconfigure::callback_sl_bm_cmv4000_imu (multisense_ros::sl_bm_cmv4000_imuConfig& dyn, uint32_t level) { SL_BM_IMU(); };
582 void Reconfigure::callback_sl_sgm_cmv2000_imu (multisense_ros::sl_sgm_cmv2000_imuConfig& dyn, uint32_t level) { SL_SGM_IMU(); };
583 void Reconfigure::callback_sl_sgm_cmv4000_imu (multisense_ros::sl_sgm_cmv4000_imuConfig& dyn, uint32_t level) { SL_SGM_IMU_CMV4000(); };
584 void Reconfigure::callback_mono_cmv2000 (multisense_ros::mono_cmv2000Config& dyn, uint32_t level) { SL_BM_IMU(); };
585 void Reconfigure::callback_mono_cmv4000 (multisense_ros::mono_cmv4000Config& dyn, uint32_t level) { SL_BM_IMU(); };
586 
587 //
588 // BCAM (Sony IMX104)
589 
590 void Reconfigure::callback_bcam_imx104(multisense_ros::bcam_imx104Config& dyn,
591  uint32_t level)
592 {
593  GET_CONFIG();
594  DataSource streamsEnabled = 0;
595  int32_t width, height;
596  bool resolutionChange=false;
597 
598  //
599  // Decode the resolution string
600 
601  if (2 != sscanf(dyn.resolution.c_str(), "%dx%dx", &width, &height)) {
602  ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
603  return;
604  }
605 
606  //
607  // If a resolution change is desired
608 
609  if ((resolutionChange = changeResolution(cfg, width, height, 0))) {
610 
611  //
612  // Halt streams during the resolution change
613 
614  status = driver_->getEnabledStreams(streamsEnabled);
615  if (Status_Ok != status) {
616  ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
617  Channel::statusString(status));
618  return;
619  }
620  status = driver_->stopStreams(streamsEnabled);
621  if (Status_Ok != status) {
622  ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
623  Channel::statusString(status));
624  return;
625  }
626  }
627 
628  //
629  // Set all other image config from dynamic reconfigure
630 
631  cfg.setFps(static_cast<float>(dyn.fps));
632  cfg.setGain(dyn.gain);
633  cfg.setExposure(dyn.exposure_time * 1e6);
634  cfg.setAutoExposure(dyn.auto_exposure);
635  cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
636  cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
637  cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
638  cfg.setWhiteBalance(dyn.white_balance_red,
639  dyn.white_balance_blue);
640  cfg.setAutoWhiteBalance(dyn.auto_white_balance);
641  cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
642  cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);
643 
644  //
645  // Apply, sensor enforces limits per setting.
646 
647  status = driver_->setImageConfig(cfg);
648  if (Status_Ok != status)
649  ROS_ERROR("Reconfigure: failed to set image config: %s",
650  Channel::statusString(status));
651 
652  //
653  // If we are changing the resolution, let others know about it
654 
655  if (resolutionChange) {
656 
657  if (false == resolution_change_callback_.empty())
659 
660  status = driver_->startStreams(streamsEnabled);
661  if (Status_Ok != status)
662  ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
663  Channel::statusString(status));
664  }
665 }
666 
667 //
668 // ST21 FLIR Thermal Imagers
669 // Seperate callback required due to limited subset of dyn parameters
670 // in st21_sgm_vga_imuConfig. configureCamera results in SFINAE errors
671 
672 void Reconfigure::callback_st21_vga(multisense_ros::st21_sgm_vga_imuConfig& dyn,
673  uint32_t level)
674 {
675 
676  DataSource streamsEnabled = 0;
677  int32_t width, height, disparities;
678  bool resolutionChange=false;
679 
680  GET_CONFIG();
681 
682  //
683  // Decode the resolution string
684 
685  if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
686  ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
687  return;
688  }
689 
690  //
691  // If a resolution change is desired
692 
693  if ((resolutionChange = changeResolution(cfg, width, height, disparities))) {
694 
695  //
696  // Halt streams during the resolution change
697 
698  status = driver_->getEnabledStreams(streamsEnabled);
699  if (Status_Ok != status) {
700  ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
701  Channel::statusString(status));
702  return;
703  }
704  status = driver_->stopStreams(streamsEnabled);
705  if (Status_Ok != status) {
706  ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
707  Channel::statusString(status));
708  return;
709  }
710  }
711 
712  cfg.setFps(dyn.fps);
713 
714  configureSgm(cfg, dyn);
715  configureImu(dyn);
716 
717  //
718  // Apply, sensor enforces limits per setting.
719 
720  status = driver_->setImageConfig(cfg);
721  if (Status_Ok != status)
722  ROS_ERROR("Reconfigure: failed to set image config: %s",
723  Channel::statusString(status));
724 
725  //
726  // If we are changing the resolution, let others know about it
727 
728  if (resolutionChange) {
729 
730  if (false == resolution_change_callback_.empty())
732 
733  status = driver_->startStreams(streamsEnabled);
734  if (Status_Ok != status)
735  ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
736  Channel::statusString(status));
737  }
738 
739  configureBorderClip(dyn);
740 }
741 
742 } // namespace
virtual Status startStreams(DataSource mask)=0
d
void callback_sl_bm_cmv4000(multisense_ros::sl_bm_cmv4000Config &config, uint32_t level)
void configureCamera(crl::multisense::image::Config &cfg, const T &dyn)
boost::function< void(int, int)> border_clip_change_callback_
Definition: reconfigure.h:151
void setStereoPostFilterStrength(float s)
#define SL_BM_IMU()
void configureBorderClip(const T &dyn)
virtual Status getEnabledStreams(DataSource &mask)=0
virtual Status setMotorSpeed(float rpm)=0
void setAutoWhiteBalanceThresh(float t)
std::vector< crl::multisense::system::DeviceMode > device_modes_
Definition: reconfigure.h:113
void setAutoWhiteBalanceDecay(uint32_t d)
crl::multisense::Channel * driver_
Definition: reconfigure.h:98
void callback_mono_cmv4000(multisense_ros::mono_cmv4000Config &config, uint32_t level)
virtual Status stopStreams(DataSource mask)=0
#define SL_SGM_IMU()
void callback_mono_cmv2000(multisense_ros::mono_cmv2000Config &config, uint32_t level)
void callback_sl_sgm_cmv4000_imu(multisense_ros::sl_sgm_cmv4000_imuConfig &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::st21_sgm_vga_imuConfig > > server_st21_vga_
Definition: reconfigure.h:127
void callback_st21_vga(multisense_ros::st21_sgm_vga_imuConfig &config, uint32_t level)
void setResolution(uint32_t w, uint32_t h)
uint32_t disparities() const
virtual Status networkTimeSynchronization(bool enabled)=0
bool setDutyCycle(float percent)
#define GET_CONFIG()
#define ROS_WARN(...)
virtual Status getVersionInfo(system::VersionInfo &v)=0
virtual Status getImuConfig(uint32_t &samplesPerMessage, std::vector< imu::Config > &c)=0
void callback_sl_sgm_cmv2000_imu(multisense_ros::sl_sgm_cmv2000_imuConfig &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_bm_cmv4000Config > > server_sl_bm_cmv4000_
Definition: reconfigure.h:122
ros::NodeHandle device_nh_
Definition: reconfigure.h:108
void callback_sl_bm_cmv4000_imu(multisense_ros::sl_bm_cmv4000_imuConfig &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::bcam_imx104Config > > server_bcam_imx104_
Definition: reconfigure.h:126
virtual Status setImuConfig(bool storeSettingsInFlash, uint32_t samplesPerMessage, const std::vector< imu::Config > &c)=0
#define SL_BM()
void configureImu(const T &dyn)
void configureCropMode(crl::multisense::image::Config &cfg, const T &dyn)
virtual Status getDeviceModes(std::vector< system::DeviceMode > &m)=0
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_sgm_cmv4000_imuConfig > > server_sl_sgm_cmv4000_imu_
Definition: reconfigure.h:125
void setDisparities(uint32_t d)
virtual Status setLightingConfig(const lighting::Config &c)=0
virtual Status setImageConfig(const image::Config &c)=0
void setAutoExposureMax(uint32_t m)
void setWhiteBalance(float r, float b)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_sgm_cmv2000_imuConfig > > server_sl_sgm_cmv2000_imu_
Definition: reconfigure.h:124
void configureSgm(crl::multisense::image::Config &cfg, const T &dyn)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_bm_cmv2000_imuConfig > > server_sl_bm_cmv2000_imu_
Definition: reconfigure.h:121
uint32_t DataSource
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::mono_cmv2000Config > > server_mono_cmv2000_
Definition: reconfigure.h:128
bool changeResolution(crl::multisense::image::Config &cfg, int32_t width, int32_t height, int32_t disparities)
virtual Status setTransmitDelay(const image::TransmitDelay &c)=0
#define SL_SGM_IMU_CMV4000()
void callback_sl_bm_cmv2000(multisense_ros::sl_bm_cmv2000Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::mono_cmv4000Config > > server_mono_cmv4000_
Definition: reconfigure.h:129
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
void callback_bcam_imx104(multisense_ros::bcam_imx104Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_bm_cmv4000_imuConfig > > server_sl_bm_cmv4000_imu_
Definition: reconfigure.h:123
void setAutoExposureDecay(uint32_t d)
void callback_sl_bm_cmv2000_imu(multisense_ros::sl_bm_cmv2000_imuConfig &config, uint32_t level)
boost::function< void()> resolution_change_callback_
Definition: reconfigure.h:103
std::vector< crl::multisense::imu::Config > imu_configs_
Definition: reconfigure.h:115
#define ROS_ERROR(...)
boost::shared_ptr< dynamic_reconfigure::Server< multisense_ros::sl_bm_cmv2000Config > > server_sl_bm_cmv2000_
Definition: reconfigure.h:120


multisense_ros
Author(s):
autogenerated on Wed Aug 5 2020 03:32:50