reconfigure.cpp

Go to the documentation of this file.

#include <multisense_ros/reconfigure.h>

using namespace crl::multisense;

namespace multisense_ros {

Reconfigure::Reconfigure(Channel* driver,
                         std::function<void (crl::multisense::image::Config)> resolutionChangeCallback,
                         std::function<void (BorderClip, double)> borderClipChangeCallback,
                         std::function<void (double)> maxPointCloudRangeCallback):
    driver_(driver),
    resolution_change_callback_(resolutionChangeCallback),
    device_nh_(""),
    imu_samples_per_message_(0),
    lighting_supported_(false),
    motor_supported_(false),
    crop_mode_changed_(false),
    ptp_supported_(false),
    border_clip_type_(BorderClip::NONE),
    border_clip_value_(0.0),
    border_clip_change_callback_(borderClipChangeCallback),
    max_point_cloud_range_callback_(maxPointCloudRangeCallback)
{
    system::DeviceInfo  deviceInfo;
    system::VersionInfo versionInfo;

    //
    // Query device and version information from sensor

    Status status = driver_->getVersionInfo(versionInfo);
    if (Status_Ok != status) {
        ROS_ERROR("Reconfigure: failed to query version info: %s",
                  Channel::statusString(status));
        return;
    }
    status = driver_->getDeviceInfo(deviceInfo);
    if (Status_Ok != status) {
        ROS_ERROR("Reconfigure: failed to query device info: %s",
                  Channel::statusString(status));
        return;
    }

    if (deviceInfo.lightingType != 0)
    {
        lighting_supported_ = true;
    }
    if (deviceInfo.motorType != 0)
    {
        motor_supported_ = true;
    }
    if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == deviceInfo.hardwareRevision ||
        system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == deviceInfo.hardwareRevision ||
        system::DeviceInfo::HARDWARE_REV_MULTISENSE_KS21 == deviceInfo.hardwareRevision)
    {
        ptp_supported_ = true;
    }

    //
    // Launch the correct reconfigure server for this device configuration.

    if (system::DeviceInfo::HARDWARE_REV_BCAM == deviceInfo.hardwareRevision) {

        server_bcam_imx104_ =
            std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::bcam_imx104Config> > (
                new dynamic_reconfigure::Server<multisense_ros::bcam_imx104Config>(device_nh_));
        server_bcam_imx104_->setCallback(std::bind(&Reconfigure::callback_bcam_imx104, this,
                                                   std::placeholders::_1, std::placeholders::_2));

    } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == deviceInfo.hardwareRevision) {

        server_st21_vga_ =
            std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::st21_sgm_vga_imuConfig> > (
                new dynamic_reconfigure::Server<multisense_ros::st21_sgm_vga_imuConfig>(device_nh_));
        server_st21_vga_->setCallback(std::bind(&Reconfigure::callback_st21_vga, this,
                                                std::placeholders::_1, std::placeholders::_2));

    } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == deviceInfo.hardwareRevision) {

        switch(deviceInfo.imagerType) {
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:

            server_mono_cmv2000_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::mono_cmv2000Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::mono_cmv2000Config>(device_nh_));
            server_mono_cmv2000_->setCallback(std::bind(&Reconfigure::callback_mono_cmv2000, this,
                                                        std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:

            server_mono_cmv4000_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::mono_cmv4000Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::mono_cmv4000Config>(device_nh_));
            server_mono_cmv4000_->setCallback(std::bind(&Reconfigure::callback_mono_cmv4000, this,
                                                        std::placeholders::_1, std::placeholders::_2));

            break;
        }

    } else if (versionInfo.sensorFirmwareVersion <= 0x0202) {

        switch(deviceInfo.imagerType) {
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:

            server_sl_bm_cmv2000_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000Config>(device_nh_));
            server_sl_bm_cmv2000_->setCallback(std::bind(&Reconfigure::callback_sl_bm_cmv2000, this,
                                                         std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:

            server_sl_bm_cmv4000_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000Config>(device_nh_));
            server_sl_bm_cmv4000_->setCallback(std::bind(&Reconfigure::callback_sl_bm_cmv4000, this,
                                                         std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR:
        case system::DeviceInfo::IMAGER_TYPE_AR0234_GREY:

            server_s27_AR0234_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config>(device_nh_));
            server_s27_AR0234_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234, this,
                                                         std::placeholders::_1, std::placeholders::_2));

            break;
        default:

            ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
            return;
        }

    } else if (versionInfo.sensorFirmwareVersion < 0x0300) {

        switch(deviceInfo.imagerType) {
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:

            server_sl_bm_cmv2000_imu_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000_imuConfig> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv2000_imuConfig>(device_nh_));
            server_sl_bm_cmv2000_imu_->setCallback(std::bind(&Reconfigure::callback_sl_bm_cmv2000_imu, this,
                                                             std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:

            server_sl_bm_cmv4000_imu_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000_imuConfig> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_bm_cmv4000_imuConfig>(device_nh_));
            server_sl_bm_cmv4000_imu_->setCallback(std::bind(&Reconfigure::callback_sl_bm_cmv4000_imu, this,
                                                             std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR:
        case system::DeviceInfo::IMAGER_TYPE_AR0234_GREY:

            server_s27_AR0234_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config>(device_nh_));
            server_s27_AR0234_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234, this,
                                                      std::placeholders::_1, std::placeholders::_2));

            break;
        default:

            ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
            return;
        }

    } else {

        switch(deviceInfo.imagerType) {
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV2000_COLOR:

            server_sl_sgm_cmv2000_imu_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv2000_imuConfig> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv2000_imuConfig>(device_nh_));
            server_sl_sgm_cmv2000_imu_->setCallback(std::bind(&Reconfigure::callback_sl_sgm_cmv2000_imu, this,
                                                              std::placeholders::_1, std::placeholders::_2));

            break;
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_GREY:
        case system::DeviceInfo::IMAGER_TYPE_CMV4000_COLOR:

            server_sl_sgm_cmv4000_imu_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv4000_imuConfig> > (
                    new dynamic_reconfigure::Server<multisense_ros::sl_sgm_cmv4000_imuConfig>(device_nh_));
            server_sl_sgm_cmv4000_imu_->setCallback(std::bind(&Reconfigure::callback_sl_sgm_cmv4000_imu, this,
                                                              std::placeholders::_1, std::placeholders::_2));
            break;
        case system::DeviceInfo::IMAGER_TYPE_AR0239_COLOR:
        case system::DeviceInfo::IMAGER_TYPE_AR0234_GREY:

            server_s27_AR0234_ =
                std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config> > (
                    new dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234Config>(device_nh_));
            server_s27_AR0234_->setCallback(std::bind(&Reconfigure::callback_s27_AR0234, this,
                                                      std::placeholders::_1, std::placeholders::_2));

            break;
        default:

            ROS_ERROR("Reconfigure: unsupported imager type \"%d\"", deviceInfo.imagerType);
            return;
        }
    }
}

Reconfigure::~Reconfigure()
{
}

//
// Helper to change resolution. Will check against supported device modes

bool Reconfigure::changeResolution(image::Config& cfg,
                                   int32_t        width,
                                   int32_t        height,
                                   int32_t        disparities)
{
    //
    // See if we need to change resolutions
    if (width       == static_cast<int32_t>(cfg.width())   &&
        height      == static_cast<int32_t>(cfg.height())  &&
        disparities == static_cast<int32_t>(cfg.disparities()))
        return false;

    //
    // Query all supported resolutions from the sensor, if we haven't already

    if (device_modes_.empty()) {

        Status status = driver_->getDeviceModes(device_modes_);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to query sensor modes: %s",
                      Channel::statusString(status));
            return false;
        }
    }

    //
    // Verify that this resolution is supported

    bool supported = false;
    std::vector<system::DeviceMode>::const_iterator it = device_modes_.begin();
    for(; it != device_modes_.end(); ++it) {

        const system::DeviceMode& m = *it;

        if (width       == static_cast<int32_t>(m.width)  &&
            height      == static_cast<int32_t>(m.height) &&
            disparities == static_cast<int32_t>(m.disparities)) {

            supported = true;
            break;
        }
    }

    if (false == supported) {
        ROS_ERROR("Reconfigure: sensor does not support a resolution of: %dx%d (%d disparities)",
                  width, height, disparities);
        return false;
    }

    ROS_WARN("Reconfigure: changing sensor resolution to %dx%d (%d disparities), from %dx%d "
         "(%d disparities): reconfiguration may take up to 30 seconds",
             width, height, disparities,
             cfg.width(), cfg.height(), cfg.disparities());

    cfg.setResolution(width, height);
    cfg.setDisparities(disparities);

    return true;
}

template<class T> void Reconfigure::configureCropMode(crl::multisense::image::Config& cfg, const T& dyn)
{
    cfg.setCamMode(dyn.crop_mode == 1 ? 2000 : 4000);
    cfg.setOffset(dyn.crop_offset);
    ROS_WARN("Reconfigure: changing cam mode to %s with offset %d: reconfiguration may take up to 30 seconds",
             dyn.crop_mode == 1 ? "ON" : "OFF" , cfg.offset());
    crop_mode_changed_ = true;
}

template<class T> void Reconfigure::configureSgm(image::Config& cfg, const T& dyn)
{
    cfg.setStereoPostFilterStrength(dyn.stereo_post_filtering);
}

template<class T> void Reconfigure::configureCamera(image::Config& cfg, const T& dyn)
{
    DataSource    streamsEnabled = 0;
    int32_t       width, height, disparities;
    bool          resolutionChange=false;
    Status        status=Status_Ok;

    //
    // Decode the resolution string

    if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
        ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
        return;
    }

    //
    // If a resolution change is desired

    if ((resolutionChange = changeResolution(cfg, width, height, disparities) || crop_mode_changed_)) {
        crop_mode_changed_ = false;
        //
        // Halt streams during the resolution change
        status = driver_->getEnabledStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
                      Channel::statusString(status));
            return;
        }
        status = driver_->stopStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
                      Channel::statusString(status));
            return;
        }
    }

    //
    // Set all other image config from dynamic reconfigure

    cfg.setFps(dyn.fps);
    cfg.setGain(dyn.gain);
    cfg.setExposure(dyn.exposure_time * 1e6);
    cfg.setAutoExposure(dyn.auto_exposure);
    cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
    cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
    cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
    cfg.setWhiteBalance(dyn.white_balance_red,
                        dyn.white_balance_blue);
    cfg.setAutoWhiteBalance(dyn.auto_white_balance);
    cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
    cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);
    cfg.setHdr(dyn.hdr_enable);

    //
    // Apply, sensor enforces limits per setting.

    status = driver_->setImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to set image config: %s",
                  Channel::statusString(status));

    status = driver_->getImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to query image config: %s",
                  Channel::statusString(status));


    resolution_change_callback_(cfg);

    //
    // If we are changing the resolution, let others know about it

    if (resolutionChange) {
        status = driver_->startStreams(streamsEnabled);
        if (Status_Ok != status)
            ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
                      Channel::statusString(status));
    }

    //
    // Enable/disable network-based time synchronization.
    //
    // If enabled, sensor timestamps will be reported in the local
    // system clock's frame, using a continuously updated offset from
    // the sensor's internal clock.
    //
    // If disabled, sensor timestamps will be reported in the sensor
    // clock's frame, which is free-running from zero on power up.
    //
    // Enabled by default.

    driver_->networkTimeSynchronization(dyn.network_time_sync);

    //
    // Set our transmit delay
    image::TransmitDelay d;
    d.delay = dyn.desired_transmit_delay;
    status = driver_->setTransmitDelay(d);
    if (Status_Ok != status) {
        ROS_ERROR("Reconfigure: failed to set transmit delay: %s",
                  Channel::statusString(status));
    }
}

template<class T> void Reconfigure::configureMotor(const T& dyn)
{
    //
    // Send the desired motor speed

    if (motor_supported_) {

        const float radiansPerSecondToRpm = 9.54929659643;

        Status status = driver_->setMotorSpeed(radiansPerSecondToRpm * dyn.motor_speed);
        if (Status_Ok != status) {
            if (Status_Unsupported == status)
                motor_supported_ = false;
            else
                ROS_ERROR("Reconfigure: failed to set motor speed: %s",
                          Channel::statusString(status));
        }
    }

}

template<class T> void Reconfigure::configureLeds(const T& dyn)
{
    //
    // Send the desired lighting configuration

    if (lighting_supported_) {

        lighting::Config leds;

        if (false == dyn.lighting) {
            leds.setFlash(false);
            leds.setDutyCycle(0.0);
        } else {
            leds.setFlash(dyn.flash);
            leds.setDutyCycle(dyn.led_duty_cycle * 100.0);
        }

        Status status = driver_->setLightingConfig(leds);
        if (Status_Ok != status) {
            if (Status_Unsupported == status)
                lighting_supported_ = false;
            else
                ROS_ERROR("Reconfigure: failed to set lighting config: %s",
                          Channel::statusString(status));
        }
    }

}

template<class T> void Reconfigure::configureImu(const T& dyn)
{
    if (imu_configs_.empty()) {
        Status status = driver_->getImuConfig(imu_samples_per_message_,
                                              imu_configs_);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to query IMU config: %s",
                      Channel::statusString(status));
            return;
        }
    }

    std::vector<imu::Config> changedConfigs;
    std::vector<imu::Config>::iterator it = imu_configs_.begin();
    for(; it!=imu_configs_.end(); ++it) {

        imu::Config& c = *it;

        if ("accelerometer" == c.name &&
            (c.enabled      != dyn.accelerometer_enabled ||
             static_cast<int>(c.rateTableIndex)  != dyn.accelerometer_rate    ||
             static_cast<int>(c.rangeTableIndex) != dyn.accelerometer_range)) {

            c.enabled         = dyn.accelerometer_enabled;
            c.rateTableIndex  = dyn.accelerometer_rate;
            c.rangeTableIndex = dyn.accelerometer_range;
            changedConfigs.push_back(c);
        }

        if ("gyroscope" == c.name &&
            (c.enabled  != dyn.gyroscope_enabled ||
             static_cast<int>(c.rateTableIndex)  != dyn.gyroscope_rate    ||
             static_cast<int>(c.rangeTableIndex) != dyn.gyroscope_range)) {

            c.enabled         = dyn.gyroscope_enabled;
            c.rateTableIndex  = dyn.gyroscope_rate;
            c.rangeTableIndex = dyn.gyroscope_range;
            changedConfigs.push_back(c);
        }

        if ("magnetometer" == c.name &&
            (c.enabled     != dyn.magnetometer_enabled ||
             static_cast<int>(c.rateTableIndex)  != dyn.magnetometer_rate    ||
             static_cast<int>(c.rangeTableIndex) != dyn.magnetometer_range)) {

            c.enabled         = dyn.magnetometer_enabled;
            c.rateTableIndex  = dyn.magnetometer_rate;
            c.rangeTableIndex = dyn.magnetometer_range;
            changedConfigs.push_back(c);
        }
    }

    if (changedConfigs.size() > 0 ||
        static_cast<int>(imu_samples_per_message_) != dyn.imu_samples_per_message) {

        ROS_WARN("Reconfigure: IMU configuration changes will take effect after all IMU "
         "topic subscriptions have been closed.");

        imu_samples_per_message_ = dyn.imu_samples_per_message;

        Status status = driver_->setImuConfig(false, // store in non-volatile flash
                                              imu_samples_per_message_,
                                              changedConfigs);  // can be empty
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to set IMU configuration: %s",
                      Channel::statusString(status));
            imu_configs_.clear();
        }
    }
}

template<class T> void Reconfigure::configureBorderClip(const T& dyn)
{

    if (static_cast<BorderClip>(dyn.border_clip_type) != border_clip_type_ ||
        dyn.border_clip_value != border_clip_value_) {
        border_clip_type_ = static_cast<BorderClip>(dyn.border_clip_type);
        border_clip_value_ = dyn.border_clip_value;
        border_clip_change_callback_(border_clip_type_, border_clip_value_);
    }
}

template<class T> void Reconfigure::configurePointCloudRange(const T& dyn)
{
    max_point_cloud_range_callback_(dyn.max_point_cloud_range);
}

template<class T> void Reconfigure::configurePtp(const T& dyn)
{
    if (ptp_supported_) {
        Status status = driver_->ptpTimeSynchronization(dyn.ptp_time_sync);
        if (Status_Ok != status) {
            if (Status_Unsupported == status || Status_Unknown == status) {
                ptp_supported_ = false;
            } else {
                ROS_ERROR("Reconfigure: enable PTP time synchronization: %s",
                          Channel::statusString(status));
            }
        }
    }

    if (dyn.trigger_source != 3 ||  (ptp_supported_ && dyn.trigger_source == 3)) {
        Status status = driver_->setTriggerSource(dyn.trigger_source);
        if (Status_Ok != status) {
            if (Status_Unsupported == status || Status_Unknown == status) {
                ptp_supported_ = false;
            } else {
                ROS_ERROR("Reconfigure: failed to set trigger source: %s",
                          Channel::statusString(status));
            }
        }
    }
}

template<class T> void Reconfigure::configureStereoProfile(crl::multisense::image::Config &cfg, const T& dyn)
{
    cfg.setCameraProfile(dyn.stereo_profile);
}

#define GET_CONFIG()                                                    \
    image::Config cfg;                                                  \
    Status status = driver_->getImageConfig(cfg);                       \
    if (Status_Ok != status) {                                          \
        ROS_ERROR("Reconfigure: failed to query image config: %s",      \
                  Channel::statusString(status));                       \
        return;                                                         \
    }                                                                   \

#define SL_BM()  do {                                           \
        GET_CONFIG();                                           \
        configureCamera(cfg, dyn);                              \
        configureMotor(dyn);                                    \
        configureLeds(dyn);                                     \
        configureBorderClip(dyn);                               \
        configurePointCloudRange(dyn);                          \
    } while(0)

#define SL_BM_IMU()  do {                                       \
        GET_CONFIG();                                           \
        configureCamera(cfg, dyn);                              \
        configureMotor(dyn);                                    \
        configureLeds(dyn);                                     \
        configureImu(dyn);                                      \
        configureBorderClip(dyn);                               \
        configurePointCloudRange(dyn);                          \
    } while(0)

#define SL_SGM_IMU()  do {                                      \
        GET_CONFIG();                                           \
        configureSgm(cfg, dyn);                                 \
        configureCamera(cfg, dyn);                              \
        configureMotor(dyn);                                    \
        configureLeds(dyn);                                     \
        configureImu(dyn);                                      \
        configureBorderClip(dyn);                               \
        configurePointCloudRange(dyn);                          \
    } while(0)

#define SL_SGM()  do {                                          \
        GET_CONFIG();                                           \
        configureSgm(cfg, dyn);                                 \
        configureCamera(cfg, dyn);                              \
        configureBorderClip(dyn);                               \
    } while(0)

#define SL_SGM_IMU_CMV4000()  do {                              \
        GET_CONFIG();                                           \
        configureSgm(cfg, dyn);                                 \
        configureCropMode(cfg, dyn);                            \
        configureCamera(cfg, dyn);                              \
        configureMotor(dyn);                                    \
        configureLeds(dyn);                                     \
        configureImu(dyn);                                      \
        configureBorderClip(dyn);                               \
        configurePointCloudRange(dyn);                          \
    } while(0)

#define S27_SGM()  do {                                         \
        GET_CONFIG();                                           \
        configureSgm(cfg, dyn);                                 \
        configureStereoProfile(cfg, dyn);                       \
        configureCamera(cfg, dyn);                              \
        configureBorderClip(dyn);                               \
        configurePtp(dyn);                                      \
    } while(0)



//
// The dynamic reconfigure callbacks (MultiSense S* variations)

void Reconfigure::callback_sl_bm_cmv2000      (multisense_ros::sl_bm_cmv2000Config&      dyn, uint32_t level) { (void) level; SL_BM();       }
void Reconfigure::callback_sl_bm_cmv2000_imu  (multisense_ros::sl_bm_cmv2000_imuConfig&  dyn, uint32_t level) { (void) level; SL_BM_IMU();   }
void Reconfigure::callback_sl_bm_cmv4000      (multisense_ros::sl_bm_cmv4000Config&      dyn, uint32_t level) { (void) level; SL_BM();       }
void Reconfigure::callback_sl_bm_cmv4000_imu  (multisense_ros::sl_bm_cmv4000_imuConfig&  dyn, uint32_t level) { (void) level; SL_BM_IMU();   }
void Reconfigure::callback_sl_sgm_cmv2000_imu (multisense_ros::sl_sgm_cmv2000_imuConfig& dyn, uint32_t level) { (void) level; SL_SGM_IMU();  }
void Reconfigure::callback_sl_sgm_cmv4000_imu (multisense_ros::sl_sgm_cmv4000_imuConfig& dyn, uint32_t level) { (void) level; SL_SGM_IMU_CMV4000();  }
void Reconfigure::callback_mono_cmv2000       (multisense_ros::mono_cmv2000Config&       dyn, uint32_t level) { (void) level; SL_BM_IMU();   }
void Reconfigure::callback_mono_cmv4000       (multisense_ros::mono_cmv4000Config&       dyn, uint32_t level) { (void) level; SL_BM_IMU();   }
void Reconfigure::callback_s27_AR0234         (multisense_ros::s27_sgm_AR0234Config&     dyn, uint32_t level) { (void) level; S27_SGM();   }

//
// BCAM (Sony IMX104)

void Reconfigure::callback_bcam_imx104(multisense_ros::bcam_imx104Config& dyn,
                                       uint32_t                           level)
{
    (void) level;

    GET_CONFIG();
    DataSource  streamsEnabled = 0;
    int32_t     width, height;
    bool        resolutionChange=false;

    //
    // Decode the resolution string

    if (2 != sscanf(dyn.resolution.c_str(), "%dx%dx", &width, &height)) {
        ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
        return;
    }

    //
    // If a resolution change is desired

    if ((resolutionChange = changeResolution(cfg, width, height, 0))) {

        //
        // Halt streams during the resolution change

        status = driver_->getEnabledStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
                      Channel::statusString(status));
            return;
        }
        status = driver_->stopStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
                      Channel::statusString(status));
            return;
        }
    }

    //
    // Set all other image config from dynamic reconfigure

    cfg.setFps(static_cast<float>(dyn.fps));
    cfg.setGain(dyn.gain);
    cfg.setExposure(dyn.exposure_time * 1e6);
    cfg.setAutoExposure(dyn.auto_exposure);
    cfg.setAutoExposureMax(dyn.auto_exposure_max_time * 1e6);
    cfg.setAutoExposureDecay(dyn.auto_exposure_decay);
    cfg.setAutoExposureThresh(dyn.auto_exposure_thresh);
    cfg.setWhiteBalance(dyn.white_balance_red,
                        dyn.white_balance_blue);
    cfg.setAutoWhiteBalance(dyn.auto_white_balance);
    cfg.setAutoWhiteBalanceDecay(dyn.auto_white_balance_decay);
    cfg.setAutoWhiteBalanceThresh(dyn.auto_white_balance_thresh);

    //
    // Apply, sensor enforces limits per setting.

    status = driver_->setImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to set image config: %s",
                  Channel::statusString(status));

    status = driver_->getImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to query image config: %s",
                  Channel::statusString(status));

    resolution_change_callback_(cfg);

    //
    // If we are changing the resolution, let others know about it

    if (resolutionChange) {

        status = driver_->startStreams(streamsEnabled);
        if (Status_Ok != status)
            ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
                      Channel::statusString(status));
    }
}

//
// ST21 FLIR Thermal Imagers
// Seperate callback required due to limited subset of dyn parameters
// in st21_sgm_vga_imuConfig. configureCamera results in SFINAE errors

void Reconfigure::callback_st21_vga(multisense_ros::st21_sgm_vga_imuConfig& dyn,
                                       uint32_t                           level)
{
    (void) level;
    DataSource    streamsEnabled = 0;
    int32_t       width, height, disparities;
    bool          resolutionChange=false;

    GET_CONFIG();

    //
    // Decode the resolution string

    if (3 != sscanf(dyn.resolution.c_str(), "%dx%dx%d", &width, &height, &disparities)) {
        ROS_ERROR("Reconfigure: malformed resolution string: \"%s\"", dyn.resolution.c_str());
        return;
    }

    //
    // If a resolution change is desired

    if ((resolutionChange = changeResolution(cfg, width, height, disparities))) {

        //
        // Halt streams during the resolution change

        status = driver_->getEnabledStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to get enabled streams: %s",
                      Channel::statusString(status));
            return;
        }
        status = driver_->stopStreams(streamsEnabled);
        if (Status_Ok != status) {
            ROS_ERROR("Reconfigure: failed to stop streams for a resolution change: %s",
                      Channel::statusString(status));
            return;
        }
    }

    cfg.setFps(dyn.fps);

    configureSgm(cfg, dyn);
    configureImu(dyn);

    //
    // Apply, sensor enforces limits per setting.

    status = driver_->setImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to set image config: %s",
                  Channel::statusString(status));

    status = driver_->getImageConfig(cfg);
    if (Status_Ok != status)
        ROS_ERROR("Reconfigure: failed to query image config: %s",
                  Channel::statusString(status));


    resolution_change_callback_(cfg);

    //
    // If we are changing the resolution, let others know about it

    if (resolutionChange) {

        status = driver_->startStreams(streamsEnabled);
        if (Status_Ok != status)
            ROS_ERROR("Reconfigure: failed to restart streams after a resolution change: %s",
                      Channel::statusString(status));
    }

    configureBorderClip(dyn);
    configurePointCloudRange(dyn);
}

} // namespace